Skip to content

Commit

Permalink
[tricycle_controller] Use generate_parameter_library (#957)
Browse files Browse the repository at this point in the history 
(cherry picked from commit 8d732f1)

# Conflicts:
#	tricycle_controller/src/tricycle_controller.cpp
#	tricycle_controller/test/test_tricycle_controller.cpp
  • Loading branch information
@christophfroehlich
christophfroehlich committed Mar 13, 2024
1 parent 468e030 commit dbe2658
Show file tree
Hide file tree
Showing 8 changed files with 300 additions and 127 deletions.
13 changes: 10 additions & 3 deletions tricycle_controller/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
builtin_interfaces
controller_interface
geometry_msgs
generate_parameter_library
hardware_interface
nav_msgs
pluginlib
Expand All @@ -28,6 +29,10 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
find_package(${Dependency} REQUIRED)
endforeach()

generate_parameter_library(tricycle_controller_parameters
src/tricycle_controller_parameter.yaml
)

add_library(tricycle_controller SHARED
src/tricycle_controller.cpp
src/odometry.cpp
Expand All @@ -39,6 +44,7 @@ target_include_directories(tricycle_controller PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/tricycle_controller>
)
target_link_libraries(tricycle_controller PUBLIC tricycle_controller_parameters)
ament_target_dependencies(tricycle_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_compile_definitions(tricycle_controller PRIVATE _USE_MATH_DEFINES)

Expand All @@ -50,8 +56,7 @@ if(BUILD_TESTING)
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)
test/test_tricycle_controller.cpp)
target_link_libraries(test_tricycle_controller
tricycle_controller
)
Expand All @@ -66,8 +71,9 @@ if(BUILD_TESTING)
tf2_msgs
)

