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;
|
class MarkerBase;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
namespace planning_scene {
|
||||||
|
MOVEIT_CLASS_FORWARD(PlanningScene)
|
||||||
|
}
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(MarkerVisualization)
|
MOVEIT_CLASS_FORWARD(MarkerVisualization)
|
||||||
@ -26,6 +30,8 @@ MOVEIT_CLASS_FORWARD(MarkerVisualization)
|
|||||||
*
|
*
|
||||||
* Markers within a specific namespace are created as children of a
|
* Markers within a specific namespace are created as children of a
|
||||||
* corresponding scene node, which allows for fast toggling of visibility.
|
* 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
|
class MarkerVisualization
|
||||||
{
|
{
|
||||||
@ -36,7 +42,8 @@ class MarkerVisualization
|
|||||||
std::map<QString, Ogre::SceneNode*> namespaces_;
|
std::map<QString, Ogre::SceneNode*> namespaces_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers);
|
MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers,
|
||||||
|
const planning_scene::PlanningScene& end_scene);
|
||||||
~MarkerVisualization();
|
~MarkerVisualization();
|
||||||
|
|
||||||
void createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node);
|
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();
|
ref_scene = ref_scene->diff();
|
||||||
|
|
||||||
if (sub.markers.size())
|
if (sub.markers.size())
|
||||||
data_[i].markers_.reset(new MarkerVisualization(sub.markers));
|
data_[i].markers_.reset(new MarkerVisualization(sub.markers, *ref_scene));
|
||||||
else
|
else
|
||||||
data_[i].markers_.reset();
|
data_[i].markers_.reset();
|
||||||
++i;
|
++i;
|
||||||
|
|||||||
@ -1,4 +1,5 @@
|
|||||||
#include <moveit/visualization_tools/marker_visualization.h>
|
#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/marker_base.h>
|
||||||
#include "rviz/default_plugin/markers/arrow_marker.h"
|
#include "rviz/default_plugin/markers/arrow_marker.h"
|
||||||
@ -12,6 +13,8 @@
|
|||||||
|
|
||||||
#include <rviz/display_context.h>
|
#include <rviz/display_context.h>
|
||||||
#include <rviz/frame_manager.h>
|
#include <rviz/frame_manager.h>
|
||||||
|
#include <ros/console.h>
|
||||||
|
#include <eigen_conversions/eigen_msg.h>
|
||||||
#include <OgreSceneManager.h>
|
#include <OgreSceneManager.h>
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
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
|
// remember marker message, postpone rviz::MarkerBase creation until later
|
||||||
for (const auto& marker : markers) {
|
for (const auto& marker : markers) {
|
||||||
// create MarkerData with nil Marker pointer
|
// create MarkerData with nil Marker pointer
|
||||||
MarkerData data;
|
MarkerData data;
|
||||||
data.first.reset(new visualization_msgs::Marker(marker));
|
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));
|
markers_.push_back(std::move(data));
|
||||||
// remember namespace name
|
// remember namespace name
|
||||||
namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr));
|
namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr));
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user