Skip to content

Fix seg fault with attached objects during motion execution (backport #3466) #3470

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

Merged
merged 1 commit into from
May 13, 2025
Merged

Fix seg fault with attached objects during motion execution (backport #3466) #3470

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
14 changes: 12 additions & 2 deletions moveit_core/robot_state/src/robot_state.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1217,14 +1217,24 @@ void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bo
attached_bodies.clear();
attached_bodies.reserve(attached_body_map_.size());
for (const auto& it : attached_body_map_)
attached_bodies.push_back(it.second.get());
{
if (it.second)
{
attached_bodies.push_back(it.second.get());
}
}
}

void RobotState::getAttachedBodies(std::map<std::string, const AttachedBody*>& attached_bodies) const
{
attached_bodies.clear();
for (const auto& it : attached_body_map_)
attached_bodies[it.first] = it.second.get();
{
if (it.second && !it.first.empty())
{
attached_bodies[it.first] = it.second.get();
}
}
}

void RobotState::getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const
Expand Down
34 changes: 19 additions & 15 deletions moveit_ros/planning/plan_execution/src/plan_execution.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -283,17 +283,20 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
collision_detection::CollisionRequest req;
req.group_name = t.getGroupName();
req.pad_environment_collisions = false;
moveit::core::RobotState state = plan.planning_scene->getCurrentState();
moveit::core::RobotState start_state = plan.planning_scene->getCurrentState();
std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects, waypoint_attached_objects;
state.getAttachedBodies(current_attached_objects);
waypoint_attached_objects = plan_components_attached_objects_[path_segment.first];
start_state.getAttachedBodies(current_attached_objects);
if (plan_components_attached_objects_.size() > static_cast<size_t>(path_segment.first))
waypoint_attached_objects = plan_components_attached_objects_[path_segment.first];
moveit::core::RobotState waypoint_state(start_state);
for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
{
collision_detection::CollisionResult res;
state = t.getWayPoint(i);
waypoint_attached_objects.clear(); // clear out the last waypoints attached objects
waypoint_state = t.getWayPoint(i);
if (plan_components_attached_objects_[path_segment.first].empty())
{
state.getAttachedBodies(waypoint_attached_objects);
waypoint_state.getAttachedBodies(waypoint_attached_objects);
}

// If sample state has attached objects that are not in the current state, remove them from the sample state
Expand All @@ -302,7 +305,7 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
if (current_attached_objects.find(name) == current_attached_objects.end())
{
RCLCPP_DEBUG(logger_, "Attached object '%s' is not in the current scene. Removing it.", name.c_str());
state.clearAttachedBody(name);
waypoint_state.clearAttachedBody(name);
}
}

Expand All @@ -312,35 +315,35 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
if (waypoint_attached_objects.find(name) == waypoint_attached_objects.end())
{
RCLCPP_DEBUG(logger_, "Attached object '%s' is not in the robot state. Adding it.", name.c_str());
state.attachBody(std::make_unique<moveit::core::AttachedBody>(*object));
waypoint_state.attachBody(std::make_unique<moveit::core::AttachedBody>(*object));
}
}

if (acm)
{
plan.planning_scene->checkCollision(req, res, state, *acm);
plan.planning_scene->checkCollision(req, res, waypoint_state, *acm);
}
else
{
plan.planning_scene->checkCollision(req, res, state);
plan.planning_scene->checkCollision(req, res, waypoint_state);
}

if (res.collision || !plan.planning_scene->isStateFeasible(state, false))
if (res.collision || !plan.planning_scene->isStateFeasible(waypoint_state, false))
{
RCLCPP_INFO(logger_, "Trajectory component '%s' is invalid for waypoint %ld out of %ld",
plan.plan_components[path_segment.first].description.c_str(), i, wpc);

// call the same functions again, in verbose mode, to show what issues have been detected
plan.planning_scene->isStateFeasible(state, true);
plan.planning_scene->isStateFeasible(waypoint_state, true);
req.verbose = true;
res.clear();
if (acm)
{
plan.planning_scene->checkCollision(req, res, state, *acm);
plan.planning_scene->checkCollision(req, res, waypoint_state, *acm);
}
else
{
plan.planning_scene->checkCollision(req, res, state);
plan.planning_scene->checkCollision(req, res, waypoint_state);
}
return false;
}
Expand Down Expand Up @@ -469,7 +472,7 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
{
const auto& trajectory = component.trajectory;
std::map<std::string, const moveit::core::AttachedBody*> trajectory_attached_objects;
if (trajectory)
if (trajectory && trajectory->getWayPointCount() > 0)
{
std::map<std::string, const moveit::core::AttachedBody*> attached_objects;
trajectory->getWayPoint(0).getAttachedBodies(trajectory_attached_objects);
Expand All @@ -483,7 +486,8 @@ moveit_msgs::msg::MoveItErrorCodes plan_execution::PlanExecution::executeAndMoni
}
}
}
plan_components_attached_objects_.push_back(trajectory_attached_objects);
if (!trajectory_attached_objects.empty())
plan_components_attached_objects_.push_back(trajectory_attached_objects);
}

while (rclcpp::ok() && !execution_complete_ && !path_became_invalid_)
Expand Down
Loading