From 9f45e6c2fce97ed0a38cc61445f0b2fe4506aca8 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Fri, 17 Mar 2023 12:31:22 -0400
Subject: [PATCH] Add callback for velocity scaling override + fix params
 namespace not being set (#2021)

---
 .../include/moveit_servo/servo_calcs.h        |  2 ++
 .../include/moveit_servo/servo_parameters.h   |  2 +-
 moveit_ros/moveit_servo/src/servo_calcs.cpp   | 26 +++++++++++++++++--
 .../moveit_servo/src/servo_parameters.cpp     |  2 ++
 4 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
index 5420f54584..b417ef6a34 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
+++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
@@ -337,6 +337,8 @@ class ServoCalcs
   // dynamic parameters
   std::string robot_link_command_frame_;
   rcl_interfaces::msg::SetParametersResult robotLinkCommandFrameCallback(const rclcpp::Parameter& parameter);
+  double override_velocity_scaling_factor_;
+  rcl_interfaces::msg::SetParametersResult overrideVelocityScalingFactorCallback(const rclcpp::Parameter& parameter);
 
   // Load a smoothing plugin
   pluginlib::ClassLoader<online_signal_smoothing::SmoothingBaseClass> smoothing_loader_;
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h
index efd580543a..81772b8549 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h
+++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h
@@ -54,7 +54,7 @@ struct ServoParameters
   using SharedConstPtr = std::shared_ptr<const ServoParameters>;
 
   // Parameter namespace
-  const std::string ns;
+  std::string ns;
 
   // ROS Parameters
   // Note that all of these are effectively const because the only way to create one of these
diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp
index 6e071b1d95..8201bf3269 100644
--- a/moveit_ros/moveit_servo/src/servo_calcs.cpp
+++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp
@@ -81,6 +81,7 @@ ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node,
   , stop_requested_(true)
   , paused_(false)
   , robot_link_command_frame_(parameters->robot_link_command_frame)
+  , override_velocity_scaling_factor_(parameters->override_velocity_scaling_factor)
   , smoothing_loader_("moveit_core", "online_signal_smoothing::SmoothingBaseClass")
 {
   // Register callback for changes in robot_link_command_frame
@@ -90,7 +91,16 @@ ServoCalcs::ServoCalcs(const rclcpp::Node::SharedPtr& node,
                                                                     });
   if (!callback_success)
   {
-    throw std::runtime_error("Failed to register setParameterCallback");
+    throw std::runtime_error("Failed to register setParameterCallback for robot_link_command_frame");
+  }
+
+  // Register callback for changes in override_velocity_scaling_factor
+  callback_success = parameters_->registerSetParameterCallback(
+      parameters->ns + ".override_velocity_scaling_factor",
+      [this](const rclcpp::Parameter& parameter) { return overrideVelocityScalingFactorCallback(parameter); });
+  if (!callback_success)
+  {
+    throw std::runtime_error("Failed to register setParameterCallback for override_velocity_scaling_factor");
   }
 
   // MoveIt Setup
@@ -517,6 +527,18 @@ rcl_interfaces::msg::SetParametersResult ServoCalcs::robotLinkCommandFrameCallba
   return result;
 };
 
+rcl_interfaces::msg::SetParametersResult
+ServoCalcs::overrideVelocityScalingFactorCallback(const rclcpp::Parameter& parameter)
+{
+  const std::lock_guard<std::mutex> lock(main_loop_mutex_);
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  override_velocity_scaling_factor_ = parameter.as_double();
+  RCLCPP_INFO_STREAM(LOGGER, "override_velocity_scaling_factor changed to: "
+                                 << std::to_string(override_velocity_scaling_factor_));
+  return result;
+};
+
 // Perform the servoing calculations
 bool ServoCalcs::cartesianServoCalcs(geometry_msgs::msg::TwistStamped& cmd,
                                      trajectory_msgs::msg::JointTrajectory& joint_trajectory)
@@ -705,7 +727,7 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta,
 
   // Enforce SRDF velocity limits
   enforceVelocityLimits(joint_model_group_, parameters_->publish_period, internal_joint_state_,
-                        parameters_->override_velocity_scaling_factor);
+                        override_velocity_scaling_factor_);
 
   // Enforce SRDF position limits, might halt if needed, set prev_vel to 0
   const auto joints_to_halt = enforcePositionLimits(internal_joint_state_);
diff --git a/moveit_ros/moveit_servo/src/servo_parameters.cpp b/moveit_ros/moveit_servo/src/servo_parameters.cpp
index a6f566a509..702717b808 100644
--- a/moveit_ros/moveit_servo/src/servo_parameters.cpp
+++ b/moveit_ros/moveit_servo/src/servo_parameters.cpp
@@ -95,6 +95,7 @@ void ServoParameters::declare(const std::string& ns,
   using rclcpp::ParameterType::PARAMETER_INTEGER;
   using rclcpp::ParameterType::PARAMETER_STRING;
   auto parameters = ServoParameters{};
+  parameters.ns = ns;
 
   // ROS Parameters
   node_parameters->declare_parameter(
@@ -265,6 +266,7 @@ ServoParameters ServoParameters::get(const std::string& ns,
                                      const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr& node_parameters)
 {
   auto parameters = ServoParameters{};
+  parameters.ns = ns;
 
   // ROS Parameters
   parameters.use_gazebo = node_parameters->get_parameter(ns + ".use_gazebo").as_bool();