allow arbitrary (planning scene) frames for markers

This commit is contained in:
Robert Haschke 2018-03-23 17:06:39 +01:00
parent bffe5f9a55
commit 936681f4c3
3 changed files with 40 additions and 3 deletions

View File

@ -18,6 +18,10 @@ class DisplayContext;
class MarkerBase;
}
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene)
}
namespace moveit_rviz_plugin {
MOVEIT_CLASS_FORWARD(MarkerVisualization)
@ -26,6 +30,8 @@ MOVEIT_CLASS_FORWARD(MarkerVisualization)
*
* Markers within a specific namespace are created as children of a
* corresponding scene node, which allows for fast toggling of visibility.
* Placement of markers always refers to the frames of a (fixed) planning scene
* and is transformed once w.r.t. its planning frame during construction.
*/
class MarkerVisualization
{
@ -36,7 +42,8 @@ class MarkerVisualization
std::map<QString, Ogre::SceneNode*> namespaces_;
public:
MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers);
MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers,
const planning_scene::PlanningScene& end_scene);
~MarkerVisualization();
void createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node);

View File

@ -121,7 +121,7 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta
ref_scene = ref_scene->diff();
if (sub.markers.size())
data_[i].markers_.reset(new MarkerVisualization(sub.markers));
data_[i].markers_.reset(new MarkerVisualization(sub.markers, *ref_scene));
else
data_[i].markers_.reset();
++i;

View File

@ -1,4 +1,5 @@
#include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz/default_plugin/markers/marker_base.h>
#include "rviz/default_plugin/markers/arrow_marker.h"
@ -12,6 +13,8 @@
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
#include <ros/console.h>
#include <eigen_conversions/eigen_msg.h>
#include <OgreSceneManager.h>
namespace moveit_rviz_plugin {
@ -53,13 +56,40 @@ rviz::MarkerBase* createMarker(int marker_type, rviz::DisplayContext* context, O
}
}
MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::Marker> &markers)
namespace {
// express marker pose relative to planning frame (of end scene)
bool toPlanningFrame(visualization_msgs::Marker &marker,
const planning_scene::PlanningScene &scene)
{
if (marker.header.frame_id == scene.getPlanningFrame())
return true;
if (!scene.knowsFrameTransform(marker.header.frame_id)) {
ROS_WARN_ONCE("unknown frame '%s' for solution marker in namespace '%s'",
marker.header.frame_id.c_str(), marker.ns.c_str());
return false;
}
Eigen::Affine3d pose;
tf::poseMsgToEigen(marker.pose, pose);
const Eigen::Affine3d tm = scene.getFrameTransform(marker.header.frame_id);
tf::poseEigenToMsg(tm * pose, marker.pose);
marker.header.frame_id = scene.getPlanningFrame();
return true;
}
}
MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::Marker> &markers,
const planning_scene::PlanningScene &end_scene)
{
// remember marker message, postpone rviz::MarkerBase creation until later
for (const auto& marker : markers) {
// create MarkerData with nil Marker pointer
MarkerData data;
data.first.reset(new visualization_msgs::Marker(marker));
// express marker pose relative to planning frame of end_scene
if (!toPlanningFrame(const_cast<visualization_msgs::Marker&>(*data.first), end_scene))
continue;
markers_.push_back(std::move(data));
// remember namespace name
namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr));