Enhancement/moveit ros1 ports (#3041)

* Ports moveit/moveit#3592 * Ports moveit/moveit#3590 * Fixes compile errors --------- Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
moveit · Oct 28, 2024 · 02ebcba · 02ebcba
1 parent d2ab1b6
commit 02ebcba
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 38 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
@@ -182,7 +182,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
@@ -135,6 +135,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());
@@ -406,28 +407,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,
@@ -862,8 +865,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     auto req = std::make_shared<moveit_msgs::srv::GetCartesianPath::Request>();
     moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response;
 
-    constructRobotState(req->start_state);
-
+    req->start_state = considered_start_state_;
     req->group_name = opt_.group_name;
     req->header.frame_id = getPoseReferenceFrame();
     req->header.stamp = getClock()->now();
@@ -1008,14 +1010,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
 
   void constructRobotState(moveit_msgs::msg::RobotState& state) const
   {
-    if (considered_start_state_)
-    {
-      moveit::core::robotStateToRobotStateMsg(*considered_start_state_, state);
-    }
-    else
-    {
-      state.is_diff = true;
-    }
+    state = considered_start_state_;
   }
 
   void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const
@@ -1028,8 +1023,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     request.pipeline_id = planning_pipeline_id_;
     request.planner_id = planner_id_;
     request.workspace_parameters = workspace_parameters_;
-
-    constructRobotState(request.start_state);
+    request.start_state = considered_start_state_;
 
     if (active_target_ == JOINT)
     {
@@ -1217,7 +1211,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_;
@@ -1484,18 +1478,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)