Skip to content

Commit 45d0083

Browse files
petkovichpetkovich
authored
Steering odometry library and controllers (ros-controls#484)
1 parent 70e282f commit 45d0083

File tree

57 files changed

+5773
-1
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

57 files changed

+5773
-1
lines changed

‎.pre-commit-config.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -126,7 +126,7 @@ repos:
126126
exclude: CHANGELOG\.rst$
127127

128128
- repo: https://github.com/pre-commit/pygrep-hooks
129-
rev: v1.9.0
129+
rev: v1.10.0
130130
hooks:
131131
- id: rst-backticks
132132
exclude: CHANGELOG\.rst$

‎ackermann_steering_controller/CMakeLists.txt‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
project(ackermann_steering_controller LANGUAGES CXX)
3+
4+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
# find dependencies
9+
set(THIS_PACKAGE_INCLUDE_DEPENDS
10+
controller_interface
11+
hardware_interface
12+
generate_parameter_library
13+
pluginlib
14+
rclcpp
15+
rclcpp_lifecycle
16+
realtime_tools
17+
std_srvs
18+
steering_controllers_library
19+
)
20+
21+
find_package(ament_cmake REQUIRED)
22+
find_package(backward_ros REQUIRED)
23+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
24+
find_package(${Dependency} REQUIRED)
25+
endforeach()
26+
27+
generate_parameter_library(ackermann_steering_controller_parameters
28+
src/ackermann_steering_controller.yaml
29+
)
30+
31+
add_library(
32+
ackermann_steering_controller
33+
SHARED
34+
src/ackermann_steering_controller.cpp
35+
)
36+
target_compile_features(ackermann_steering_controller PUBLIC cxx_std_17)
37+
target_include_directories(ackermann_steering_controller PUBLIC
38+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
39+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
40+
target_link_libraries(ackermann_steering_controller PUBLIC
41+
ackermann_steering_controller_parameters)
42+
ament_target_dependencies(ackermann_steering_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
43+
44+
# Causes the visibility macros to use dllexport rather than dllimport,
45+
# which is appropriate when building the dll but not consuming it.
46+
target_compile_definitions(ackermann_steering_controller PRIVATE "ACKERMANN_STEERING_CONTROLLER_BUILDING_DLL")
47+
48+
pluginlib_export_plugin_description_file(
49+
controller_interface ackermann_steering_controller.xml)
50+
51+
if(BUILD_TESTING)
52+
find_package(ament_cmake_gmock REQUIRED)
53+
find_package(controller_manager REQUIRED)
54+
find_package(hardware_interface REQUIRED)
55+
find_package(ros2_control_test_assets REQUIRED)
56+
57+
58+
add_rostest_with_parameters_gmock(test_load_ackermann_steering_controller
59+
test/test_load_ackermann_steering_controller.cpp
60+
${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml
61+
)
62+
ament_target_dependencies(test_load_ackermann_steering_controller
63+
controller_manager
64+
hardware_interface
65+
ros2_control_test_assets
66+
)
67+
68+
add_rostest_with_parameters_gmock(test_ackermann_steering_controller
69+
test/test_ackermann_steering_controller.cpp
70+
${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_params.yaml
71+
)
72+
target_include_directories(test_ackermann_steering_controller PRIVATE include)
73+
target_link_libraries(test_ackermann_steering_controller ackermann_steering_controller)
74+
ament_target_dependencies(
75+
test_ackermann_steering_controller
76+
controller_interface
77+
hardware_interface
78+
)
79+
80+
add_rostest_with_parameters_gmock(
81+
test_ackermann_steering_controller_preceeding test/test_ackermann_steering_controller_preceeding.cpp
82+
${CMAKE_CURRENT_SOURCE_DIR}/test/ackermann_steering_controller_preceeding_params.yaml)
83+
target_include_directories(test_ackermann_steering_controller_preceeding PRIVATE include)
84+
target_link_libraries(test_ackermann_steering_controller_preceeding ackermann_steering_controller)
85+
ament_target_dependencies(
86+
test_ackermann_steering_controller_preceeding
87+
controller_interface
88+
hardware_interface
89+
)
90+
endif()
91+
92+
install(
93+
DIRECTORY include/
94+
DESTINATION include/ackermann_steering_controller
95+
)
96+
97+
install(
98+
TARGETS ackermann_steering_controller ackermann_steering_controller_parameters
99+
EXPORT export_ackermann_steering_controller
100+
RUNTIME DESTINATION bin
101+
ARCHIVE DESTINATION lib
102+
LIBRARY DESTINATION lib
103+
)
104+
105+
ament_export_targets(export_ackermann_steering_controller HAS_LIBRARY_TARGET)
106+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
107+
ament_package()

‎ackermann_steering_controller/ackermann_steering_controller.xml‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
<library path="ackermann_steering_controller">
2+
<class name="ackermann_steering_controller/AckermannSteeringController"
3+
type="ackermann_steering_controller::AckermannSteeringController" base_class_type="controller_interface::ChainableControllerInterface">
4+
<description>
5+
Steering controller for Ackermann (car-like) kinematics with two traction and two steering joints.
6+
</description>
7+
</class>
8+
</library>

‎ackermann_steering_controller/doc/userdoc.rst‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
.. _ackermann_steering_controller_userdoc:
2+
3+
ackermann_steering_controller
4+
=============================
5+
6+
This controller implements the kinematics with two axes and four wheels, where the wheels on one axis are fixed (traction/drive) wheels, and the wheels on the other axis are steerable.
7+
8+
The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel.
9+
10+
For more details on controller's execution and interfaces check the :ref:`Steering Controller Library <steering_controllers_library>`.
11+
12+
13+
Parameters
14+
,,,,,,,,,,,
15+
16+
For list of parameters and their meaning YAML file in the ``src`` folder of the controller's package.
17+
18+
For an exemplary parameterization see the ``test`` folder of the controller's package.

‎ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.hpp‎

Lines changed: 65 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,65 @@
1+
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
//
15+
// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl
16+
//
17+
18+
#ifndef ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_
19+
#define ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_
20+
21+
#include <memory>
22+
23+
#include "ackermann_steering_controller/visibility_control.h"
24+
#include "ackermann_steering_controller_parameters.hpp"
25+
#include "steering_controllers_library/steering_controllers_library.hpp"
26+
27+
namespace ackermann_steering_controller
28+
{
29+
// name constants for state interfaces
30+
static constexpr size_t STATE_TRACTION_RIGHT_WHEEL = 0;
31+
static constexpr size_t STATE_TRACTION_LEFT_WHEEL = 1;
32+
static constexpr size_t STATE_STEER_RIGHT_WHEEL = 2;
33+
static constexpr size_t STATE_STEER_LEFT_WHEEL = 3;
34+
35+
// name constants for command interfaces
36+
static constexpr size_t CMD_TRACTION_RIGHT_WHEEL = 0;
37+
static constexpr size_t CMD_TRACTION_LEFT_WHEEL = 1;
38+
static constexpr size_t CMD_STEER_RIGHT_WHEEL = 2;
39+
static constexpr size_t CMD_STEER_LEFT_WHEEL = 3;
40+
41+
static constexpr size_t NR_STATE_ITFS = 4;
42+
static constexpr size_t NR_CMD_ITFS = 4;
43+
static constexpr size_t NR_REF_ITFS = 2;
44+
45+
class AckermannSteeringController : public steering_controllers_library::SteeringControllersLibrary
46+
{
47+
public:
48+
AckermannSteeringController();
49+
50+
ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn
51+
configure_odometry() override;
52+
53+
ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry(
54+
const rclcpp::Duration & period) override;
55+
56+
ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC void
57+
initialize_implementation_parameter_listener() override;
58+
59+
protected:
60+
std::shared_ptr<ackermann_steering_controller::ParamListener> ackermann_param_listener_;
61+
ackermann_steering_controller::Params ackermann_params_;
62+
};
63+
} // namespace ackermann_steering_controller
64+
65+
#endif // ACKERMANN_STEERING_CONTROLLER__ACKERMANN_STEERING_CONTROLLER_HPP_

‎ackermann_steering_controller/include/ackermann_steering_controller/visibility_control.h‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_
16+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_
17+
18+
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
19+
// https://gcc.gnu.org/wiki/Visibility
20+
21+
#if defined _WIN32 || defined __CYGWIN__
22+
#ifdef __GNUC__
23+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport))
24+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport))
25+
#else
26+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport)
27+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport)
28+
#endif
29+
#ifdef ACKERMANN_STEERING_CONTROLLER__VISIBILITY_BUILDING_DLL
30+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \
31+
ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT
32+
#else
33+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC \
34+
ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT
35+
#endif
36+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE \
37+
ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC
38+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL
39+
#else
40+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default")))
41+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_IMPORT
42+
#if __GNUC__ >= 4
43+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default")))
44+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden")))
45+
#else
46+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC
47+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_LOCAL
48+
#endif
49+
#define ACKERMANN_STEERING_CONTROLLER__VISIBILITY_PUBLIC_TYPE
50+
#endif
51+
52+
#endif // ACKERMANN_STEERING_CONTROLLER__VISIBILITY_CONTROL_H_

