Merge branch 'main' into ch3/trajectory-cache-refactor

moveit · Oct 29, 2024 · 1ddac46 · 1ddac46
2 parents 2a3f274 + ae05a5e
commit 1ddac46
Show file tree

Hide file tree

Showing 4 changed files with 24 additions and 39 deletions.
diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py
@@ -249,7 +249,7 @@ def generate_move_group_launch(moveit_config):
         parameters=move_group_params,
         extra_debug_args=["--debug"],
         # Set the display variable, in case OpenGL code is used internally
-        additional_env={"DISPLAY": os.environ["DISPLAY"]},
+        additional_env={"DISPLAY": os.environ.get("DISPLAY", "")},
     )
     return ld
 

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/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp
@@ -383,6 +383,7 @@ void ServoNode::servoLoop()
     {
       // if no new command was created, use current robot state
       updateSlidingWindow(current_state, joint_cmd_rolling_window_, servo_params_.max_expected_latency, cur_time);
+      servo_->resetSmoothing(current_state);
     }
 
     status_msg.code = static_cast<int8_t>(servo_->getStatus());

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)