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

fix: ensure attached objects update during motion execution #3327

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
+46 −6
Open

fix: ensure attached objects update during motion execution #3327

Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b94e50b
fix: ensure attached objects update during motion execution
MarcoMagriDev Feb 6, 2025
20df675
feat: adds `getAttachedBodies` override that returns a map instead of…
MarcoMagriDev Feb 6, 2025
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
48 changes: 42 additions & 6 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,34 +283,70 @@ bool plan_execution::PlanExecution::isRemainingPathValid(const ExecutableMotionP
collision_detection::CollisionRequest req;
req.group_name = t.getGroupName();
req.pad_environment_collisions = false;
auto getAttachedObjects = [](const moveit::core::RobotState& state) {
std::vector<const moveit::core::AttachedBody*> attached_bodies;
state.getAttachedBodies(attached_bodies);
std::map<std::string, const moveit::core::AttachedBody*> attached_objects;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does it make sense to add this function to RobotState itself instead? Like a getAttachedBodiedMap() function?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That sounds like a good idea! Adding a getAttachedBodiesMap() function to RobotState could make it more reusable and improve clarity. Would you like to propose this as a separate PR, or do you plan to integrate it into your current changes?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was suggesting including this in your PR, if you would be up for it?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I have just created the getAttachedBodies version that returns a map instead of a vector and updated the PR accordingly. Let me know if this aligns with your ideas!

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It looks good, though I would consider not having it call the other one since it seems inefficient to create a vector, then loop through that to create a map. Maybe just go straight by looping through and putting together the map?

for (const auto& ab : attached_bodies)
{
attached_objects[ab->getName()] = ab;
}
return attached_objects;
};

moveit::core::RobotState state = plan.planning_scene->getCurrentState();
std::map<std::string, const moveit::core::AttachedBody*> current_attached_objects = getAttachedObjects(state);
for (std::size_t i = std::max(path_segment.second - 1, 0); i < wpc; ++i)
{
state = t.getWayPoint(i);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for this!

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The only simplification I could think of is that, under the assumption that "attached objects are set to the waypoint's robot state at planning time" (which actually holds) we could query sample_attached_object only once. However, I proposed updating it for each waypoint to ensure robustness, even if this assumption doesn't hold. Do you see any potential issues or improvements with this?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah thanks for asking.

I guess I would be thinking about:

  1. Does it slow things down to be potentially attaching/detaching objects at each waypoint during this check?
  2. It's difficult for me to know what the user's intent would be on a case by case basis. Right now it seems like the current state is treated as the source of truth, but I'm unsure whether users want that in every case. I guess the previous implementation did the opposite and treated the attached objects from the pre-planned waypoint as the source of truth.

Is it worth maybe adding a flag to this function for whether one wants to prioritize current state vs. planned states? And elevate this up to the config/parameter level?

collision_detection::CollisionResult res;
std::map<std::string, const moveit::core::AttachedBody*> sample_attached_object = getAttachedObjects(state);

// If sample state has attached objects that are not in the current state, remove them from the sample state
for (const auto& [name, object] : sample_attached_object)
{
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);
}
}

// If current state has attached objects that are not in the sample state, add them to the sample state
for (const auto& [name, object] : current_attached_objects)
{
if (sample_attached_object.find(name) == sample_attached_object.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));
}
}

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

if (res.collision || !plan.planning_scene->isStateFeasible(t.getWayPoint(i), false))
if (res.collision || !plan.planning_scene->isStateFeasible(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(t.getWayPoint(i), true);
plan.planning_scene->isStateFeasible(state, true);
req.verbose = true;
res.clear();
if (acm)
{
plan.planning_scene->checkCollision(req, res, t.getWayPoint(i), *acm);
plan.planning_scene->checkCollision(req, res, state, *acm);
}
else
{
plan.planning_scene->checkCollision(req, res, t.getWayPoint(i));
plan.planning_scene->checkCollision(req, res, state);
}
return false;
}
Expand Down
Loading