mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
allow arbitrary (planning scene) frames for markers
This commit is contained in:
parent
bffe5f9a55
commit
936681f4c3
@ -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);
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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));
|
||||
|
||||
Loading…
Reference in New Issue
Block a user