Skip to content

Commit

Permalink
Enhancement/moveit ros1 ports (#3041)
Browse files Browse the repository at this point in the history
* Ports moveit/moveit#3592

* Ports moveit/moveit#3590

* Fixes compile errors

---------

Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
(cherry picked from commit 02ebcba)

# Conflicts:
#	moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
  • Loading branch information
TSNoble authored and mergify[bot] committed Nov 21, 2024
1 parent c2b0e3d commit f0a5ba9
Show file tree
Hide file tree
Showing 2 changed files with 46 additions and 4 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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<moveit::core::RobotState>(getRobotModel());
joint_state_target_->setToDefaultValues();
Expand Down Expand Up @@ -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<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()
{
<<<<<<< HEAD
if (considered_start_state_)
return considered_start_state_;
else
Expand All @@ -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,
Expand Down Expand Up @@ -955,12 +975,17 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
auto req = std::make_shared<moveit_msgs::srv::GetCartesianPath::Request>();
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;
Expand Down Expand Up @@ -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_;
Expand All @@ -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)
{
Expand Down Expand Up @@ -1341,7 +1378,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_;
Expand Down Expand Up @@ -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);
Expand All @@ -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)
Expand Down

0 comments on commit f0a5ba9

Please sign in to comment.