From ad19ea5479b00523995ac3e86b5b89295787806b Mon Sep 17 00:00:00 2001 From: Captain Yoshi Date: Sun, 12 May 2024 05:03:47 -0400 Subject: [PATCH] Add ability to move CollisionObjects (#567) --- .../task_constructor/stages/modify_planning_scene.h | 2 ++ core/src/stages/modify_planning_scene.cpp | 13 ++++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index 802daed3..51972be1 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -81,6 +81,8 @@ public: void addObject(const moveit_msgs::CollisionObject& collision_object); /// Remove an object from the planning scene void removeObject(const std::string& object_name); + /// Move an object from the planning scene + void moveObject(const moveit_msgs::CollisionObject& collision_object); /// conviency methods accepting a single object name inline void attachObject(const std::string& object, const std::string& link); diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 6b1e7fc8..c321e9d1 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -72,6 +72,15 @@ void ModifyPlanningScene::removeObject(const std::string& object_name) { collision_objects_.push_back(obj); } +void ModifyPlanningScene::moveObject(const moveit_msgs::CollisionObject& collision_object) { + if (collision_object.operation != moveit_msgs::CollisionObject::MOVE) { + ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": moveObject is called with object's operation not set " + "to MOVE -- ignoring the object"); + return; + } + collision_objects_.push_back(collision_object); +} + void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) { collision_matrix_edits_.push_back(CollisionMatrixPairs({ first, second, allow })); } @@ -128,7 +137,7 @@ std::pair ModifyPlanningScene::apply(const Interf InterfaceState state(scene); SubTrajectory traj; try { - // add/remove objects + // add/remove/move objects for (auto& collision_object : collision_objects_) processCollisionObject(*scene, collision_object, invert); @@ -157,6 +166,8 @@ void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene& const_cast(object).operation = moveit_msgs::CollisionObject::REMOVE; else if (op == moveit_msgs::CollisionObject::REMOVE) throw std::runtime_error("cannot apply removeObject() backwards"); + else if (op == moveit_msgs::CollisionObject::MOVE) + throw std::runtime_error("cannot apply moveObject() backwards"); } scene.processCollisionObjectMsg(object);