diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp index 6f101f1609..ad93bfd038 100644 --- a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp +++ b/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 --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 1d2b3836fc..45e9db2409 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/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 --git a/diff_drive_controller/src/diff_drive_controller_parameter.yaml b/diff_drive_controller/src/diff_drive_controller_parameter.yaml index b852ebc6ca..97acd56721 100644 --- a/diff_drive_controller/src/diff_drive_controller_parameter.yaml +++ b/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 --git a/diff_drive_controller/test/test_diff_drive_controller.cpp b/diff_drive_controller/test/test_diff_drive_controller.cpp index 2b5d0d4cc9..3e7d8b356a 100644 --- a/diff_drive_controller/test/test_diff_drive_controller.cpp +++ b/diff_drive_controller/test/test_diff_drive_controller.cpp @@ -672,6 +672,136 @@ 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(); + + auto wait_for_limiter = [&](double expected_vel) + { + 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(expected_vel, left_wheel_vel_cmd_->get_optional().value(), 1e-3); + EXPECT_NEAR(expected_vel, right_wheel_vel_cmd_->get_optional().value(), 1e-3); + } + }; + + 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 + wait_for_limiter(0.0); + + 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 + wait_for_limiter(linear / wheel_radius); + } + // 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 / std::abs(max_deceleration); + 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 + wait_for_limiter(0.0); + } + // 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 + wait_for_limiter(linear / wheel_radius); + } +} + TEST_F(TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1) { ASSERT_EQ(