Enhancement/moveit ros1 ports (backport #3041) (#3118)

* Enhancement/moveit ros1 ports (#3041) * Ports moveit/moveit#3592 * Ports moveit/moveit#3590 * Fixes compile errors --------- Co-authored-by: Sebastian Jahr <[email protected]> (cherry picked from commit 02ebcba) # Conflicts: # moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp * Accepts all incoming changes. (#3120) * Fixes member names (#3121) --------- Co-authored-by: Tom Noble <[email protected]> Co-authored-by: Mark Johnson <[email protected]> Co-authored-by: Tom Noble <[email protected]>
moveit · Nov 21, 2024 · 172c128 · 172c128
1 parent c2b0e3d
commit 172c128
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 32 deletions.
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
@@ -173,7 +173,8 @@ void CommandListManager::setStartState(const MotionResponseCont& motion_plan_res
   RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) };
   if (rob_state_op)
   {
-    moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state);
+    moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state, false);
+    start_state.is_diff = true;
   }
 }
 

diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
@@ -117,6 +117,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
       throw std::runtime_error(error);
     }
 
+    setStartStateToCurrentState();
     joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_);
 
     joint_state_target_ = std::make_shared<moveit::core::RobotState>(getRobotModel());
@@ -384,26 +385,30 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     return *joint_state_target_;
   }
 
+  void setStartState(const moveit_msgs::msg::RobotState& start_state)
+  {
+    considered_start_state_ = start_state;
+  }
+
   void setStartState(const moveit::core::RobotState& start_state)
   {
-    considered_start_state_ = std::make_shared<moveit::core::RobotState>(start_state);
+    considered_start_state_ = moveit_msgs::msg::RobotState();
+    moveit::core::robotStateToRobotStateMsg(start_state, considered_start_state_, true);
   }
 
   void setStartStateToCurrentState()
   {
-    considered_start_state_.reset();
+    // set message to empty diff
+    considered_start_state_ = moveit_msgs::msg::RobotState();
+    considered_start_state_.is_diff = true;
   }
 
   moveit::core::RobotStatePtr getStartState()
   {
-    if (considered_start_state_)
-      return considered_start_state_;
-    else
-    {
-      moveit::core::RobotStatePtr s;
-      getCurrentState(s);
-      return s;
-    }
+    moveit::core::RobotStatePtr s;
+    getCurrentState(s);
+    moveit::core::robotStateMsgToRobotState(considered_start_state_, *s, true);
+    return s;
   }
 
   bool setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, const std::string& end_effector_link,
@@ -955,11 +960,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;
-
+    req->start_state = considered_start_state_;
     req->group_name = opt_.group_name_;
     req->header.frame_id = getPoseReferenceFrame();
     req->header.stamp = getClock()->now();
@@ -1093,6 +1094,11 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     return allowed_planning_time_;
   }
 
+  void constructRobotState(moveit_msgs::msg::RobotState& state) const
+  {
+    state = considered_start_state_;
+  }
+
   void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const
   {
     request.group_name = opt_.group_name_;
@@ -1103,11 +1109,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     request.pipeline_id = planning_pipeline_id_;
     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;
+    request.start_state = considered_start_state_;
 
     if (active_target_ == JOINT)
     {
@@ -1341,7 +1343,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
   std::shared_ptr<rclcpp_action::Client<moveit_msgs::action::ExecuteTrajectory>> execute_action_client_;
 
   // general planning params
-  moveit::core::RobotStatePtr considered_start_state_;
+  moveit_msgs::msg::RobotState considered_start_state_;
   moveit_msgs::msg::WorkspaceParameters workspace_parameters_;
   double allowed_planning_time_;
   std::string planning_pipeline_id_;
@@ -1626,16 +1628,7 @@ void MoveGroupInterface::stop()
 
 void MoveGroupInterface::setStartState(const moveit_msgs::msg::RobotState& start_state)
 {
-  moveit::core::RobotStatePtr rs;
-  if (start_state.is_diff)
-    impl_->getCurrentState(rs);
-  else
-  {
-    rs = std::make_shared<moveit::core::RobotState>(getRobotModel());
-    rs->setToDefaultValues();  // initialize robot state values for conversion
-  }
-  moveit::core::robotStateMsgToRobotState(start_state, *rs);
-  setStartState(*rs);
+  impl_->setStartState(start_state);
 }
 
 void MoveGroupInterface::setStartState(const moveit::core::RobotState& start_state)