mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
marker_tools
This commit is contained in:
parent
365fee9b60
commit
01d43e3f3b
@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
moveit_task_constructor_msgs
|
||||
roscpp
|
||||
visualization_msgs
|
||||
rviz_marker_tools
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
|
||||
43
core/include/moveit/task_constructor/marker_tools.h
Normal file
43
core/include/moveit/task_constructor/marker_tools.h
Normal 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);
|
||||
|
||||
} }
|
||||
@ -17,6 +17,7 @@
|
||||
<build_depend>moveit_ros_planning_interface</build_depend>
|
||||
<build_depend>moveit_task_constructor_msgs</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
<build_depend>rviz_marker_tools</build_depend>
|
||||
|
||||
<run_depend>eigen_conversions</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
@ -26,6 +27,7 @@
|
||||
<run_depend>moveit_ros_planning_interface</run_depend>
|
||||
<run_depend>moveit_task_constructor_msgs</run_depend>
|
||||
<run_depend>visualization_msgs</run_depend>
|
||||
<run_depend>rviz_marker_tools</run_depend>
|
||||
|
||||
<test_depend>rosunit</test_depend>
|
||||
</package>
|
||||
|
||||
@ -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
|
||||
|
||||
132
core/src/marker_tools.cpp
Normal file
132
core/src/marker_tools.cpp
Normal 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);
|
||||
}
|
||||
|
||||
} }
|
||||
Loading…
Reference in New Issue
Block a user