[diff_drive] Remove unused parameter and add simple validation #abi-b…

…reaking (backport #958) (#1056)

diff_drive_controller/CMakeLists.txt

@@ -53,8 +53,7 @@ if(BUILD_TESTING)
   find_package(ros2_control_test_assets REQUIRED)
 
   ament_add_gmock(test_diff_drive_controller
-    test/test_diff_drive_controller.cpp
-    ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml)
+    test/test_diff_drive_controller.cpp)
   target_link_libraries(test_diff_drive_controller
     diff_drive_controller
   )
@@ -69,8 +68,9 @@ if(BUILD_TESTING)
     tf2_msgs
   )
 
-  ament_add_gmock(test_load_diff_drive_controller
+  add_rostest_with_parameters_gmock(test_load_diff_drive_controller
     test/test_load_diff_drive_controller.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_diff_drive_controller.yaml
   )
   ament_target_dependencies(test_load_diff_drive_controller
     controller_manager

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -110,6 +110,12 @@ class DiffDriveController : public controller_interface::ControllerInterface
   // Parameters from ROS for diff_drive_controller
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;
+  /* Number of wheels on each side 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 for 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. */
+  int wheels_per_side_;
 
   Odometry odometry_;
 

diff_drive_controller/src/diff_drive_controller.cpp

@@ -150,7 +150,7 @@ controller_interface::return_type DiffDriveController::update(
   {
     double left_feedback_mean = 0.0;
     double right_feedback_mean = 0.0;
-    for (size_t index = 0; index < static_cast<size_t>(params_.wheels_per_side); ++index)
+    for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
     {
       const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value();
       const double right_feedback =
@@ -167,8 +167,8 @@ controller_interface::return_type DiffDriveController::update(
       left_feedback_mean += left_feedback;
       right_feedback_mean += right_feedback;
     }
-    left_feedback_mean /= static_cast<double>(params_.wheels_per_side);
-    right_feedback_mean /= static_cast<double>(params_.wheels_per_side);
+    left_feedback_mean /= static_cast<double>(wheels_per_side_);
+    right_feedback_mean /= static_cast<double>(wheels_per_side_);
 
     if (params_.position_feedback)
     {
@@ -258,7 +258,7 @@ controller_interface::return_type DiffDriveController::update(
     (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius;
 
   // Set wheels velocities:
-  for (size_t index = 0; index < static_cast<size_t>(params_.wheels_per_side); ++index)
+  for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
   {
     registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left);
     registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right);
@@ -287,12 +287,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  if (params_.left_wheel_names.empty())
-  {
-    RCLCPP_ERROR(logger, "Wheel names parameters are empty!");
-    return controller_interface::CallbackReturn::ERROR;
-  }
-
   const double wheel_separation = params_.wheel_separation_multiplier * params_.wheel_separation;
   const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
   const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;
@@ -322,7 +316,7 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   }
 
   // left and right sides are both equal at this point
-  params_.wheels_per_side = params_.left_wheel_names.size();
+  wheels_per_side_ = static_cast<int>(params_.left_wheel_names.size());
 
   if (publish_limited_velocity_)
   {

diff_drive_controller/src/diff_drive_controller_parameter.yaml

@@ -2,23 +2,24 @@ diff_drive_controller:
   left_wheel_names: {
     type: string_array,
     default_value: [],
-    description: "Link names of the left side wheels",
+    description: "Names of the left side wheels' joints",
+    validation: {
+      not_empty<>: []
+    }
   }
   right_wheel_names: {
     type: string_array,
     default_value: [],
-    description: "Link names of the right side wheels",
+    description: "Names of the right side wheels' joints",
+    validation: {
+      not_empty<>: []
+    }
   }
   wheel_separation: {
     type: double,
     default_value: 0.0,
     description: "Shortest distance between the left and right wheels. If this parameter is wrong, the robot will not behave correctly in curves.",
   }
-  wheels_per_side: {
-    type: int,
-    default_value: 0,
-    description: "Number of wheels on each side 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 for 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: {
     type: double,
     default_value: 0.0,

diff_drive_controller/test/config/test_diff_drive_controller.yaml

@@ -2,7 +2,6 @@ test_diff_drive_controller:
   ros__parameters:
     left_wheel_names: ["left_wheels"]
     right_wheel_names: ["right_wheels"]
-    write_op_modes: ["motor_controller"]
 
     wheel_separation: 0.40
     wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
@@ -21,7 +20,7 @@ test_diff_drive_controller:
     open_loop: true
     enable_odom_tf: true
 
-    cmd_vel_timeout: 500 # milliseconds
+    cmd_vel_timeout: 0.5 # seconds
     publish_limited_velocity: true
     velocity_rolling_window_size: 10