diff_drive_controller: set parameters as read_only

diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp

@@ -55,7 +55,33 @@ class SpeedLimiter
       min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration,
       max_deceleration_reverse, min_jerk, max_jerk);
   }
-
+  /**
+   * \brief Update parameters at runtime
+   * \param [in] min_velocity Minimum velocity [m/s], usually <= 0
+   * \param [in] max_velocity Maximum velocity [m/s], usually >= 0
+   * \param [in] max_acceleration_reverse Maximum acceleration in reverse direction [m/s^2], usually
+   * <= 0
+   * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
+   * \param [in] max_deceleration Maximum deceleration [m/s^2], usually <= 0
+   * \param [in] max_deceleration_reverse Maximum deceleration in reverse direction [m/s^2], usually
+   * >= 0
+   * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
+   * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
+   *
+   * \note
+   * If max_* values are NAN, the respective limit is deactivated
+   * If min_* values are NAN (unspecified), defaults to -max
+   * If min_first_derivative_pos/max_first_derivative_neg values are NAN, symmetric limits are used
+   */
+  void set_params(
+    double min_velocity, double max_velocity, double max_acceleration_reverse,
+    double max_acceleration, double max_deceleration, double max_deceleration_reverse,
+    double min_jerk, double max_jerk)
+  {
+    speed_limiter_.set_params(
+      min_velocity, max_velocity, max_acceleration_reverse, max_acceleration, max_deceleration,
+      max_deceleration_reverse, min_jerk, max_jerk);
+  }
   /**
    * \brief Limit the velocity, acceleration, and jerk
    * \param [in, out] v  Velocity [m/s]

diff_drive_controller/src/diff_drive_controller.cpp

@@ -147,6 +147,28 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
 {
   auto logger = get_node()->get_logger();
 
+  if (param_listener_->try_update_params(params_))
+  {
+    cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout);
+    try
+    {
+      limiter_linear_->set_params(
+        params_.linear.x.min_velocity, params_.linear.x.max_velocity,
+        params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration,
+        params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse,
+        params_.linear.x.min_jerk, params_.linear.x.max_jerk);
+      limiter_angular_->set_params(
+        params_.angular.z.min_velocity, params_.angular.z.max_velocity,
+        params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration,
+        params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse,
+        params_.angular.z.min_jerk, params_.angular.z.max_jerk);
+    }
+    catch (const std::invalid_argument & e)
+    {
+      RCLCPP_ERROR(logger, "Failed to update speed limiter parameters: %s", e.what());
+    }
+  }
+
   // command may be limited further by SpeedLimit,
   // without affecting the stored twist command
   double linear_command = reference_interfaces_[0];

diff_drive_controller/src/diff_drive_controller_parameter.yaml

@@ -3,6 +3,7 @@ diff_drive_controller:
     type: string_array,
     default_value: [],
     description: "Names of the left side wheels' joints",
+    read_only: true,
     validation: {
       not_empty<>: []
     }
@@ -11,6 +12,7 @@ diff_drive_controller:
     type: string_array,
     default_value: [],
     description: "Names of the right side wheels' joints",
+    read_only: true,
     validation: {
       not_empty<>: []
     }
@@ -19,6 +21,7 @@ diff_drive_controller:
     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.",
+    read_only: true,
     validation: {
       gt<>: [0.0]
     }
@@ -27,6 +30,7 @@ diff_drive_controller:
     type: double,
     default_value: 0.0,
     description: "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 than expected.",
+    read_only: true,
     validation: {
       gt<>: [0.0]
     }
@@ -35,56 +39,67 @@ diff_drive_controller:
     type: double,
     default_value: 1.0,
     description: "Correction factor when the actual wheel separation differs from the nominal value in the ``wheel_separation`` parameter.",
+    read_only: true,
   }
   left_wheel_radius_multiplier: {
     type: double,
     default_value: 1.0,
     description: "Correction factor when radius of left wheels differs from the nominal value in ``wheel_radius`` parameter.",
+    read_only: true,
   }
   right_wheel_radius_multiplier: {
     type: double,
     default_value: 1.0,
     description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.",
+    read_only: true,
   }
   tf_frame_prefix_enable: {
     type: bool,
     default_value: true,
     description:  "Deprecated: this parameter will be removed in a future release. Use 'tf_frame_prefix' instead.",
+    read_only: true,
   }
   tf_frame_prefix: {
     type: string,
     default_value: "",
     description:  "(optional) Prefix to be appended to the tf frames, will be added to odom_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.",
+    read_only: true,
   }
   odom_frame_id: {
     type: string,
     default_value: "odom",
     description:  "Name of the frame for odometry. This frame is parent of ``base_frame_id`` when controller publishes odometry.",
+    read_only: true,
   }
   base_frame_id: {
     type: string,
     default_value: "base_link",
     description: "Name of the robot's base frame that is child of the odometry frame.",
+    read_only: true,
   }
   pose_covariance_diagonal: {
     type: double_array,
     default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
     description: "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]``.",
+    read_only: true,
   }
   twist_covariance_diagonal: {
     type: double_array,
     default_value: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
     description: "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]``.",
+    read_only: true,
   }
   open_loop: {
     type: bool,
     default_value: false,
     description: "If set to true the odometry of the robot will be calculated from the commanded values and not from feedback.",
+    read_only: true,
   }
   position_feedback: {
     type: bool,
     default_value: true,
     description: "Is there position feedback from hardware.",
+    read_only: true,
   }
   enable_odom_tf: {
     type: bool,
@@ -105,12 +120,14 @@ diff_drive_controller:
     type: int,
     default_value: 10,
     description: "Size of the rolling window for calculation of mean velocity use in odometry.",
+    read_only: true,
   }
   # TODO(bhavin-umatiya): Remove this parameter as it is deprecated
   publish_rate: {
     type: double,
     default_value: 50.0, # Hz
     description: "Publishing rate (Hz) of the odometry and TF messages. This parameter is deprecated and will be removed in a future release.",
+    read_only: true,
   }
   linear:
     x:

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -672,6 +672,152 @@ TEST_F(TestDiffDriveController, test_speed_limiter)
   }
 }
 
+TEST_F(TestDiffDriveController, test_speed_limiter_runtime_update)
+{
+  // If you send a linear velocity command without acceleration limits,
+  // then the wheel velocity command (rotations/s) will be:
+  // ideal_wheel_velocity_command (rotations/s) = linear_velocity_command (m/s) / wheel_radius (m).
+  // (The velocity command looks like a step function).
+  // However, if there are acceleration limits, then the actual wheel velocity command
+  // should always be less than the ideal velocity, and should only become
+  // equal at time = linear_velocity_command (m/s) / acceleration_limit (m/s^2).
+  // This test verifies that parameters can be updated at runtime.
+  const double max_acceleration_1 = 2.0;
+  const double max_acceleration_2 = 5.0;
+  const double max_deceleration = -4.0;
+  ASSERT_EQ(
+    InitController(
+      left_wheel_names, right_wheel_names,
+      {
+        rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(max_acceleration_1)),
+        rclcpp::Parameter("linear.x.max_deceleration", rclcpp::ParameterValue(max_deceleration)),
+      }),
+    controller_interface::return_type::OK);
+  rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(controller_->get_node()->get_node_base_interface());
+
+  ASSERT_TRUE(configure_succeeds(controller_));
+
+  assignResourcesPosFeedback();
+
+  ASSERT_TRUE(activate_succeeds(controller_));
+
+  waitForSetup(executor);
+
+  // send msg
+  publish(0.0, 0.0);
+  // wait for msg is be published to the system
+  controller_->wait_for_twist(executor);
+  // wait for the speed limiter to fill the queue
+  for (int i = 0; i < 3; ++i)
+  {
+    ASSERT_EQ(
+      controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+      controller_interface::return_type::OK);
+    EXPECT_NEAR(0.0, left_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    EXPECT_NEAR(0.0, right_wheel_vel_cmd_->get_optional().value(), 1e-3);
+  }
+
+  const double dt = 0.001;
+  const double wheel_radius = 0.1;
+  // Phase 1: accelerate with max_acceleration = 2.0
+  {
+    const double linear = 1.0;
+    // send msg
+    publish(linear, 0.0);
+    // wait for msg is be published to the system
+    controller_->wait_for_twist(executor);
+    const double time_acc = linear / max_acceleration_1;
+    for (int i = 0; i < floor(time_acc / dt) - 1; ++i)
+    {
+      ASSERT_EQ(
+        controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)),
+        controller_interface::return_type::OK);
+    }
+    ASSERT_EQ(
+      controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)),
+      controller_interface::return_type::OK);
+    EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    // wait for the speed limiter to fill the queue
+    for (int i = 0; i < 3; ++i)
+    {
+      ASSERT_EQ(
+        controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+        controller_interface::return_type::OK);
+      EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3);
+      EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    }
+  }
+  // Stop the robot
+  {
+    const double linear = 0.0;
+    // send msg
+    publish(linear, 0.0);
+    // wait for msg is be published to the system
+    controller_->wait_for_twist(executor);
+    const double time_dec = 1.0 / 4.0;
+    for (int i = 0; i < floor(time_dec / dt) - 1; ++i)
+    {
+      ASSERT_EQ(
+        controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)),
+        controller_interface::return_type::OK);
+    }
+    ASSERT_EQ(
+      controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)),
+      controller_interface::return_type::OK);
+    EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    // wait for the speed limiter to fill the queue
+    for (int i = 0; i < 3; ++i)
+    {
+      ASSERT_EQ(
+        controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+        controller_interface::return_type::OK);
+      EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3);
+      EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    }
+  }
+  // Phase 2: update parameter at runtime to max_acceleration = 5.0
+  {
+    auto result = controller_->get_node()->set_parameter(
+      rclcpp::Parameter("linear.x.max_acceleration", rclcpp::ParameterValue(max_acceleration_2)));
+    ASSERT_TRUE(result.successful);
+  }
+  // Phase 3: accelerate with max_acceleration = 5.0
+  {
+    const double linear = 1.0;
+    // send msg
+    publish(linear, 0.0);
+    // wait for msg is be published to the system
+    controller_->wait_for_twist(executor);
+    const double time_acc_1 = linear / max_acceleration_1;
+    const double time_acc_2 = linear / max_acceleration_2;
+    // With higher acceleration, should reach target faster
+    ASSERT_LT(time_acc_2, time_acc_1);
+    for (int i = 0; i < floor(time_acc_2 / dt) - 1; ++i)
+    {
+      ASSERT_EQ(
+        controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)),
+        controller_interface::return_type::OK);
+    }
+    ASSERT_EQ(
+      controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(dt)),
+      controller_interface::return_type::OK);
+    EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    // wait for the speed limiter to fill the queue
+    for (int i = 0; i < 3; ++i)
+    {
+      ASSERT_EQ(
+        controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
+        controller_interface::return_type::OK);
+      EXPECT_NEAR(linear / wheel_radius, left_wheel_vel_cmd_->get_optional().value(), 1e-3);
+      EXPECT_NEAR(linear / wheel_radius, right_wheel_vel_cmd_->get_optional().value(), 1e-3);
+    }
+  }
+}
+
 TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1)
 {
   ASSERT_EQ(