Skip to content

Commit

Permalink
Fix bug in applying planning scene diffs that have attached collision…
Browse files Browse the repository at this point in the history 
… objects (#3124)

* Add test that trigger the bug in applying scene that have robot state
diffs

* Fix a bug in planning scene diffs where having applying two diffs would cause them to override the aco

* Fix a bug where removing an attached collision object would keep it in the scene

* Update moveit_core/planning_scene/test/test_planning_scene.cpp

Co-authored-by: AndyZe <andyz@utexas.edu>

* Update moveit_core/planning_scene/test/test_planning_scene.cpp

Co-authored-by: AndyZe <andyz@utexas.edu>

* Update moveit_core/planning_scene/src/planning_scene.cpp

Co-authored-by: AndyZe <andyz@utexas.edu>

* Check parent pointer

Co-authored-by: AndyZe <andyz@utexas.edu>
  • Loading branch information
@JafarAbdi @AndyZe
JafarAbdi and AndyZe committed May 16, 2022
1 parent 28f87cf commit 7e73858
Show file tree
Hide file tree
Showing 2 changed files with 137 additions and 1 deletion.
14 changes: 13 additions & 1 deletion moveit_core/planning_scene/src/planning_scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -552,8 +552,8 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce
else
{
scene_msg.robot_state = moveit_msgs::msg::RobotState();
scene_msg.robot_state.is_diff = true;
}
scene_msg.robot_state.is_diff = true;

if (acm_)
acm_->getMessage(scene_msg.allowed_collision_matrix);
Expand Down Expand Up @@ -611,6 +611,18 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce
if (do_omap)
getOctomapMsg(scene_msg.world.octomap);
}

// Ensure any detached collision objects get detached from the parent planning scene, too
for (const auto& collision_object : scene_msg.world.collision_objects)
{
if (parent_ && parent_->getCurrentState().hasAttachedBody(collision_object.id))
{
moveit_msgs::msg::AttachedCollisionObject aco;
aco.object.id = collision_object.id;
aco.object.operation = moveit_msgs::msg::CollisionObject::REMOVE;
scene_msg.robot_state.attached_collision_objects.push_back(aco);
}
}
}

namespace
Expand Down
124 changes: 124 additions & 0 deletions moveit_core/planning_scene/test/test_planning_scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -356,6 +356,130 @@ TEST_P(CollisionDetectorTests, ClearDiff)
parent.reset();
}

// Returns a planning scene diff message
moveit_msgs::msg::PlanningScene create_planning_scene_diff(planning_scene::PlanningScene& ps,
const std::string& object_name, const int8_t operation,
const bool attach_object = false,
const bool create_object = true)
{
// Helper function to create an object for RobotStateDiffBug
auto add_object = [](const std::string& object_name, const int8_t operation) {
moveit_msgs::msg::CollisionObject co;
co.header.frame_id = "panda_link0";
co.id = object_name;
co.operation = operation;
co.primitives.push_back([] {
shape_msgs::msg::SolidPrimitive primitive;
primitive.type = shape_msgs::msg::SolidPrimitive::SPHERE;
primitive.dimensions.push_back(1.0);
return primitive;
}());
co.primitive_poses.push_back([] {
geometry_msgs::msg::Pose pose;
pose.orientation.w = 1.0;
return pose;
}());
co.pose = co.primitive_poses[0];
return co;
};
// Helper function to create an attached object for RobotStateDiffBug
auto add_attached_object = [](const std::string& object_name, const int8_t operation) {
moveit_msgs::msg::AttachedCollisionObject aco;
aco.object.operation = operation;
aco.object.id = object_name;
aco.link_name = "panda_link0";
return aco;
};

auto new_ps = ps.diff();
if (operation == moveit_msgs::msg::CollisionObject::REMOVE ||
(operation == moveit_msgs::msg::CollisionObject::ADD && create_object))
new_ps->processCollisionObjectMsg(add_object(object_name, operation));
if (attach_object)
new_ps->processAttachedCollisionObjectMsg(add_attached_object(object_name, operation));
moveit_msgs::msg::PlanningScene scene_msg;
new_ps->getPlanningSceneDiffMsg(scene_msg);
return scene_msg;
}

// Returns collision objects names sorted alphabetically
std::set<std::string> get_collision_objects_names(const planning_scene::PlanningScene& ps)
{
std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
ps.getCollisionObjectMsgs(collision_objects);
std::set<std::string> collision_objects_names;
for (const auto& collision_object : collision_objects)
collision_objects_names.emplace(collision_object.id);
return collision_objects_names;
}

// Returns attached collision objects names sorted alphabetically
std::set<std::string> get_attached_collision_objects_names(const planning_scene::PlanningScene& ps)
{
std::vector<moveit_msgs::msg::AttachedCollisionObject> collision_objects;
ps.getAttachedCollisionObjectMsgs(collision_objects);
std::set<std::string> collision_objects_names;
for (const auto& collision_object : collision_objects)
collision_objects_names.emplace(collision_object.object.id);
return collision_objects_names;
}

TEST(PlanningScene, RobotStateDiffBug)
{
auto urdf_model = moveit::core::loadModelInterface("panda");
auto srdf_model = std::make_shared<srdf::Model>();
auto ps = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);

// It works as expected for collision objects case
{
const auto ps1 = create_planning_scene_diff(*ps, "object1", moveit_msgs::msg::CollisionObject::ADD);
const auto ps2 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD);

ps->usePlanningSceneMsg(ps2);
ps->usePlanningSceneMsg(ps1);
EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{ "object1", "object2" }));
}

{
const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::REMOVE);

ps->usePlanningSceneMsg(ps1);
EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{ "object1" }));
}

// Attached collision objects case
ps = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);

// Test that triggered a bug related to having two diffs that add two different objects
{
const auto ps1 = create_planning_scene_diff(*ps, "object1", moveit_msgs::msg::CollisionObject::ADD, true);
const auto ps2 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD, true);

ps->usePlanningSceneMsg(ps1);
ps->usePlanningSceneMsg(ps2);
EXPECT_TRUE(get_collision_objects_names(*ps).empty());
// Should print object1 and object2 -- it prints object2 only (or object1 depending on the order of operation)
EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set<std::string>{ "object1", "object2" }));
}

// Test that triggered a bug related to removing an object when having a robot state diff
{
const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::REMOVE, true);
ps->usePlanningSceneMsg(ps1);

EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{ "object2" }));
EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set<std::string>{ "object1" }));
}

{
const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::msg::CollisionObject::ADD, true, false);
ps->usePlanningSceneMsg(ps1);

EXPECT_TRUE(get_collision_objects_names(*ps).empty());
EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set<std::string>{ "object1", "object2" }));
}
}

#ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10
#define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
#endif
Expand Down

0 comments on commit 7e73858

Please sign in to comment.