Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Tricycle controller #345

Merged
merged 18 commits into from
Aug 3, 2022
1 change: 1 addition & 0 deletions ros2_controllers/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<exec_depend>joint_state_broadcaster</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend>
<exec_depend>position_controllers</exec_depend>
<exec_depend>tricycle_controller</exec_depend>
<exec_depend>velocity_controllers</exec_depend>

<export>
Expand Down
118 changes: 118 additions & 0 deletions tricycle_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,118 @@
cmake_minimum_required(VERSION 3.8)
project(tricycle_controller)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(controller_interface REQUIRED)
find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(nav_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rcpputils REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(realtime_tools REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_msgs REQUIRED)
find_package(ackermann_msgs REQUIRED)

add_library(
${PROJECT_NAME}
SHARED
src/tricycle_controller.cpp
src/odometry.cpp
src/traction_limiter.cpp
src/steering_limiter.cpp
)
target_include_directories(
${PROJECT_NAME}
PRIVATE
include
)

tonynajjar marked this conversation as resolved.
Show resolved Hide resolved
ament_target_dependencies(
${PROJECT_NAME}
ackermann_msgs
builtin_interfaces
controller_interface
geometry_msgs
hardware_interface
nav_msgs
std_srvs
pluginlib
rclcpp
rclcpp_lifecycle
rcpputils
realtime_tools
tf2
tf2_msgs
)

pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml)

install(TARGETS ${PROJECT_NAME}
RUNTIME DESTINATION bin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
)
install(
DIRECTORY
include/
DESTINATION include
)

if(BUILD_TESTING)
find_package(ament_cmake_gmock REQUIRED)
find_package(controller_manager REQUIRED)
find_package(ros2_control_test_assets REQUIRED)

ament_add_gmock(test_tricycle_controller
test/test_tricycle_controller.cpp
ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml)
target_include_directories(test_tricycle_controller PRIVATE include)
target_link_libraries(test_tricycle_controller
${PROJECT_NAME}
)

ament_target_dependencies(test_tricycle_controller
geometry_msgs
hardware_interface
nav_msgs
rclcpp
rclcpp_lifecycle
realtime_tools
tf2
tf2_msgs
)

ament_add_gmock(
test_load_tricycle_controller
test/test_load_tricycle_controller.cpp
)
target_include_directories(test_load_tricycle_controller PRIVATE include)
ament_target_dependencies(test_load_tricycle_controller
controller_manager
ros2_control_test_assets
)
endif()

ament_export_include_directories(
include
)
ament_export_libraries(
${PROJECT_NAME}
)
ament_export_dependencies(
controller_interface
geometry_msgs
hardware_interface
rclcpp
rclcpp_lifecycle
)

ament_package()
43 changes: 43 additions & 0 deletions tricycle_controller/config/tricycle_drive_controller.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
tricycle_controller:
ros__parameters:
# Model
traction_joint_name: traction_joint # Name of traction joint in URDF
steering_joint_name: steering_joint # Name of steering joint in URDF
wheel_radius: 0.0 # Radius of front wheel
wheelbase: 0.0 # Distance between center of back wheels and front wheel

# Odometry
odom_frame_id: odom
base_frame_id: base_link
publish_rate: 50.0 # publish rate of odom and tf
open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry
enable_odom_tf: true # If True, publishes odom<-base_link TF
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
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
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
velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom

# Rate Limiting
traction: # All values should be positive
# min_velocity: 0.0
# max_velocity: 1000.0
# min_acceleration: 0.0
max_acceleration: 5.0
# min_deceleration: 0.0
max_deceleration: 8.0
# min_jerk: 0.0
# max_jerk: 1000.0
steering:
min_position: -1.57
max_position: 1.57
# min_velocity: 0.0
max_velocity: 1.0
# min_acceleration: 0.0
# max_acceleration: 1000.0

# cmd_vel input
cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received
use_stamped_vel: false # Set to True if using TwistStamped.

# Debug
publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s.
24 changes: 24 additions & 0 deletions tricycle_controller/doc/userdoc.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
.. _tricycle_controller_userdoc:

tricycle_controller
=====================

Controller for mobile robots with tricycle drive.
Input for control are robot base_link twist commands which are translated to traction and steering
commands for the tricycle drive base. Odometry is computed from hardware feedback and published.

