Add service for getting the currently set scaling factor

StoglRobotics-forks · May 24, 2024 · 77e609d · 77e609d
1 parent a685a5d
commit 77e609d
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 0 deletions.
diff --git a/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -25,13 +25,15 @@
 #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"
 #include "joint_trajectory_controller/interpolation_methods.hpp"
 #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<double> scaling_factor_rt_buff_;
   rclcpp::Service<control_msgs::srv::SetScalingFactor>::SharedPtr set_scaling_factor_srv_;
+  rclcpp::Service<control_msgs::srv::GetScalingFactor>::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
@@ -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<control_msgs::srv::GetScalingFactor>(
+    "~/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();