diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 09b544a4..94a3c1cb 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS moveit_task_constructor_msgs roscpp visualization_msgs + rviz_marker_tools ) catkin_package( diff --git a/core/include/moveit/task_constructor/marker_tools.h b/core/include/moveit/task_constructor/marker_tools.h new file mode 100644 index 00000000..05f88803 --- /dev/null +++ b/core/include/moveit/task_constructor/marker_tools.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include +#include + +namespace planning_scene { +MOVEIT_CLASS_FORWARD(PlanningScene) +} +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(RobotState) +} } + +namespace moveit { namespace task_constructor { + +/** signature of callback function, passing the generated marker and the name of the robot link / scene object */ +typedef std::function MarkerCallback; + +/** generate marker msgs to visualize the planning scene, calling the given callback for each of them + * object_names: set of links to include (or all if empty) */ +void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr &scene, + const MarkerCallback& callback, + const std::vector &object_names = {}); + +/** generate marker msgs to visualize robot's collision geometry, calling the given callback for each of them + * link_names: set of links to include (or all if empty) */ +void generateCollisionMarkers(const moveit::core::RobotState &robot_state, + const MarkerCallback& callback, + const std::vector &link_names = {}); + +/** 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 = {}); + +/** generate marker msgs to visualize the planning scene, calling the given callback for each of them + * calls generateMarkersForRobot() and generateMarkersForObjects() */ +void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr &scene, + const MarkerCallback& callback); + +} } diff --git a/core/package.xml b/core/package.xml index 97fa1db4..afad5e46 100644 --- a/core/package.xml +++ b/core/package.xml @@ -17,6 +17,7 @@ moveit_ros_planning_interface moveit_task_constructor_msgs visualization_msgs + rviz_marker_tools eigen_conversions geometry_msgs @@ -26,6 +27,7 @@ moveit_ros_planning_interface moveit_task_constructor_msgs visualization_msgs + rviz_marker_tools rosunit diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index a6fd4b7e..86a461ab 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -2,6 +2,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/container.h ${PROJECT_INCLUDE}/container_p.h ${PROJECT_INCLUDE}/introspection.h + ${PROJECT_INCLUDE}/marker_tools.h ${PROJECT_INCLUDE}/properties.h ${PROJECT_INCLUDE}/stage.h ${PROJECT_INCLUDE}/stage_p.h @@ -11,6 +12,7 @@ add_library(${PROJECT_NAME} container.cpp introspection.cpp + marker_tools.cpp properties.cpp stage.cpp storage.cpp diff --git a/core/src/marker_tools.cpp b/core/src/marker_tools.cpp new file mode 100644 index 00000000..0522fe98 --- /dev/null +++ b/core/src/marker_tools.cpp @@ -0,0 +1,132 @@ +#include +#include +#include +#include + +namespace vm = visualization_msgs; + +namespace moveit { namespace task_constructor { + +/** generate marker msgs to visualize the planning scene, calling the given callback for each of them + * object_names: set of links to include (or all if empty) */ +void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr &scene, + const MarkerCallback& callback, + const std::vector &object_names) +{ + scene->printKnownObjects(std::cout); +/* + const std::vector* names = object_names.empty() ? &scene->getCollisionObjectMsg() + : &link_names; + for (const auto &name : *names) { + visualization_msgs::MarkerArray markers; + robot_state.getRobotMarkers(markers, {name}, false); + for (auto &marker : markers.markers) + callback(marker, name); + } +*/ +} + +visualization_msgs::Marker& createGeometryMarker(visualization_msgs::Marker& marker, const urdf::Geometry& geom, const urdf::Pose& pose, + const urdf::Color& color) { + rviz_marker_tools::makeFromGeometry(marker, geom); + marker.pose.position.x = pose.position.x; + marker.pose.position.y = pose.position.y; + marker.pose.position.z = pose.position.z; + marker.pose.orientation.w = pose.rotation.w; + marker.pose.orientation.x = pose.rotation.x; + marker.pose.orientation.y = pose.rotation.y; + marker.pose.orientation.z = pose.rotation.z; + marker.color.r = color.r; + marker.color.g = color.g; + marker.color.b = color.b; + marker.color.a = color.a; + return marker; +} + +const urdf::Color& materialColor(const urdf::ModelInterface& model, const std::string& material_name) { + static urdf::Color default_color; + if (default_color.r == 0.0f) { + default_color.r = 0.8f; + default_color.g = 0.0f; + default_color.b = 0.0f; + default_color.a = 1.0f; + }; + urdf::MaterialSharedPtr material; + if (!material_name.empty()) + material = model.getMaterial(material_name); + return material ? material->color : default_color; +} + +// type traits to access collision/visual array or single element +template const std::vector>& elements_vector(const urdf::Link& link); +template <> const std::vector& elements_vector(const urdf::Link& link) { return link.collision_array; } +template <> const std::vector& elements_vector(const urdf::Link& link) { return link.visual_array; } + +template const boost::shared_ptr& element(const urdf::Link& link); +template <> const urdf::CollisionSharedPtr& element(const urdf::Link& link) { return link.collision; } +template <> const urdf::VisualSharedPtr& element(const urdf::Link& link) { return link.visual; } + +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; } + +/** 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 +void generateMarkers(const moveit::core::RobotState &robot_state, + const MarkerCallback& callback, + const std::vector &link_names = {}) +{ + const std::vector* names = link_names.empty() ? &robot_state.getRobotModel()->getLinkModelNames() + : &link_names; + const urdf::ModelInterfaceSharedPtr& model = robot_state.getRobotModel()->getURDF(); + if (!model) return; + + visualization_msgs::Marker m; + m.header.frame_id = model->getRoot()->name; + + // code adapted from rviz::RobotLink::createVisual() / createCollision() + for (const auto &name : *names) { + const urdf::LinkConstSharedPtr& link = model->getLink(name); + if (!link) return; + + bool valid_found = false; + auto element_handler = [&](const boost::shared_ptr& element){ + if (element && element->geometry) { + createGeometryMarker(m, *element->geometry, element->origin, + materialColor(*model, materialName(*element))); + m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose); + callback(m, name); + valid_found = true; + } + }; + + for(const auto& element : elements_vector(*link)) + element_handler(element); + + if (!valid_found) + element_handler(element(*link)); + } +} + +void generateCollisionMarkers(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_names) { + generateMarkers(robot_state, callback, link_names); +} + +/** generate marker msgs to visualize the planning scene, calling the given callback for each of them + * calls generateMarkersForRobot() and generateMarkersForObjects() */ +void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr &scene, + const MarkerCallback &callback) { + generateMarkers(scene->getCurrentState(), callback); + generateMarkersForObjects(scene, callback); +} + +} }