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 bf2ed0a614..3e9fed1fdd 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 @@ -281,6 +281,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::shared_ptr response); private: + void update_pids(); + bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 42dd7d4ec5..786c0bbaf6 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -127,6 +127,17 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + // update dynamic parameters + if (param_listener_->is_old(params_)) + { + params_ = param_listener_->get_params(); + // use_closed_loop_pid_adapter_ is updated in on_configure only + if (use_closed_loop_pid_adapter_) + { + update_pids(); + default_tolerances_ = get_segment_tolerances(params_); + } + } auto compute_error_for_joint = [&]( JointTrajectoryPoint & error, int index, @@ -716,29 +727,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( ff_velocity_scale_.resize(dof_); tmp_command_.resize(dof_, 0.0); - // Init PID gains from ROS parameters - for (size_t i = 0; i < dof_; ++i) - { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - pids_[i] = std::make_shared( - gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); - - // TODO(destogl): remove this in ROS2 Iron - // Check deprecated style for "ff_velocity_scale" parameter definition. - if (gains.ff_velocity_scale == 0.0) - { - RCLCPP_WARN( - get_node()->get_logger(), - "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " - "Maybe you are using deprecated format 'ff_velocity_scale/'!"); - - ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); - } - else - { - ff_velocity_scale_[i] = gains.ff_velocity_scale; - } - } + update_pids(); } // Configure joint position error normalization from ROS parameters (angle_wraparound) @@ -822,8 +811,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.command_interfaces).c_str(), get_interface_list(params_.state_interfaces).c_str()); - default_tolerances_ = get_segment_tolerances(params_); - + // parse remaining parameters const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -944,32 +932,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); - if (params_.cmd_timeout > 0.0) - { - if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) - { - cmd_timeout_ = params_.cmd_timeout; - } - else - { - // deactivate timeout - RCLCPP_WARN( - logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", - params_.cmd_timeout, default_tolerances_.goal_time_tolerance); - cmd_timeout_ = 0.0; - } - } - else - { - cmd_timeout_ = 0.0; - } - return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + // update the dynamic map parameters + param_listener_->refresh_dynamic_parameters(); + + // get parameters from the listener in case they were updated + params_ = param_listener_->get_params(); + + // parse remaining parameters + default_tolerances_ = get_segment_tolerances(params_); + // order all joints in the storage for (const auto & interface : params_.command_interfaces) { @@ -1045,6 +1022,28 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } rt_is_holding_.writeFromNonRT(true); + // parse timeout parameter + if (params_.cmd_timeout > 0.0) + { + if (params_.cmd_timeout > default_tolerances_.goal_time_tolerance) + { + cmd_timeout_ = params_.cmd_timeout; + } + else + { + // deactivate timeout + RCLCPP_WARN( + get_node()->get_logger(), + "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, + default_tolerances_.goal_time_tolerance); + cmd_timeout_ = 0.0; + } + } + else + { + cmd_timeout_ = 0.0; + } + return CallbackReturn::SUCCESS; } @@ -1642,6 +1641,39 @@ bool JointTrajectoryController::has_active_trajectory() const return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } +void JointTrajectoryController::update_pids() +{ + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + if (pids_[i]) + { + // update PIDs with gains from ROS parameters + pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + else + { + // Init PIDs with gains from ROS parameters + pids_[i] = std::make_shared( + gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + } + // Check deprecated style for "ff_velocity_scale" parameter definition. + if (gains.ff_velocity_scale == 0.0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "'ff_velocity_scale' parameters is not defined under 'gains..' structure. " + "Maybe you are using deprecated format 'ff_velocity_scale/'!"); + + ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + params_.joints[i], 0.0); + } + else + { + ff_velocity_scale_[i] = gains.ff_velocity_scale; + } + } +} + void JointTrajectoryController::init_hold_position_msg() { hold_position_msg_ptr_ = std::make_shared(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 43592c5d44..5812c7032b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -465,6 +465,42 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) } } +/** + * @brief check if dynamic parameters are updated + */ +TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) +{ + rclcpp::executors::MultiThreadedExecutor executor; + + SetUpAndActivateTrajectoryController(executor); + + updateController(); + auto pids = traj_controller_->get_pids(); + + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_EQ(pids.size(), 3); + auto gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, 0.0); + + double kp = 1.0; + SetPidParameters(kp); + updateController(); + + pids = traj_controller_->get_pids(); + EXPECT_EQ(pids.size(), 3); + gain_0 = pids.at(0)->getGains(); + EXPECT_EQ(gain_0.p_gain_, kp); + } + else + { + // nothing to check here, skip further test + EXPECT_EQ(pids.size(), 0); + } + + executor.cancel(); +} + /** * @brief check if hold on startup is deactivated */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b3f65536cb..58ca34ce14 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -130,6 +130,8 @@ class TestableJointTrajectoryController bool is_open_loop() const { return params_.open_loop_control; } + std::vector get_pids() const { return pids_; } + bool has_active_traj() const { return has_active_trajectory(); } bool has_trivial_traj() const