diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index ad4abb6335..da94c945c5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -81,6 +81,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa controller_interface::CallbackReturn on_error( const rclcpp_lifecycle::State & previous_state) override; + // New methods to handle velocity and acceleration tolerances + void setVelocityTolerance(double tolerance); + void setAccelerationTolerance(double tolerance); + protected: // To reduce number of variables and to make the code shorter the interfaces are ordered in types // as the following constants @@ -290,6 +294,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]); } } + + double velocity_tolerance_; + double acceleration_tolerance_; }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d98bd25bd0..ad30e08692 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -55,6 +55,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() // Create the parameter listener and get the parameters param_listener_ = std::make_shared(get_node()); params_ = param_listener_->get_params(); + // Read new velocity and acceleration tolerances + velocity_tolerance_ = get_node()->get_parameter("constraints.velocity_tolerance").as_double(); + acceleration_tolerance_ = + get_node()->get_parameter("constraints.acceleration_tolerance").as_double(); } catch (const std::exception & e) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 2a4c5b343f..e7ebde558f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -160,3 +160,13 @@ joint_trajectory_controller: default_value: 0.0, description: "Per-joint trajectory offset tolerance at the goal position.", } + velocity_tolerance: { + type: double, + default_value: 0.0, + description: "Velocity tolerance for trajectory segments.", + } + acceleration_tolerance: { + type: double, + default_value: 0.0, + description: "Acceleration tolerance for trajectory segments.", + } diff --git a/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml b/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml index 356f82d685..a5a2c321d3 100644 --- a/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml +++ b/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml @@ -10,3 +10,7 @@ test_joint_trajectory_controller: state_interfaces: - position + + constraints: + velocity_tolerance: 0.05 + acceleration_tolerance: 0.1