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 b60158f24c..6b90cb07f2 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 @@ -24,6 +24,7 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" +#include "control_msgs/srv/set_scaling_factor.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -311,8 +312,15 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + bool set_scaling_factor( + control_msgs::srv::SetScalingFactor::Request::SharedPtr req, + control_msgs::srv::SetScalingFactor::Response::SharedPtr resp); + urdf::Model model_; + realtime_tools::RealtimeBuffer scaling_factor_rt_buff_; + rclcpp::Service::SharedPtr set_scaling_factor_srv_; + /** * @brief Assigns the values from a trajectory point interface to a joint interface. * diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c85e48288d..45223d3302 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -117,22 +117,32 @@ JointTrajectoryController::state_interface_configuration() const conf.names.push_back(joint_name + "/" + interface_type); } } - conf.names.push_back(params_.speed_scaling_interface_name); + if (params_.exchange_scaling_factor_with_hardware) + { + conf.names.push_back(params_.speed_scaling_interface_name); + } return conf; } controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (state_interfaces_.back().get_name() == params_.speed_scaling_interface_name) + if (params_.exchange_scaling_factor_with_hardware) { - scaling_factor_ = state_interfaces_.back().get_value(); + if (state_interfaces_.back().get_name() == params_.speed_scaling_interface_name) + { + scaling_factor_ = state_interfaces_.back().get_value(); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", + params_.speed_scaling_interface_name.c_str()); + } } else { - RCLCPP_ERROR( - get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", - params_.speed_scaling_interface_name.c_str()); + scaling_factor_ = *(scaling_factor_rt_buff_.readFromRT()); } if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -468,8 +478,7 @@ 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(); }; @@ -539,8 +548,7 @@ 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(); }; @@ -900,10 +908,19 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( resize_joint_trajectory_point(state_error_, dof_); resize_joint_trajectory_point(last_commanded_state_, dof_); + // create services query_state_srv_ = get_node()->create_service( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + set_scaling_factor_srv_ = get_node()->create_service( + "~/set_scaling_factor", std::bind( + &JointTrajectoryController::set_scaling_factor, this, + std::placeholders::_1, std::placeholders::_2)); + + // set scaling factor to low value default + scaling_factor_rt_buff_.writeFromNonRT(params_.scaling_factor_initial_default); + return CallbackReturn::SUCCESS; } @@ -1600,6 +1617,24 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( } } +bool JointTrajectoryController::set_scaling_factor( + control_msgs::srv::SetScalingFactor::Request::SharedPtr req, + control_msgs::srv::SetScalingFactor::Response::SharedPtr resp) +{ + if (req->scaling_factor < 0 && req->scaling_factor > 1) + { + RCLCPP_WARN( + get_node()->get_logger(), "Scaling factor has to be in range [0, 1]. Ignoring input!"); + resp->success = false; + return true; + } + + RCLCPP_INFO(get_node()->get_logger(), "New scaling factor will be %f", req->scaling_factor); + scaling_factor_rt_buff_.writeFromNonRT(req->scaling_factor); + resp->success = true; + return true; +} + bool JointTrajectoryController::has_active_trajectory() const { return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 0940ca8ab3..cc85332c4d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -39,6 +39,17 @@ 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: { type: string, default_value: "speed_scaling/speed_scaling_factor",