diff --git a/core/include/moveit/task_constructor/marker_tools.h b/core/include/moveit/task_constructor/marker_tools.h index 05f88803..3b1a6f32 100644 --- a/core/include/moveit/task_constructor/marker_tools.h +++ b/core/include/moveit/task_constructor/marker_tools.h @@ -10,6 +10,7 @@ MOVEIT_CLASS_FORWARD(PlanningScene) } namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotState) +MOVEIT_CLASS_FORWARD(LinkModel) } } namespace moveit { namespace task_constructor { @@ -28,12 +29,18 @@ void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr &scen void generateCollisionMarkers(const moveit::core::RobotState &robot_state, const MarkerCallback& callback, const std::vector &link_names = {}); +void generateCollisionMarkers(const moveit::core::RobotState &robot_state, + const MarkerCallback& callback, + const std::vector &link_models); /** generate marker msgs to visualize robot's visual geometry, calling the given callback for each of them * link_names: set of links to include (or all if empty) */ void generateVisualMarkers(const moveit::core::RobotState &robot_state, const MarkerCallback& callback, const std::vector &link_names = {}); +void generateVisualMarkers(const moveit::core::RobotState &robot_state, + const MarkerCallback& callback, + const std::vector &link_models); /** generate marker msgs to visualize the planning scene, calling the given callback for each of them * calls generateMarkersForRobot() and generateMarkersForObjects() */ diff --git a/core/src/marker_tools.cpp b/core/src/marker_tools.cpp index 0522fe98..df865434 100644 --- a/core/src/marker_tools.cpp +++ b/core/src/marker_tools.cpp @@ -70,6 +70,15 @@ template const std::string& materialName(const T& element); template <> const std::string& materialName(const urdf::Visual& element) { return element.material_name; } template <> const std::string& materialName(const urdf::Collision& element) { static std::string empty; return empty; } +std::vector linkNames(const std::vector& link_models) +{ + std::vector names; + names.reserve(link_models.size()); + for (const moveit::core::LinkModel* link : link_models) + names.push_back(link->getName()); + return names; +} + /** generate marker msgs to visualize the robot state, calling the given callback for each of them * link_names: set of links to include (or all if empty) */ template // with T = urdf::Visual or urdf::Collision @@ -101,9 +110,11 @@ void generateMarkers(const moveit::core::RobotState &robot_state, } }; + // either we have an array of collision/visual elements for(const auto& element : elements_vector(*link)) element_handler(element); + // or there is a single such element if (!valid_found) element_handler(element(*link)); } @@ -114,12 +125,22 @@ void generateCollisionMarkers(const moveit::core::RobotState &robot_state, const std::vector &link_names) { generateMarkers(robot_state, callback, link_names); } +void generateCollisionMarkers(const moveit::core::RobotState &robot_state, + const MarkerCallback &callback, + const std::vector &link_models) { + generateMarkers(robot_state, callback, linkNames(link_models)); +} void generateVisualMarkers(const moveit::core::RobotState &robot_state, const MarkerCallback& callback, const std::vector &link_names) { generateMarkers(robot_state, callback, link_names); } +void generateVisualMarkers(const moveit::core::RobotState &robot_state, + const MarkerCallback &callback, + const std::vector &link_models) { + generateMarkers(robot_state, callback, linkNames(link_models)); +} /** generate marker msgs to visualize the planning scene, calling the given callback for each of them * calls generateMarkersForRobot() and generateMarkersForObjects() */