ament_add_gmock(test_load_tricycle_controller
add_rostest_with_parameters_gmock(test_load_tricycle_controller
test/test_load_tricycle_controller.cpp
${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_tricycle_controller.yaml
)
target_link_libraries(test_load_tricycle_controller
tricycle_controller
Expand All @@ -85,6 +91,7 @@ install(
install(
TARGETS
tricycle_controller
tricycle_controller_parameters
EXPORT export_tricycle_controller
RUNTIME DESTINATION bin
ARCHIVE DESTINATION lib
Expand Down
10 changes: 9 additions & 1 deletion tricycle_controller/doc/userdoc.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,8 @@
tricycle_controller
=====================

Controller for mobile robots with tricycle drive.
Controller for mobile robots with a single double-actuated wheel, including traction and steering. An example is a tricycle robot with the actuated wheel in the front and two trailing wheels on the rear axle.

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.

Expand All @@ -24,3 +25,10 @@ Other features
Odometry publishing
Velocity, acceleration and jerk limits
Automatic stop after command timeout

Parameters
--------------

This controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters.

.. generate_parameter_library_details:: ../src/tricycle_controller_parameter.yaml
27 changes: 6 additions & 21 deletions tricycle_controller/include/tricycle_controller/tricycle_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,9 @@
#include "tricycle_controller/traction_limiter.hpp"
#include "tricycle_controller/visibility_control.h"

// auto-generated by generate_parameter_library
#include "tricycle_controller_parameters.hpp"

namespace tricycle_controller
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
Expand Down Expand Up @@ -109,31 +112,14 @@ class TricycleController : public controller_interface::ControllerInterface
double convert_trans_rot_vel_to_steering_angle(double v, double omega, double wheelbase);
std::tuple<double, double> twist_to_ackermann(double linear_command, double angular_command);

std::string traction_joint_name_;
std::string steering_joint_name_;
// Parameters from ROS for tricycle_controller
std::shared_ptr<ParamListener> param_listener_;
Params params_;

// HACK: put into vector to avoid initializing structs because they have no default constructors
std::vector<TractionHandle> traction_joint_;
std::vector<SteeringHandle> steering_joint_;

struct WheelParams
{
double wheelbase = 0.0;
double radius = 0.0;
} wheel_params_;

struct OdometryParams
{
bool open_loop = false;
bool enable_odom_tf = false;
bool odom_only_twist = false; // for doing the pose integration in separate node
std::string base_frame_id = "base_link";
std::string odom_frame_id = "odom";
std::array<double, 6> pose_covariance_diagonal;
std::array<double, 6> twist_covariance_diagonal;
} odom_params_;

bool publish_ackermann_command_ = false;
std::shared_ptr<rclcpp::Publisher<AckermannDrive>> ackermann_command_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<AckermannDrive>>
realtime_ackermann_command_publisher_ = nullptr;
Expand Down Expand Up @@ -168,7 +154,6 @@ class TricycleController : public controller_interface::ControllerInterface
SteeringLimiter limiter_steering_;

bool is_halted = false;
bool use_stamped_vel_ = true;

void reset_odometry(
const std::shared_ptr<rmw_request_id_t> request_header,
Expand Down
1 change: 1 addition & 0 deletions tricycle_controller/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
<author email="tony.najjar@logivations.com">Tony Najjar</author>

<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>generate_parameter_library</build_depend>

<depend>ackermann_msgs</depend>
<depend>backward_ros</depend>
Expand Down
119 changes: 45 additions & 74 deletions tricycle_controller/src/tricycle_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ CallbackReturn TricycleController::on_init()
{
try
{
<<<<<<< HEAD
// with the lifecycle node being initialized, we can declare parameters
auto_declare<std::string>("traction_joint_name", std::string());
auto_declare<std::string>("steering_joint_name", std::string());
Expand Down Expand Up @@ -87,6 +88,11 @@ CallbackReturn TricycleController::on_init()
auto_declare<double>("steering.min_velocity", NAN);
auto_declare<double>("steering.max_acceleration", NAN);
auto_declare<double>("steering.min_acceleration", NAN);
=======
// Create the parameter listener and get the parameters
param_listener_ = std::make_shared<ParamListener>(get_node());
params_ = param_listener_->get_params();
>>>>>>> 8d732f1 ([tricycle_controller] Use generate_parameter_library (#957))
}
catch (const std::exception & e)
{
Expand All @@ -101,17 +107,17 @@ InterfaceConfiguration TricycleController::command_interface_configuration() con
{
InterfaceConfiguration command_interfaces_config;
command_interfaces_config.type = interface_configuration_type::INDIVIDUAL;
command_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY);
command_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION);
command_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY);
command_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION);
return command_interfaces_config;
}

InterfaceConfiguration TricycleController::state_interface_configuration() const
{
InterfaceConfiguration state_interfaces_config;
state_interfaces_config.type = interface_configuration_type::INDIVIDUAL;
state_interfaces_config.names.push_back(traction_joint_name_ + "/" + HW_IF_VELOCITY);
state_interfaces_config.names.push_back(steering_joint_name_ + "/" + HW_IF_POSITION);
state_interfaces_config.names.push_back(params_.traction_joint_name + "/" + HW_IF_VELOCITY);
state_interfaces_config.names.push_back(params_.steering_joint_name + "/" + HW_IF_POSITION);
return state_interfaces_config;
}

Expand Down Expand Up @@ -151,7 +157,7 @@ controller_interface::return_type TricycleController::update(
double Ws_read = traction_joint_[0].velocity_state.get().get_value(); // in radians/s
double alpha_read = steering_joint_[0].position_state.get().get_value(); // in radians

if (odom_params_.open_loop)
if (params_.open_loop)
{
odometry_.updateOpenLoop(linear_command, angular_command, period);
}
Expand All @@ -172,7 +178,7 @@ controller_interface::return_type TricycleController::update(
{
auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.stamp = time;
if (!odom_params_.odom_only_twist)
if (!params_.odom_only_twist)
{
odometry_message.pose.pose.position.x = odometry_.getX();
odometry_message.pose.pose.position.y = odometry_.getY();
Expand All @@ -186,7 +192,7 @@ controller_interface::return_type TricycleController::update(
realtime_odometry_publisher_->unlockAndPublish();
}

if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
{
auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
transform.header.stamp = time;
Expand Down Expand Up @@ -239,7 +245,7 @@ controller_interface::return_type TricycleController::update(
previous_commands_.emplace(ackermann_command);

// Publish ackermann command
if (publish_ackermann_command_ && realtime_ackermann_command_publisher_->trylock())
if (params_.publish_ackermann_command && realtime_ackermann_command_publisher_->trylock())
{
auto & realtime_ackermann_command = realtime_ackermann_command_publisher_->msg_;
// speed in AckermannDrive is defined desired forward speed (m/s) but we use it here as wheel
Expand All @@ -258,74 +264,40 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
{
auto logger = get_node()->get_logger();

// update parameters
traction_joint_name_ = get_node()->get_parameter("traction_joint_name").as_string();
steering_joint_name_ = get_node()->get_parameter("steering_joint_name").as_string();
if (traction_joint_name_.empty())
// update parameters if they have changed
if (param_listener_->is_old(params_))
{
RCLCPP_ERROR(logger, "'traction_joint_name' parameter was empty");
return CallbackReturn::ERROR;
params_ = param_listener_->get_params();
RCLCPP_INFO(logger, "Parameters were updated");
}
if (steering_joint_name_.empty())
{
RCLCPP_ERROR(logger, "'steering_joint_name' parameter was empty");
return CallbackReturn::ERROR;
}

wheel_params_.wheelbase = get_node()->get_parameter("wheelbase").as_double();
wheel_params_.radius = get_node()->get_parameter("wheel_radius").as_double();

odometry_.setWheelParams(wheel_params_.wheelbase, wheel_params_.radius);
odometry_.setVelocityRollingWindowSize(
get_node()->get_parameter("velocity_rolling_window_size").as_int());
odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius);
odometry_.setVelocityRollingWindowSize(params_.velocity_rolling_window_size);

odom_params_.odom_frame_id = get_node()->get_parameter("odom_frame_id").as_string();
odom_params_.base_frame_id = get_node()->get_parameter("base_frame_id").as_string();

auto pose_diagonal = get_node()->get_parameter("pose_covariance_diagonal").as_double_array();
std::copy(
pose_diagonal.begin(), pose_diagonal.end(), odom_params_.pose_covariance_diagonal.begin());

auto twist_diagonal = get_node()->get_parameter("twist_covariance_diagonal").as_double_array();
std::copy(
twist_diagonal.begin(), twist_diagonal.end(), odom_params_.twist_covariance_diagonal.begin());

odom_params_.open_loop = get_node()->get_parameter("open_loop").as_bool();
odom_params_.enable_odom_tf = get_node()->get_parameter("enable_odom_tf").as_bool();
odom_params_.odom_only_twist = get_node()->get_parameter("odom_only_twist").as_bool();

cmd_vel_timeout_ =
std::chrono::milliseconds{get_node()->get_parameter("cmd_vel_timeout").as_int()};
publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool();
use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool();
cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout};
params_.publish_ackermann_command =
get_node()->get_parameter("publish_ackermann_command").as_bool();
params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool();

try
{
limiter_traction_ = TractionLimiter(
get_node()->get_parameter("traction.min_velocity").as_double(),
get_node()->get_parameter("traction.max_velocity").as_double(),
get_node()->get_parameter("traction.min_acceleration").as_double(),
get_node()->get_parameter("traction.max_acceleration").as_double(),
get_node()->get_parameter("traction.min_deceleration").as_double(),
get_node()->get_parameter("traction.max_deceleration").as_double(),
get_node()->get_parameter("traction.min_jerk").as_double(),
get_node()->get_parameter("traction.max_jerk").as_double());
params_.traction.min_velocity, params_.traction.max_velocity,
params_.traction.min_acceleration, params_.traction.max_acceleration,
params_.traction.min_deceleration, params_.traction.max_deceleration,
params_.traction.min_jerk, params_.traction.max_jerk);
}
catch (const std::invalid_argument & e)
{
RCLCPP_ERROR(get_node()->get_logger(), "Error configuring traction limiter: %s", e.what());
return CallbackReturn::ERROR;
}

try
{
limiter_steering_ = SteeringLimiter(
get_node()->get_parameter("steering.min_position").as_double(),
get_node()->get_parameter("steering.max_position").as_double(),
get_node()->get_parameter("steering.min_velocity").as_double(),
get_node()->get_parameter("steering.max_velocity").as_double(),
get_node()->get_parameter("steering.min_acceleration").as_double(),
get_node()->get_parameter("steering.max_acceleration").as_double());
params_.steering.min_position, params_.steering.max_position, params_.steering.min_velocity,
params_.steering.max_velocity, params_.steering.min_acceleration,
params_.steering.max_acceleration);
}
catch (const std::invalid_argument & e)
{
Expand All @@ -347,7 +319,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
previous_commands_.emplace(empty_ackermann_drive);

// initialize ackermann command publisher
if (publish_ackermann_command_)
if (params_.publish_ackermann_command)
{
ackermann_command_publisher_ = get_node()->create_publisher<AckermannDrive>(
DEFAULT_ACKERMANN_OUT_TOPIC, rclcpp::SystemDefaultsQoS());
Expand All @@ -357,7 +329,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
}

// initialize command subscriber
if (use_stamped_vel_)
if (params_.use_stamped_vel)
{
velocity_command_subscriber_ = get_node()->create_subscription<TwistStamped>(
DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
Expand Down Expand Up @@ -409,8 +381,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
odometry_publisher_);

auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.frame_id = odom_params_.odom_frame_id;
odometry_message.child_frame_id = odom_params_.base_frame_id;
odometry_message.header.frame_id = params_.odom_frame_id;
odometry_message.child_frame_id = params_.base_frame_id;

// initialize odom values zeros
odometry_message.twist =
Expand All @@ -421,13 +393,12 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
{
// 0, 7, 14, 21, 28, 35
const size_t diagonal_index = NUM_DIMENSIONS * index + index;
odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index];
odometry_message.twist.covariance[diagonal_index] =
odom_params_.twist_covariance_diagonal[index];
odometry_message.pose.covariance[diagonal_index] = params_.pose_covariance_diagonal[index];
odometry_message.twist.covariance[diagonal_index] = params_.twist_covariance_diagonal[index];
}

// initialize transform publisher and message
if (odom_params_.enable_odom_tf)
if (params_.enable_odom_tf)
{
odometry_transform_publisher_ = get_node()->create_publisher<tf2_msgs::msg::TFMessage>(
DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS());
Expand All @@ -438,8 +409,8 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
// keeping track of odom and base_link transforms only
auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_;
odometry_transform_message.transforms.resize(1);
odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id;
odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id;
odometry_transform_message.transforms.front().header.frame_id = params_.odom_frame_id;
odometry_transform_message.transforms.front().child_frame_id = params_.base_frame_id;
}

// Create odom reset service
Expand All @@ -456,8 +427,8 @@ CallbackReturn TricycleController::on_activate(const rclcpp_lifecycle::State &)
RCLCPP_INFO(get_node()->get_logger(), "On activate: Initialize Joints");

// Initialize the joints
const auto wheel_front_result = get_traction(traction_joint_name_, traction_joint_);
const auto steering_result = get_steering(steering_joint_name_, steering_joint_);
const auto wheel_front_result = get_traction(params_.traction_joint_name, traction_joint_);
const auto steering_result = get_steering(params_.steering_joint_name, steering_joint_);
if (wheel_front_result == CallbackReturn::ERROR || steering_result == CallbackReturn::ERROR)
{
return CallbackReturn::ERROR;
Expand Down Expand Up @@ -644,12 +615,12 @@ std::tuple<double, double> TricycleController::twist_to_ackermann(double Vx, dou
if (Vx == 0 && theta_dot != 0)
{ // is spin action
alpha = theta_dot > 0 ? M_PI_2 : -M_PI_2;
Ws = abs(theta_dot) * wheel_params_.wheelbase / wheel_params_.radius;
Ws = abs(theta_dot) * params_.wheelbase / params_.wheel_radius;
return std::make_tuple(alpha, Ws);
}

alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, wheel_params_.wheelbase);
Ws = Vx / (wheel_params_.radius * std::cos(alpha));
alpha = convert_trans_rot_vel_to_steering_angle(Vx, theta_dot, params_.wheelbase);
Ws = Vx / (params_.wheel_radius * std::cos(alpha));
return std::make_tuple(alpha, Ws);
}

Expand Down
Loading

0 comments on commit dbe2658

Please sign in to comment.