[DiffDriveController] Use generate parameter library (#386)

Co-authored-by: Denis Štogl <[email protected]>
Co-authored-by: Bence Magyar <[email protected]>

Author: 3 people

diff_drive_controller/CMakeLists.txt

@@ -30,6 +30,12 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
 
+find_package(generate_parameter_library REQUIRED)
+
+generate_parameter_library(diff_drive_controller_parameters
+  src/diff_drive_controller_parameter.yaml
+)
+
 add_library(${PROJECT_NAME} SHARED
   src/diff_drive_controller.cpp
   src/odometry.cpp
@@ -39,6 +45,9 @@ target_include_directories(${PROJECT_NAME}
   PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
 ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(${PROJECT_NAME}
+  diff_drive_controller_parameters
+)
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(${PROJECT_NAME} PRIVATE "DIFF_DRIVE_CONTROLLER_BUILDING_DLL")

diff_drive_controller/doc/userdoc.rst

@@ -70,79 +70,10 @@ Services
 Parameters
 ------------
 
-Wheels
-,,,,,,,
-left_wheel_names [array<string>]
-  Link names of the left side wheels.
-
-right_wheel_names  [array<string>]
-  Link names of the right side wheels.
-
-wheel_separation [double]
-  Shortest distance between the left and right wheels.
-  If this parameter is wrong, the robot will not behave correctly in curves.
-
-wheels_per_side [integer]
-  Number of wheels on each wide of the robot.
-  This is important to take the wheels slip into account when multiple wheels on each side are present.
-  If there are more wheels then control signals for each side, you should enter number or control signals.
-  For example, Husky has two wheels on each side, but they use one control signal, in this case "1" is the correct value of the parameter.
-
-wheel_radius [double]
-  Radius of a wheel, i.e., wheels size, used for transformation of linear velocity into wheel rotations.
-  If this parameter is wrong the robot will move faster or slower then expected.
-
-wheel_separation_multiplier [double]
-  Correction factor for wheel separation (TODO(destogl): Please help me describe this correctly)
-
-left_wheel_radius_multiplier [double]
-  Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter.
-
-right_wheel_radius_multiplier [double]
-  Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.
-
-Odometry
-,,,,,,,,,
-base_frame_id [string] (default: "base_link")
-  Name of the frame for odometry.
-  This frame is parent of ``base_frame_id`` when controller publishes odometry.
-
-odom_frame_id [string] (default: "odom")
-  Name of the robot's base frame that is child of the odometry frame.
+Check `parameter definition file for details <https://github.com/ros-controls/ros2_controllers/blob/master/diff_drive_controller/src/diff_drive_controller_parameter.yaml>`_.
 
-pose_covariance_diagonal [array<double>[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]")
-  Odometry covariance for the encoder output of the robot for the pose.
-  These values should be tuned to your robot's sample odometry data, but these values are a good place to start:
-  ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.
-
-twist_covariance_diagonal [array<double>[6]] (default: "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]")
-  Odometry covariance for the encoder output of the robot for the speed.
-  These values should be tuned to your robot's sample odometry data, but these values are a good place to start:
-  ``[0.001, 0.001, 0.001, 0.001, 0.001, 0.01]``.
-
-open_loop [bool] (default: "false")
-  If set to false the odometry of the robot will be calculated from the commanded values and not from feedback.
-
-position_feedback [bool] (default: "true")
-  Is there position feedback from hardware.
-
-enable_odom_tf [bool] (default: "true")
-  Publish transformation between ``odom_frame_id`` and ``base_frame_id``.
-
-velocity_rolling_window_size [int] (default: "10")
-  (TODO(destogl): Please help me describe this correctly)
-
-Commands
-```````````
-cmd_vel_timeout [double] (default: "0.5s")
-  Timeout after which input command on ``cmd_vel`` topic is considered staled.
-
-publish_limited_velocity [double] (default: "false")
-  Publish limited velocity value.
-
-
-use_stamped_vel [bool] (default: "true")
-  Use stamp from input velocity message to calculate how old the command actually is.
+Note that the documentation on parameters for joint limits can be found in `their header file <https://github.com/ros-controls/ros2_control/blob/master/joint_limits/include/joint_limits/joint_limits_rosparam.hpp#L56-L75>`_.
+Those parameters are:
 
 linear.x [JointLimits structure]
   Joint limits structure for the linear X-axis.
@@ -153,6 +84,3 @@ angular.z [JointLimits structure]
   Joint limits structure for the rotation about Z-axis.
   The limiter ignores position limits.
   For details see ``joint_limits`` package from ros2_control repository.
-
-publish_rate [double] (default: "50.0")
-  Publishing rate (Hz) of the odometry and TF messages.

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -42,6 +42,8 @@
 #include "realtime_tools/realtime_publisher.h"
 #include "tf2_msgs/msg/tf_message.hpp"
 
+#include "diff_drive_controller_parameters.hpp"
+
 namespace diff_drive_controller
 {
 class DiffDriveController : public controller_interface::ControllerInterface
@@ -101,35 +103,18 @@ class DiffDriveController : public controller_interface::ControllerInterface
     const std::string & side, const std::vector<std::string> & wheel_names,
     std::vector<WheelHandle> & registered_handles);
 
-  std::vector<std::string> left_wheel_names_;
-  std::vector<std::string> right_wheel_names_;
-
   std::vector<WheelHandle> registered_left_wheel_handles_;
   std::vector<WheelHandle> registered_right_wheel_handles_;
 
-  struct WheelParams
-  {
-    size_t wheels_per_side = 0;
-    double separation = 0.0;  // w.r.t. the midpoint of the wheel width
-    double radius = 0.0;      // Assumed to be the same for both wheels
-    double separation_multiplier = 1.0;
-    double left_radius_multiplier = 1.0;
-    double right_radius_multiplier = 1.0;
-  } wheel_params_;
-
-  struct OdometryParams
-  {
-    bool open_loop = false;
-    bool position_feedback = true;
-    bool enable_odom_tf = true;
-    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_;
+  // Parameters from ROS for diff_drive_controller
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
 
   Odometry odometry_;
 
+  // Timeout to consider cmd_vel commands old
+  std::chrono::milliseconds cmd_vel_timeout_{500};
+
   std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
   std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
     realtime_odometry_publisher_ = nullptr;
@@ -139,9 +124,6 @@ class DiffDriveController : public controller_interface::ControllerInterface
   std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
     realtime_odometry_transform_publisher_ = nullptr;
 
-  // Timeout to consider cmd_vel commands old
-  std::chrono::milliseconds cmd_vel_timeout_{500};
-
   bool subscriber_is_active_ = false;
   rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr

diff_drive_controller/package.xml

@@ -9,6 +9,7 @@
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>generate_parameter_library</build_depend>
 
   <depend>controller_interface</depend>
   <depend>geometry_msgs</depend>