Add new accessor methods to move group interface (#2925)

Signed-off-by: methylDragon <methylDragon@gmail.com>

moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

@@ -236,13 +236,19 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
       If the value is greater than 1, it is set to 1.0. */
   void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
 
+  /** \brief Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). */
+  double getMaxVelocityScalingFactor() const;
+
   /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration.
       Allowed values are in (0,1]. The maximum joint acceleration specified
       in the robot model is multiplied by the factor. If the value is 0, it is set to
       the default value, which is defined in joint_limits.yaml of the moveit_config.
       If the value is greater than 1, it is set to 1.0. */
   void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
 
+  /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */
+  double getMaxAccelerationScalingFactor() const;
+
   /** \brief Get the number of seconds set by setPlanningTime() */
   double getPlanningTime() const;
 
@@ -796,6 +802,14 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
   /** \brief How often is the system allowed to move the camera to update environment model when looking */
   void setLookAroundAttempts(int32_t attempts);
 
+  /** \brief Build a RobotState message for use with plan() or computeCartesianPath()
+   *  If the move_group has a custom set start state, this method will use that as the robot state.
+   *
+   *  Otherwise, the robot state will be with `is_diff` set to true, causing it to be an offset from the current state
+   *  of the robot at time of the state's use.
+   */
+  void constructRobotState(moveit_msgs::msg::RobotState& state);
+
   /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it
       in \e request */
   void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request);

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

@@ -359,11 +359,21 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     setMaxScalingFactor(max_velocity_scaling_factor_, value, "velocity_scaling_factor", 0.1);
   }
 
+  double getMaxVelocityScalingFactor() const
+  {
+    return max_velocity_scaling_factor_;
+  }
+
   void setMaxAccelerationScalingFactor(double value)
   {
     setMaxScalingFactor(max_acceleration_scaling_factor_, value, "acceleration_scaling_factor", 0.1);
   }
 
+  double getMaxAccelerationScalingFactor() const
+  {
+    return max_acceleration_scaling_factor_;
+  }
+
   void setMaxScalingFactor(double& variable, const double target_value, const char* factor_name, double fallback_value)
   {
     if (target_value > 1.0)
@@ -857,14 +867,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     auto req = std::make_shared<moveit_msgs::srv::GetCartesianPath::Request>();
     moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response;
 
-    if (considered_start_state_)
-    {
-      moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req->start_state);
-    }
-    else
-    {
-      req->start_state.is_diff = true;
-    }
+    constructRobotState(req->start_state);
 
     req->group_name = opt_.group_name;
     req->header.frame_id = getPoseReferenceFrame();
@@ -1009,6 +1012,18 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     return allowed_planning_time_;
   }
 
+  void constructRobotState(moveit_msgs::msg::RobotState& state) const
+  {
+    if (considered_start_state_)
+    {
+      moveit::core::robotStateToRobotStateMsg(*considered_start_state_, state);
+    }
+    else
+    {
+      state.is_diff = true;
+    }
+  }
+
   void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const
   {
     request.group_name = opt_.group_name;
@@ -1020,14 +1035,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     request.planner_id = planner_id_;
     request.workspace_parameters = workspace_parameters_;
 
-    if (considered_start_state_)
-    {
-      moveit::core::robotStateToRobotStateMsg(*considered_start_state_, request.start_state);
-    }
-    else
-    {
-      request.start_state.is_diff = true;
-    }
+    constructRobotState(request.start_state);
 
     if (active_target_ == JOINT)
     {
@@ -1441,11 +1449,21 @@ void MoveGroupInterface::setMaxVelocityScalingFactor(double max_velocity_scaling
   impl_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
 }
 
+double MoveGroupInterface::getMaxVelocityScalingFactor() const
+{
+  return impl_->getMaxVelocityScalingFactor();
+}
+
 void MoveGroupInterface::setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor)
 {
   impl_->setMaxAccelerationScalingFactor(max_acceleration_scaling_factor);
 }
 
+double MoveGroupInterface::getMaxAccelerationScalingFactor() const
+{
+  return impl_->getMaxAccelerationScalingFactor();
+}
+
 moveit::core::MoveItErrorCode MoveGroupInterface::asyncMove()
 {
   return impl_->move(false);
@@ -2326,6 +2344,11 @@ bool MoveGroupInterface::detachObject(const std::string& name)
   return impl_->detachObject(name);
 }
 
+void MoveGroupInterface::constructRobotState(moveit_msgs::msg::RobotState& state)
+{
+  impl_->constructRobotState(state);
+}
+
 void MoveGroupInterface::constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& goal_out)
 {
   impl_->constructMotionPlanRequest(goal_out);