From e7bb598359cbe2868648a1be89689f73053f040c Mon Sep 17 00:00:00 2001 From: Jafar Date: Mon, 16 May 2022 22:30:37 +0300 Subject: [PATCH] Fix bug in applying planning scene diffs that have attached collision objects (#3124) (#1251) * 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 * Update moveit_core/planning_scene/test/test_planning_scene.cpp Co-authored-by: AndyZe * Update moveit_core/planning_scene/src/planning_scene.cpp Co-authored-by: AndyZe * Check parent pointer Co-authored-by: AndyZe Co-authored-by: AndyZe (cherry picked from commit d482c1d8d7fec75b55db5f4430e04b2998fed97b) --- .../planning_scene/src/planning_scene.cpp | 14 +- .../test/test_planning_scene.cpp | 124 ++++++++++++++++++ 2 files changed, 137 insertions(+), 1 deletion(-) diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 9a0e314a0f..368ecfabeb 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -558,8 +558,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); @@ -617,6 +617,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 diff --git a/moveit_core/planning_scene/test/test_planning_scene.cpp b/moveit_core/planning_scene/test/test_planning_scene.cpp index a7ad233427..01e8791edb 100644 --- a/moveit_core/planning_scene/test/test_planning_scene.cpp +++ b/moveit_core/planning_scene/test/test_planning_scene.cpp @@ -328,6 +328,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 get_collision_objects_names(const planning_scene::PlanningScene& ps) +{ + std::vector collision_objects; + ps.getCollisionObjectMsgs(collision_objects); + std::set 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 get_attached_collision_objects_names(const planning_scene::PlanningScene& ps) +{ + std::vector collision_objects; + ps.getAttachedCollisionObjectMsgs(collision_objects); + std::set 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(); + auto ps = std::make_shared(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{ "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{ "object1" })); + } + + // Attached collision objects case + ps = std::make_shared(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{ "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{ "object2" })); + EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set{ "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{ "object1", "object2" })); + } +} + #ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10 #define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__) #endif