From f0a5ba972158faa19c42fed741f181d5d490dd4f Mon Sep 17 00:00:00 2001 From: Tom Noble <53005340+TSNoble@users.noreply.github.com> Date: Mon, 28 Oct 2024 20:37:34 +0000 Subject: [PATCH] Enhancement/moveit ros1 ports (#3041) * Ports https://github.com/moveit/moveit/pull/3592/ * Ports https://github.com/moveit/moveit/pull/3590 * Fixes compile errors --------- Co-authored-by: Sebastian Jahr (cherry picked from commit 02ebcba723d80392012bbfa7f495860be464041b) # Conflicts: # moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp --- .../src/command_list_manager.cpp | 3 +- .../src/move_group_interface.cpp | 47 +++++++++++++++++-- 2 files changed, 46 insertions(+), 4 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 index a75751d77f..b3c0dceb28 100644 --- 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 index f722681ca1..7911a5c09e 100644 --- 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,7 +117,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl throw std::runtime_error(error); } +<<<<<<< HEAD joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name_); +======= + setStartStateToCurrentState(); + joint_model_group_ = getRobotModel()->getJointModelGroup(opt.group_name); +>>>>>>> 02ebcba72 (Enhancement/moveit ros1 ports (#3041)) joint_state_target_ = std::make_shared(getRobotModel()); joint_state_target_->setToDefaultValues(); @@ -384,18 +389,27 @@ 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(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() { +<<<<<<< HEAD if (considered_start_state_) return considered_start_state_; else @@ -404,6 +418,12 @@ class MoveGroupInterface::MoveGroupInterfaceImpl getCurrentState(s); return s; } +======= + moveit::core::RobotStatePtr s; + getCurrentState(s); + moveit::core::robotStateMsgToRobotState(considered_start_state_, *s, true); + return s; +>>>>>>> 02ebcba72 (Enhancement/moveit ros1 ports (#3041)) } bool setJointValueTarget(const geometry_msgs::msg::Pose& eef_pose, const std::string& end_effector_link, @@ -955,12 +975,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl auto req = std::make_shared(); moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response; +<<<<<<< HEAD if (considered_start_state_) moveit::core::robotStateToRobotStateMsg(*considered_start_state_, req->start_state); else req->start_state.is_diff = true; req->group_name = opt_.group_name_; +======= + req->start_state = considered_start_state_; + req->group_name = opt_.group_name; +>>>>>>> 02ebcba72 (Enhancement/moveit ros1 ports (#3041)) req->header.frame_id = getPoseReferenceFrame(); req->header.stamp = getClock()->now(); req->waypoints = waypoints; @@ -1093,6 +1118,14 @@ class MoveGroupInterface::MoveGroupInterfaceImpl return allowed_planning_time_; } +<<<<<<< HEAD +======= + void constructRobotState(moveit_msgs::msg::RobotState& state) const + { + state = considered_start_state_; + } + +>>>>>>> 02ebcba72 (Enhancement/moveit ros1 ports (#3041)) void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request) const { request.group_name = opt_.group_name_; @@ -1103,11 +1136,15 @@ class MoveGroupInterface::MoveGroupInterfaceImpl request.pipeline_id = planning_pipeline_id_; request.planner_id = planner_id_; request.workspace_parameters = workspace_parameters_; +<<<<<<< HEAD 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_; +>>>>>>> 02ebcba72 (Enhancement/moveit ros1 ports (#3041)) if (active_target_ == JOINT) { @@ -1341,7 +1378,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl std::shared_ptr> 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,6 +1663,7 @@ void MoveGroupInterface::stop() void MoveGroupInterface::setStartState(const moveit_msgs::msg::RobotState& start_state) { +<<<<<<< HEAD moveit::core::RobotStatePtr rs; if (start_state.is_diff) impl_->getCurrentState(rs); @@ -1636,6 +1674,9 @@ void MoveGroupInterface::setStartState(const moveit_msgs::msg::RobotState& start } moveit::core::robotStateMsgToRobotState(start_state, *rs); setStartState(*rs); +======= + impl_->setStartState(start_state); +>>>>>>> 02ebcba72 (Enhancement/moveit ros1 ports (#3041)) } void MoveGroupInterface::setStartState(const moveit::core::RobotState& start_state)