Skip to content

Commit 6f6fe85

Browse files
tonynajjarmamueluth
tonynajjar
authored and
mamueluth
committed
Tricycle controller (ros-controls#345)
1 parent ec6f58f commit 6f6fe85

18 files changed

+2203
-0
lines changed

‎ros2_controllers/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
<exec_depend>joint_state_broadcaster</exec_depend>
2020
<exec_depend>joint_trajectory_controller</exec_depend>
2121
<exec_depend>position_controllers</exec_depend>
22+
<exec_depend>tricycle_controller</exec_depend>
2223
<exec_depend>velocity_controllers</exec_depend>
2324

2425
<export>

‎tricycle_controller/CMakeLists.txt‎

Lines changed: 118 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,118 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(tricycle_controller)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
# find dependencies
9+
find_package(ament_cmake REQUIRED)
10+
find_package(controller_interface REQUIRED)
11+
find_package(hardware_interface REQUIRED)
12+
find_package(pluginlib REQUIRED)
13+
find_package(nav_msgs REQUIRED)
14+
find_package(std_srvs REQUIRED)
15+
find_package(geometry_msgs REQUIRED)
16+
find_package(rclcpp REQUIRED)
17+
find_package(rcpputils REQUIRED)
18+
find_package(rclcpp_lifecycle REQUIRED)
19+
find_package(realtime_tools REQUIRED)
20+
find_package(tf2 REQUIRED)
21+
find_package(tf2_msgs REQUIRED)
22+
find_package(ackermann_msgs REQUIRED)
23+
24+
add_library(
25+
${PROJECT_NAME}
26+
SHARED
27+
src/tricycle_controller.cpp
28+
src/odometry.cpp
29+
src/traction_limiter.cpp
30+
src/steering_limiter.cpp
31+
)
32+
target_include_directories(
33+
${PROJECT_NAME}
34+
PRIVATE
35+
include
36+
)
37+
38+
ament_target_dependencies(
39+
${PROJECT_NAME}
40+
ackermann_msgs
41+
builtin_interfaces
42+
controller_interface
43+
geometry_msgs
44+
hardware_interface
45+
nav_msgs
46+
std_srvs
47+
pluginlib
48+
rclcpp
49+
rclcpp_lifecycle
50+
rcpputils
51+
realtime_tools
52+
tf2
53+
tf2_msgs
54+
)
55+
56+
pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml)
57+
58+
install(TARGETS ${PROJECT_NAME}
59+
RUNTIME DESTINATION bin
60+
ARCHIVE DESTINATION lib
61+
LIBRARY DESTINATION lib
62+
)
63+
install(
64+
DIRECTORY
65+
include/
66+
DESTINATION include
67+
)
68+
69+
if(BUILD_TESTING)
70+
find_package(ament_cmake_gmock REQUIRED)
71+
find_package(controller_manager REQUIRED)
72+
find_package(ros2_control_test_assets REQUIRED)
73+
74+
ament_add_gmock(test_tricycle_controller
75+
test/test_tricycle_controller.cpp
76+
ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml)
77+
target_include_directories(test_tricycle_controller PRIVATE include)
78+
target_link_libraries(test_tricycle_controller
79+
${PROJECT_NAME}
80+
)
81+
82+
ament_target_dependencies(test_tricycle_controller
83+
geometry_msgs
84+
hardware_interface
85+
nav_msgs
86+
rclcpp
87+
rclcpp_lifecycle
88+
realtime_tools
89+
tf2
90+
tf2_msgs
91+
)
92+
93+
ament_add_gmock(
94+
test_load_tricycle_controller
95+
test/test_load_tricycle_controller.cpp
96+
)
97+
target_include_directories(test_load_tricycle_controller PRIVATE include)
98+
ament_target_dependencies(test_load_tricycle_controller
99+
controller_manager
100+
ros2_control_test_assets
101+
)
102+
endif()
103+
104+
ament_export_include_directories(
105+
include
106+
)
107+
ament_export_libraries(
108+
${PROJECT_NAME}
109+
)
110+
ament_export_dependencies(
111+
controller_interface
112+
geometry_msgs
113+
hardware_interface
114+
rclcpp
115+
rclcpp_lifecycle
116+
)
117+
118+
ament_package()

‎tricycle_controller/config/tricycle_drive_controller.yaml‎

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
tricycle_controller:
2+
ros__parameters:
3+
# Model
4+
traction_joint_name: traction_joint # Name of traction joint in URDF
5+
steering_joint_name: steering_joint # Name of steering joint in URDF
6+
wheel_radius: 0.0 # Radius of front wheel
7+
wheelbase: 0.0 # Distance between center of back wheels and front wheel
8+
9+
# Odometry
10+
odom_frame_id: odom
11+
base_frame_id: base_link
12+
publish_rate: 50.0 # publish rate of odom and tf
13+
open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry
14+
enable_odom_tf: true # If True, publishes odom<-base_link TF
15+
odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf
16+
pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source
17+
twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source
18+
velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom
19+
20+
# Rate Limiting
21+
traction: # All values should be positive
22+
# min_velocity: 0.0
23+
# max_velocity: 1000.0
24+
# min_acceleration: 0.0
25+
max_acceleration: 5.0
26+
# min_deceleration: 0.0
27+
max_deceleration: 8.0
28+
# min_jerk: 0.0
29+
# max_jerk: 1000.0
30+
steering:
31+
min_position: -1.57
32+
max_position: 1.57
33+
# min_velocity: 0.0
34+
max_velocity: 1.0
35+
# min_acceleration: 0.0
36+
# max_acceleration: 1000.0
37+
38+
# cmd_vel input
39+
cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received
40+
use_stamped_vel: false # Set to True if using TwistStamped.
41+
42+
# Debug
43+
publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s.

‎tricycle_controller/doc/userdoc.rst‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
.. _tricycle_controller_userdoc:
2+
3+
tricycle_controller
4+
=====================
5+
6+
Controller for mobile robots with tricycle drive.
7+
Input for control are robot base_link twist commands which are translated to traction and steering
8+
commands for the tricycle drive base. Odometry is computed from hardware feedback and published.
9+
10+
Velocity commands
11+
-----------------
12+
13+
The controller works with a velocity twist from which it extracts
14+
the x component of the linear velocity and the z component of the angular velocity.
15+
Velocities on other components are ignored.
16+
17+
18+
Other features
19+
--------------
20+
21+
Realtime-safe implementation.
22+
Odometry publishing
23+
Velocity, acceleration and jerk limits
24+
Automatic stop after command timeout

‎tricycle_controller/include/tricycle_controller/odometry.hpp‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
// Copyright 2022 Pixel Robotics.
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+
/*
16+
* Author: Tony Najjar
17+
*/
18+
19+
#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_
20+
#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_
21+
22+
#include <cmath>
23+
24+
#include "rclcpp/time.hpp"
25+
#include "rcppmath/rolling_mean_accumulator.hpp"
26+
27+
namespace tricycle_controller
28+
{
29+
class Odometry
30+
{
31+
public:
32+
explicit Odometry(size_t velocity_rolling_window_size = 10);
33+
34+
bool update(double left_vel, double right_vel, const rclcpp::Duration & dt);
35+
void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt);
36+
void resetOdometry();
37+
38+
double getX() const { return x_; }
39+
double getY() const { return y_; }
40+
double getHeading() const { return heading_; }
41+
double getLinear() const { return linear_; }
42+
double getAngular() const { return angular_; }
43+
44+
void setWheelParams(double wheel_separation, double wheel_radius);
45+
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
46+
47+
private:
48+
using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
49+
50+
void integrateRungeKutta2(double linear, double angular);
51+
void integrateExact(double linear, double angular);
52+
void resetAccumulators();
53+
54+
// Current pose:
55+
double x_; // [m]
56+
double y_; // [m]
57+
double heading_; // [rad]
58+
59+
// Current velocity:
60+
double linear_; // [m/s]
61+
double angular_; // [rad/s]
62+
63+
// Wheel kinematic parameters [m]:
64+
double wheelbase_;
65+
double wheel_radius_;
66+
67+
// Rolling mean accumulators for the linear and angular velocities:
68+
size_t velocity_rolling_window_size_;
69+
RollingMeanAccumulator linear_accumulator_;
70+
RollingMeanAccumulator angular_accumulator_;
71+
};
72+
73+
} // namespace tricycle_controller
74+
75+
#endif // TRICYCLE_CONTROLLER__ODOMETRY_HPP_

‎tricycle_controller/include/tricycle_controller/steering_limiter.hpp‎

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
// Copyright 2022 Pixel Robotics.
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+
/*
16+
* Author: Tony Najjar
17+
*/
18+
19+
#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_
20+
#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_
21+
22+
#include <cmath>
23+
24+
namespace tricycle_controller
25+
{
26+
27+
class SteeringLimiter
28+
{
29+
public:
30+
/**
31+
* \brief Constructor
32+
* \param [in] min_position Minimum position [m] or [rad]
33+
* \param [in] max_position Maximum position [m] or [rad]
34+
* \param [in] min_velocity Minimum velocity [m/s] or [rad/s]
35+
* \param [in] max_velocity Maximum velocity [m/s] or [rad/s]
36+
* \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2]
37+
* \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2]
38+
*/
39+
SteeringLimiter(
40+
double min_position = NAN, double max_position = NAN,
41+
double min_velocity = NAN, double max_velocity = NAN,
42+
double min_acceleration = NAN, double max_acceleration = NAN);
43+
44+
/**
45+
* \brief Limit the position, velocity and acceleration
46+
* \param [in, out] p position [m] or [rad]
47+
* \param [in] p0 Previous position to p [m] or [rad]
48+
* \param [in] p1 Previous position to p0 [m] or [rad]
49+
* \param [in] dt Time step [s]
50+
* \return Limiting factor (1.0 if none)
51+
*/
52+
double limit(double & p, double p0, double p1, double dt);
53+
54+
/**
55+
* \brief Limit the jerk
56+
* \param [in, out] p position [m] or [rad]
57+
* \param [in] p0 Previous position to p [m] or [rad]
58+
* \param [in] p1 Previous position to p0 [m] or [rad]
59+
* \param [in] dt Time step [s]
60+
* \return Limiting factor (1.0 if none)
61+
*/
62+
double limit_position(double & p);
63+
64+
/**
65+
* \brief Limit the velocity
66+
* \param [in, out] p position [m]
67+
* \return Limiting factor (1.0 if none)
68+
*/
69+
double limit_velocity(double & p, double p0, double dt);
70+
71+
/**
72+
* \brief Limit the acceleration
73+
* \param [in, out] p Position [m] or [rad]
74+
* \param [in] p0 Previous position [m] or [rad]
75+
* \param [in] dt Time step [s]
76+
* \return Limiting factor (1.0 if none)
77+
*/
78+
double limit_acceleration(double & p, double p0, double p1, double dt);
79+
80+
private:
81+
82+
// Position limits:
83+
double min_position_;
84+
double max_position_;
85+
86+
// Velocity limits:
87+
double min_velocity_;
88+
double max_velocity_;
89+
90+
// Acceleration limits:
91+
double min_acceleration_;
92+
double max_acceleration_;
93+
94+
};
95+
96+
} // namespace tricycle_controller
97+
98+
#endif // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_

0 commit comments

Comments
 (0)