Reduce arguments to command processor

moveit · Jun 26, 2023 · 7c66202 · 7c66202
1 parent a0af0a2
commit 7c66202
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 12 deletions.
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/command_processor.hpp b/moveit_ros/moveit_servo/include/moveit_servo/command_processor.hpp
@@ -58,7 +58,6 @@ class CommandProcessor
 {
 public:
   CommandProcessor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-                   const moveit::core::JointModelGroup* joint_model_group, moveit::core::RobotStatePtr& current_state,
                    servo::Params& servo_params, StatusCode& servo_status);
 
   /**
@@ -116,7 +115,7 @@ class CommandProcessor
   size_t num_joints_;
   const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor_;
   const moveit::core::JointModelGroup* joint_model_group_;
-  moveit::core::RobotStatePtr& robot_state_;
+  moveit::core::RobotStatePtr robot_state_;
   servo::Params& servo_params_;
   StatusCode& servo_status_;
 

diff --git a/moveit_ros/moveit_servo/src/command_processor.cpp b/moveit_ros/moveit_servo/src/command_processor.cpp
@@ -48,15 +48,11 @@ namespace moveit_servo
 {
 
 CommandProcessor::CommandProcessor(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
-                                   const moveit::core::JointModelGroup* joint_model_group,
-                                   moveit::core::RobotStatePtr& current_state, servo::Params& servo_params,
-                                   StatusCode& servo_status)
-  : planning_scene_monitor_{ planning_scene_monitor }
-  , joint_model_group_{ joint_model_group }
-  , robot_state_{ current_state }
-  , servo_params_{ servo_params }
-  , servo_status_{ servo_status }
+                                   servo::Params& servo_params, StatusCode& servo_status)
+  : planning_scene_monitor_{ planning_scene_monitor }, servo_params_{ servo_params }, servo_status_{ servo_status }
 {
+  robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState();
+  joint_model_group_ = robot_state_->getJointModelGroup(servo_params_.move_group_name);
   num_joints_ = joint_model_group_->getActiveJointModelNames().size();
   setIKSolver();
 }

diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp
@@ -88,8 +88,7 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
   else
   {
     // Create the command processor to handle the different types of servo inputs.
-    command_processor_ = std::make_unique<CommandProcessor>(planning_scene_monitor_, joint_model_group_, robot_state_,
-                                                            servo_params_, servo_status_);
+    command_processor_ = std::make_unique<CommandProcessor>(planning_scene_monitor_, servo_params_, servo_status_);
 
     // Get necessary information about joints
     joint_names_ = joint_model_group_->getActiveJointModelNames();