From 77e609d2e189e4e2656b2f70bb259438feb82cda Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 21 May 2024 10:52:12 +0200 Subject: [PATCH] Add service for getting the currently set scaling factor --- .../joint_trajectory_controller.hpp | 7 +++++++ .../src/joint_trajectory_controller.cpp | 14 ++++++++++++++ 2 files changed, 21 insertions(+) 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 6b90cb07f2..33657b7d37 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 @@ -25,6 +25,7 @@ #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_msgs/srv/get_scaling_factor.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -32,6 +33,7 @@ #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "joint_trajectory_controller/visibility_control.h" + #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -315,11 +317,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool set_scaling_factor( control_msgs::srv::SetScalingFactor::Request::SharedPtr req, control_msgs::srv::SetScalingFactor::Response::SharedPtr resp); + + bool get_scaling_factor( + control_msgs::srv::GetScalingFactor::Request::SharedPtr req, + control_msgs::srv::GetScalingFactor::Response::SharedPtr resp); urdf::Model model_; realtime_tools::RealtimeBuffer scaling_factor_rt_buff_; rclcpp::Service::SharedPtr set_scaling_factor_srv_; + rclcpp::Service::SharedPtr get_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 6e03e74852..1421511bcb 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -918,6 +918,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/set_scaling_factor", std::bind( &JointTrajectoryController::set_scaling_factor, this, std::placeholders::_1, std::placeholders::_2)); + + get_scaling_factor_srv_ = get_node()->create_service( + "~/get_scaling_factor", std::bind( + &JointTrajectoryController::get_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); @@ -1635,6 +1640,15 @@ bool JointTrajectoryController::set_scaling_factor( return true; } +bool JointTrajectoryController::get_scaling_factor( + control_msgs::srv::GetScalingFactor::Request::SharedPtr /*req*/, + control_msgs::srv::GetScalingFactor::Response::SharedPtr resp) +{ + resp->scaling_factor = *(scaling_factor_rt_buff_.readFromNonRT()); + resp->success = true; + return true; +} + bool JointTrajectoryController::has_active_trajectory() const { return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg();