diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h index 96292670..3cccf5db 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h @@ -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 namespaces_; public: - MarkerVisualization(const std::vector& markers); + MarkerVisualization(const std::vector& markers, + const planning_scene::PlanningScene& end_scene); ~MarkerVisualization(); void createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node); diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index d0280884..4cfa4376 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -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; diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index 57df5b6d..fd26c848 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -1,4 +1,5 @@ #include +#include #include #include "rviz/default_plugin/markers/arrow_marker.h" @@ -12,6 +13,8 @@ #include #include +#include +#include #include namespace moveit_rviz_plugin { @@ -53,13 +56,40 @@ rviz::MarkerBase* createMarker(int marker_type, rviz::DisplayContext* context, O } } -MarkerVisualization::MarkerVisualization(const std::vector &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 &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(*data.first), end_scene)) + continue; markers_.push_back(std::move(data)); // remember namespace name namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr));