diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 45223d3302..1638555f4d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -82,6 +82,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init() controller_interface::InterfaceConfiguration JointTrajectoryController::command_interface_configuration() const { + const auto logger = get_node()->get_logger(); controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; if (dof_ == 0) @@ -101,12 +102,19 @@ JointTrajectoryController::command_interface_configuration() const conf.names.push_back(joint_name + "/" + interface_type); } } + if (!params_.speed_scaling_command_interface_name.empty()) + { + RCLCPP_WARN( + logger, "Using a command interface for setting the scaling factor isn't supported, yet."); + conf.names.push_back(params_.speed_scaling_command_interface_name); + } return conf; } controller_interface::InterfaceConfiguration JointTrajectoryController::state_interface_configuration() const { + const auto logger = get_node()->get_logger(); controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; conf.names.reserve(dof_ * params_.state_interfaces.size()); @@ -117,9 +125,12 @@ JointTrajectoryController::state_interface_configuration() const conf.names.push_back(joint_name + "/" + interface_type); } } - if (params_.exchange_scaling_factor_with_hardware) + if (!params_.speed_scaling_state_interface_name.empty()) { - conf.names.push_back(params_.speed_scaling_interface_name); + RCLCPP_INFO( + logger, "Using scaling state from the hardware from interface %s.", + params_.speed_scaling_state_interface_name.c_str()); + conf.names.push_back(params_.speed_scaling_state_interface_name); } return conf; } @@ -127,9 +138,13 @@ JointTrajectoryController::state_interface_configuration() const controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (params_.exchange_scaling_factor_with_hardware) + if (params_.speed_scaling_state_interface_name.empty()) { - if (state_interfaces_.back().get_name() == params_.speed_scaling_interface_name) + scaling_factor_ = *(scaling_factor_rt_buff_.readFromRT()); + } + else + { + if (state_interfaces_.back().get_name() == params_.speed_scaling_state_interface_name) { scaling_factor_ = state_interfaces_.back().get_value(); } @@ -137,13 +152,9 @@ controller_interface::return_type JointTrajectoryController::update( { RCLCPP_ERROR( get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", - params_.speed_scaling_interface_name.c_str()); + params_.speed_scaling_state_interface_name.c_str()); } } - else - { - scaling_factor_ = *(scaling_factor_rt_buff_.readFromRT()); - } if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -478,7 +489,8 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), [](const auto & interface) + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -548,7 +560,8 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), [](const auto & interface) + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -919,6 +932,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::placeholders::_1, std::placeholders::_2)); // set scaling factor to low value default + RCLCPP_INFO( + logger, "Setting initial scaling factor to %2f", params_.scaling_factor_initial_default); scaling_factor_rt_buff_.writeFromNonRT(params_.scaling_factor_initial_default); return CallbackReturn::SUCCESS; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index cc85332c4d..e787a8a798 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -39,21 +39,22 @@ joint_trajectory_controller: "joint_trajectory_controller::state_interface_type_combinations": null, } } - exchange_scaling_factor_with_hardware: { - type: bool, - default_value: false, - description: "Flag that mark is hardware supports setting and reading of scaling factor.\n - If set to 'true' corresponding command and state interfaces are created." - } scaling_factor_initial_default: { type: double, default_value: 1.0, description: "The initial value of the scaling factor if not exchange with hardware takes place." } - speed_scaling_interface_name: { + speed_scaling_state_interface_name: { type: string, - default_value: "speed_scaling/speed_scaling_factor", - description: "Fully qualified name of the speed scaling interface name" + default_value: null, + read_only: true, + description: "Fully qualified name of the speed scaling state interface name" + } + speed_scaling_command_interface_name: { + type: string, + default_value: null, + read_only: true, + description: "Command interface name used for setting the speed scaling factor on the hardware." } allow_partial_joints_goal: { type: bool,