[tricycle_controller] Use generate_parameter_library (#957)

(cherry picked from commit 8d732f1)

# Conflicts:
#	tricycle_controller/src/tricycle_controller.cpp
#	tricycle_controller/test/test_tricycle_controller.cpp

tricycle_controller/CMakeLists.txt

@@ -10,6 +10,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   builtin_interfaces
   controller_interface
   geometry_msgs
+  generate_parameter_library
   hardware_interface
   nav_msgs
   pluginlib
@@ -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
@@ -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})
 
 pluginlib_export_plugin_description_file(controller_interface tricycle_controller.xml)
@@ -49,8 +55,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
   )
@@ -65,8 +70,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
@@ -84,6 +90,7 @@ install(
 install(
   TARGETS
     tricycle_controller
+    tricycle_controller_parameters
   EXPORT export_tricycle_controller
   RUNTIME DESTINATION bin
   ARCHIVE DESTINATION lib

tricycle_controller/doc/userdoc.rst

@@ -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.
 
@@ -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

tricycle_controller/include/tricycle_controller/tricycle_controller.hpp

@@ -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;
@@ -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;
@@ -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,

tricycle_controller/package.xml

@@ -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>

tricycle_controller/src/tricycle_controller.cpp

@@ -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());
@@ -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)
   {
@@ -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;
 }
 
@@ -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);
   }
@@ -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();
@@ -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;
@@ -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
@@ -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)
   {
@@ -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());
@@ -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(),
@@ -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 =
@@ -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());
@@ -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
@@ -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;
@@ -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);
 }