marker_tools

This commit is contained in:
Robert Haschke 2018-01-06 13:39:10 +01:00
parent 365fee9b60
commit 01d43e3f3b
5 changed files with 180 additions and 0 deletions

View File

@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
moveit_task_constructor_msgs moveit_task_constructor_msgs
roscpp roscpp
visualization_msgs visualization_msgs
rviz_marker_tools
) )
catkin_package( catkin_package(

View File

@ -0,0 +1,43 @@
#pragma once
#include <rviz_marker_tools/marker_creation.h>
#include <visualization_msgs/Marker.h>
#include <moveit/macros/class_forward.h>
#include <functional>
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<void(visualization_msgs::Marker&, const std::string&)> 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<std::string> &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<std::string> &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<std::string> &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);
} }

View File

@ -17,6 +17,7 @@
<build_depend>moveit_ros_planning_interface</build_depend> <build_depend>moveit_ros_planning_interface</build_depend>
<build_depend>moveit_task_constructor_msgs</build_depend> <build_depend>moveit_task_constructor_msgs</build_depend>
<build_depend>visualization_msgs</build_depend> <build_depend>visualization_msgs</build_depend>
<build_depend>rviz_marker_tools</build_depend>
<run_depend>eigen_conversions</run_depend> <run_depend>eigen_conversions</run_depend>
<run_depend>geometry_msgs</run_depend> <run_depend>geometry_msgs</run_depend>
@ -26,6 +27,7 @@
<run_depend>moveit_ros_planning_interface</run_depend> <run_depend>moveit_ros_planning_interface</run_depend>
<run_depend>moveit_task_constructor_msgs</run_depend> <run_depend>moveit_task_constructor_msgs</run_depend>
<run_depend>visualization_msgs</run_depend> <run_depend>visualization_msgs</run_depend>
<run_depend>rviz_marker_tools</run_depend>
<test_depend>rosunit</test_depend> <test_depend>rosunit</test_depend>
</package> </package>

View File

@ -2,6 +2,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/container.h ${PROJECT_INCLUDE}/container.h
${PROJECT_INCLUDE}/container_p.h ${PROJECT_INCLUDE}/container_p.h
${PROJECT_INCLUDE}/introspection.h ${PROJECT_INCLUDE}/introspection.h
${PROJECT_INCLUDE}/marker_tools.h
${PROJECT_INCLUDE}/properties.h ${PROJECT_INCLUDE}/properties.h
${PROJECT_INCLUDE}/stage.h ${PROJECT_INCLUDE}/stage.h
${PROJECT_INCLUDE}/stage_p.h ${PROJECT_INCLUDE}/stage_p.h
@ -11,6 +12,7 @@ add_library(${PROJECT_NAME}
container.cpp container.cpp
introspection.cpp introspection.cpp
marker_tools.cpp
properties.cpp properties.cpp
stage.cpp stage.cpp
storage.cpp storage.cpp

132
core/src/marker_tools.cpp Normal file
View File

@ -0,0 +1,132 @@
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/robot_state.h>
#include <eigen_conversions/eigen_msg.h>
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<std::string> &object_names)
{
scene->printKnownObjects(std::cout);
/*
const std::vector<std::string>* 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 <class T> const std::vector<boost::shared_ptr<T>>& elements_vector(const urdf::Link& link);
template <> const std::vector<urdf::CollisionSharedPtr>& elements_vector(const urdf::Link& link) { return link.collision_array; }
template <> const std::vector<urdf::VisualSharedPtr>& elements_vector(const urdf::Link& link) { return link.visual_array; }
template <class T> const boost::shared_ptr<T>& 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 <class T> 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 <class T> // with T = urdf::Visual or urdf::Collision
void generateMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<std::string> &link_names = {})
{
const std::vector<std::string>* 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<T>& 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<T>(*link))
element_handler(element);
if (!valid_found)
element_handler(element<T>(*link));
}
}
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<std::string> &link_names) {
generateMarkers<urdf::Collision>(robot_state, callback, link_names);
}
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<std::string> &link_names) {
generateMarkers<urdf::Visual>(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<urdf::Visual>(scene->getCurrentState(), callback);
generateMarkersForObjects(scene, callback);
}
} }