Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enhancement/moveit ros1 ports #3041

Merged
merged 5 commits into from
Oct 28, 2024
Merged

Enhancement/moveit ros1 ports #3041

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
2920dac
Ports https://github.com/moveit/moveit/pull/3592/
TSNoble Oct 24, 2024
6b1098d
Ports https://github.com/moveit/moveit/pull/3590
TSNoble Oct 24, 2024
d387c40
Merge branch 'main' into enhancement/moveit-ros1-ports
sjahr Oct 25, 2024
2083ecb
Fixes compile errors
TSNoble Oct 25, 2024
0b9e23a
Merge branch 'main' into enhancement/moveit-ros1-ports
sjahr Oct 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

Expand Down
57 changes: 20 additions & 37 deletions moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand All @@ -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)
{
Expand Down Expand Up @@ -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_;
Expand Down Expand Up @@ -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)
Expand Down
Loading