diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h new file mode 100644 index 00000000..1681479a --- /dev/null +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -0,0 +1,173 @@ +#pragma once + +#include +#include +#include + +namespace moveit { namespace planning_interface { +MOVEIT_CLASS_FORWARD(MoveGroupInterface) +} } + +namespace moveit { namespace task_constructor { namespace stages { + +class ModifyPlanningScene : public PropagatingEitherWay { +public: + typedef std::vector Names; + ModifyPlanningScene(std::string name); + + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + /// methods to attach objects to a robot link + template + void attachObjects(const C& objects, const std::string& attach_link); + template + void detachObjects(const C& objects, const std::string& attach_link); + /// attachObjects() and detachObjects() forward to this method, setting attach = true resp. false + void attachObjects(const Names& objects, const std::string& attach_link, bool attach); + + /// method to enable / disable collision between object pairs + template + void enableCollisions(const T1& first, const T2& second, bool enable_collision); + /// enable / disable all collisions for given object + void enableCollisions(const std::string& object, bool enable_collision); + +protected: + // list of objects to attach (true) / detach (false) to a given link + std::map > attach_objects_; + + // list of objects to mutually + struct CollisionMatrixPairs { + Names first; + Names second; + bool enable; + }; + std::list collision_matrix_edits_; + +protected: + // apply stored modifications to scene + void attachObjects(planning_scene::PlanningScene &scene, const std::pair >& pair, bool invert); + void setCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert); + planning_scene::PlanningScenePtr apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert); +}; + + +// As C++ doesn't allow for partial specialization of functions / methods, we need to use a helper class +namespace detail { + +// generic implementation +template struct AttachHelper { + static void impl(ModifyPlanningScene* stage, const T& objects, const std::string& link, bool attach) { + static_assert(std::is_base_of::value, "T must be a container of std::strings"); + stage->attachObjects(ModifyPlanningScene::Names(objects.cbegin(), objects.cend()), link, attach); + } +}; + +// specialization for Names +template<> +struct AttachHelper { + static void impl(ModifyPlanningScene* stage, const ModifyPlanningScene::Names& objects, const std::string& link, bool attach) { + stage->attachObjects(objects, link, attach); + } +}; + +// specialization for std::string +template<> +struct AttachHelper { + static void impl(ModifyPlanningScene* stage, const std::string& object, const std::string& link, bool attach) { + stage->attachObjects(ModifyPlanningScene::Names({object}), link, attach); + } +}; +// specialization for string literal +template +struct AttachHelper { + static void impl(ModifyPlanningScene* stage, const char object[N], const std::string& link, bool attach) { + stage->attachObjects(ModifyPlanningScene::Names({object}), link, attach); + } +}; + +} // namespace detail + +// implementation of methods forwards to AttachHelper::impl() +template +void ModifyPlanningScene::attachObjects(const T& first, const std::string& attach_link) { + detail::AttachHelper::impl(this, first, attach_link, true); +} +template +void ModifyPlanningScene::detachObjects(const T& first, const std::string& attach_link) { + detail::AttachHelper::impl(this, first, attach_link, false); +} + + + +// specialization for Names, implemented in .cpp +template <> +void ModifyPlanningScene::enableCollisions +(const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable_collision); + +// As C++ doesn't allow for partial specialization of functions / methods, we need to use a helper class +namespace detail { + +// generic implementation +template struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const T1& first, const T2& second, bool enable) { + static_assert(std::is_base_of::value, "T1 must be a container of std::strings"); + static_assert(std::is_base_of::value, "T2 must be a container of std::strings"); + stage->enableCollisions(ModifyPlanningScene::Names(first.cbegin(), first.cend()), + ModifyPlanningScene::Names(second.cbegin(), second.cend()), enable); + } +}; + +// specialization for Names +template<> +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable) { + stage->enableCollisions(first, second, enable); + } +}; + +// specialization for pair of std::string +template<> +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const std::string& first, const std::string& second, bool enable) { + stage->enableCollisions(ModifyPlanningScene::Names({first}), ModifyPlanningScene::Names({second}), enable); + } +}; +// specialization for pair of string literals +template +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const char first[N1], const char second[N2], bool enable) { + stage->enableCollisions(ModifyPlanningScene::Names({std::string(first)}), + ModifyPlanningScene::Names({std::string(second)}), enable); + } +}; + +// specialization for std::string, container +template +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const std::string& first, const T2& second, bool enable) { + stage->enableCollisions(ModifyPlanningScene::Names({first}), second, enable); + } +}; +// specialization for string literal, container +template +struct CollisionHelper { + static void impl(ModifyPlanningScene* stage, const char first[N1], const T2& second, bool enable) { + stage->enableCollisions(ModifyPlanningScene::Names({std::string(first)}), second, enable); + } +}; + +} // namespace detail + +// implementation of method forwards to CollisionHelper::impl() +template +inline void ModifyPlanningScene::enableCollisions(const T1& first, const T2& second, bool enable_collision) { + detail::CollisionHelper::impl(this, first, second, enable_collision); +} + +// single-object variant forwards to empty Names in second arg +inline void ModifyPlanningScene::enableCollisions(const std::string &object, bool enable_collision) { + enableCollisions(Names({object}), Names(), enable_collision); +} + +} } } diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index b0affc3f..3ed07fba 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -76,6 +76,7 @@ public: ~Task(); std::string id() const; + const moveit::core::RobotModelPtr getRobotModel() const; void add(Stage::pointer &&stage); void clear() override; diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index cfb89072..b2cd5d8c 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -4,6 +4,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/gripper.h + ${PROJECT_INCLUDE}/stages/modify_planning_scene.h ${PROJECT_INCLUDE}/stages/move.h cartesian_position_motion.cpp @@ -11,6 +12,7 @@ add_library(${PROJECT_NAME}_stages compute_ik.cpp generate_grasp_pose.cpp gripper.cpp + modify_planning_scene.cpp move.cpp ) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp new file mode 100644 index 00000000..eee087c3 --- /dev/null +++ b/core/src/stages/modify_planning_scene.cpp @@ -0,0 +1,77 @@ +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +ModifyPlanningScene::ModifyPlanningScene(std::string name) + : PropagatingEitherWay(name) +{} + +void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) +{ + auto it_inserted = attach_objects_.insert(std::make_pair(attach_link, std::make_pair(Names(), attach))); + Names &o = it_inserted.first->second.first; + o.insert(o.end(), objects.begin(), objects.end()); +} + +template <> +void ModifyPlanningScene::enableCollisions +(const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable_collision) { + collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, enable_collision})); +} + +bool ModifyPlanningScene::computeForward(const InterfaceState &from){ + planning_scene::PlanningScenePtr to = apply(from.scene(), false); + sendForward(from, InterfaceState(to), robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +bool ModifyPlanningScene::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from = apply(to.scene(), true); + sendBackward(InterfaceState(from), to, robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene, const std::pair >& pair, bool invert) +{ + moveit_msgs::AttachedCollisionObject obj; + obj.link_name = pair.first; + bool attach = pair.second.second; + if (invert) attach = !attach; + obj.object.operation = attach ? (int8_t) moveit_msgs::CollisionObject::ADD + : (int8_t) moveit_msgs::CollisionObject::REMOVE; + for (const std::string& name : pair.second.first) { + obj.object.id = name; + scene.processAttachedCollisionObjectMsg(obj); + } +} + +void ModifyPlanningScene::setCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert) +{ + collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst(); + bool enable = invert ? !pairs.enable : pairs.enable; + if (pairs.second.empty()) { + for (const auto &name : pairs.first) + acm.setEntry(name, enable); + } else + acm.setEntry(pairs.first, pairs.second, enable); +} + +// invert indicates, whether to detach instead of attach (and vice versa) +// as well as to disable instead of enable collision (and vice versa) +planning_scene::PlanningScenePtr ModifyPlanningScene::apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert) +{ + planning_scene::PlanningScenePtr result = scene->diff(); + // attach/detach objects + for (const auto &pair : attach_objects_) + attachObjects(*result, pair, invert); + + // enable/disable collisions + for (const auto &pairs : collision_matrix_edits_) + setCollisions(*result, pairs, invert); + return result; +} + +} } } diff --git a/core/src/task.cpp b/core/src/task.cpp index 2d19755e..3041a2eb 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -68,6 +68,10 @@ void Task::initModel () { if( !rml_->getModel() ) throw Exception("Task failed to construct RobotModel"); } +const moveit::core::RobotModelPtr Task::getRobotModel() const { + return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr(); +} + void Task::initScene() { assert(rml_);