‎ackermann_steering_controller/package.xml‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>ackermann_steering_controller</name>
5+
<version>0.0.0</version>
6+
<description>Steering controller for Ackermann kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it.</description>
7+
<license>Apache License 2.0</license>
8+
<maintainer email="[email protected]">Bence Magyar</maintainer>
9+
<maintainer email="[email protected]">Dr.-Ing. Denis Štogl</maintainer>
10+
<maintainer email="[email protected]">dr. sc. Tomislav Petkovic</maintainer>
11+
12+
<author email="[email protected]">Dr.-Ing. Denis Štogl</author>
13+
<author email="[email protected]">dr. sc. Tomislav Petkovic</author>
14+
15+
<buildtool_depend>ament_cmake</buildtool_depend>
16+
17+
<build_depend>generate_parameter_library</build_depend>
18+
19+
<depend>control_msgs</depend>
20+
<depend>controller_interface</depend>
21+
<depend>hardware_interface</depend>
22+
<depend>pluginlib</depend>
23+
<depend>rclcpp</depend>
24+
<depend>rclcpp_lifecycle</depend>
25+
<depend>std_srvs</depend>
26+
<depend>steering_controllers_library</depend>
27+
28+
<test_depend>ament_cmake_gmock</test_depend>
29+
<test_depend>controller_manager</test_depend>
30+
<test_depend>hardware_interface</test_depend>
31+
<test_depend>ros2_control_test_assets</test_depend>
32+
33+
<export>
34+
<build_type>ament_cmake</build_type>
35+
</export>
36+
</package>