Velocity commands
-----------------

The controller works with a velocity twist from which it extracts
the x component of the linear velocity and the z component of the angular velocity.
Velocities on other components are ignored.


Other features
--------------

Realtime-safe implementation.
Odometry publishing
Velocity, acceleration and jerk limits
Automatic stop after command timeout
75 changes: 75 additions & 0 deletions tricycle_controller/include/tricycle_controller/odometry.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
// Copyright 2022 Pixel Robotics.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*
* Author: Tony Najjar
*/

#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_
#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_

#include <cmath>

#include "rclcpp/time.hpp"
#include "rcppmath/rolling_mean_accumulator.hpp"

namespace tricycle_controller
{
class Odometry
{
public:
explicit Odometry(size_t velocity_rolling_window_size = 10);

bool update(double left_vel, double right_vel, const rclcpp::Duration & dt);
void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt);
void resetOdometry();

double getX() const { return x_; }
double getY() const { return y_; }
double getHeading() const { return heading_; }
double getLinear() const { return linear_; }
double getAngular() const { return angular_; }

void setWheelParams(double wheel_separation, double wheel_radius);
void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);

private:
using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;

void integrateRungeKutta2(double linear, double angular);
void integrateExact(double linear, double angular);
void resetAccumulators();

// Current pose:
double x_; // [m]
double y_; // [m]
double heading_; // [rad]

// Current velocity:
double linear_; // [m/s]
double angular_; // [rad/s]

// Wheel kinematic parameters [m]:
double wheelbase_;
double wheel_radius_;

// Rolling mean accumulators for the linear and angular velocities:
size_t velocity_rolling_window_size_;
RollingMeanAccumulator linear_accumulator_;
RollingMeanAccumulator angular_accumulator_;
};

} // namespace tricycle_controller

#endif // TRICYCLE_CONTROLLER__ODOMETRY_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
// Copyright 2022 Pixel Robotics.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*
* Author: Tony Najjar
*/

#ifndef TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_
#define TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_

#include <cmath>

namespace tricycle_controller
{

class SteeringLimiter
{
public:
/**
* \brief Constructor
* \param [in] min_position Minimum position [m] or [rad]
* \param [in] max_position Maximum position [m] or [rad]
* \param [in] min_velocity Minimum velocity [m/s] or [rad/s]
* \param [in] max_velocity Maximum velocity [m/s] or [rad/s]
* \param [in] min_acceleration Minimum acceleration [m/s^2] or [rad/s^2]
* \param [in] max_acceleration Maximum acceleration [m/s^2] or [rad/s^2]
*/
SteeringLimiter(
double min_position = NAN, double max_position = NAN,
double min_velocity = NAN, double max_velocity = NAN,
double min_acceleration = NAN, double max_acceleration = NAN);

/**
* \brief Limit the position, velocity and acceleration
* \param [in, out] p position [m] or [rad]
* \param [in] p0 Previous position to p [m] or [rad]
* \param [in] p1 Previous position to p0 [m] or [rad]
* \param [in] dt Time step [s]
* \return Limiting factor (1.0 if none)
*/
double limit(double & p, double p0, double p1, double dt);

/**
* \brief Limit the jerk
* \param [in, out] p position [m] or [rad]
* \param [in] p0 Previous position to p [m] or [rad]
* \param [in] p1 Previous position to p0 [m] or [rad]
* \param [in] dt Time step [s]
* \return Limiting factor (1.0 if none)
*/
double limit_position(double & p);

/**
* \brief Limit the velocity
* \param [in, out] p position [m]
* \return Limiting factor (1.0 if none)
*/
double limit_velocity(double & p, double p0, double dt);

/**
* \brief Limit the acceleration
* \param [in, out] p Position [m] or [rad]
* \param [in] p0 Previous position [m] or [rad]
* \param [in] dt Time step [s]
* \return Limiting factor (1.0 if none)
*/
double limit_acceleration(double & p, double p0, double p1, double dt);

private:

// Position limits:
double min_position_;
double max_position_;

// Velocity limits:
double min_velocity_;
double max_velocity_;

// Acceleration limits:
double min_acceleration_;
double max_acceleration_;

};

} // namespace tricycle_controller

#endif // TRICYCLE_CONTROLLER__STEERING_LIMITER_HPP_
Loading