‎ackermann_steering_controller/src/ackermann_steering_controller.cpp‎

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "ackermann_steering_controller/ackermann_steering_controller.hpp"
16+
17+
namespace ackermann_steering_controller
18+
{
19+
AckermannSteeringController::AckermannSteeringController()
20+
: steering_controllers_library::SteeringControllersLibrary()
21+
{
22+
}
23+
24+
void AckermannSteeringController::initialize_implementation_parameter_listener()
25+
{
26+
ackermann_param_listener_ =
27+
std::make_shared<ackermann_steering_controller::ParamListener>(get_node());
28+
}
29+
30+
controller_interface::CallbackReturn AckermannSteeringController::configure_odometry()
31+
{
32+
ackermann_params_ = ackermann_param_listener_->get_params();
33+
34+
const double front_wheels_radius = ackermann_params_.front_wheels_radius;
35+
const double rear_wheels_radius = ackermann_params_.rear_wheels_radius;
36+
const double front_wheel_track = ackermann_params_.front_wheel_track;
37+
const double rear_wheel_track = ackermann_params_.rear_wheel_track;
38+
const double wheelbase = ackermann_params_.wheelbase;
39+
40+
if (params_.front_steering)
41+
{
42+
odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track);
43+
}
44+
else
45+
{
46+
odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track);
47+
}
48+
49+
odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
50+
51+
set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS);
52+
53+
RCLCPP_INFO(get_node()->get_logger(), "ackermann odom configure successful");
54+
return controller_interface::CallbackReturn::SUCCESS;
55+
}
56+
57+
bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period)
58+
{
59+
if (params_.open_loop)
60+
{
61+
odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
62+
}
63+
else
64+
{
65+
const double rear_right_wheel_value = state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
66+
const double rear_left_wheel_value = state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
67+
const double front_right_steer_position =
68+
state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
69+
const double front_left_steer_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
70+
if (
71+
!std::isnan(rear_right_wheel_value) && !std::isnan(rear_left_wheel_value) &&
72+
!std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position))
73+
{
74+
if (params_.position_feedback)
75+
{
76+
// Estimate linear and angular velocity using joint information
77+
odometry_.update_from_position(
78+
rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
79+
front_left_steer_position, period.seconds());
80+
}
81+
else
82+
{
83+
// Estimate linear and angular velocity using joint information
84+
odometry_.update_from_velocity(
85+
rear_right_wheel_value, rear_left_wheel_value, front_right_steer_position,
86+
front_left_steer_position, period.seconds());
87+
}
88+
}
89+
}
90+
return true;
91+
}
92+
} // namespace ackermann_steering_controller
93+
94+
#include "pluginlib/class_list_macros.hpp"
95+
96+
PLUGINLIB_EXPORT_CLASS(
97+
ackermann_steering_controller::AckermannSteeringController,
98+
controller_interface::ChainableControllerInterface)

0 commit comments

Comments
 (0)