Merge pull request #37 from ubi-agni/relative-markers

online update of relative markers
This commit is contained in:
Robert Haschke 2018-03-26 00:20:14 +02:00 committed by GitHub
commit b15c5adbd7
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 251 additions and 96 deletions

View File

@ -88,8 +88,6 @@ TaskDisplay::TaskDisplay() : Display()
connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)),
task_list_model_.get(), SLOT(highlightStage(size_t)));
marker_visual_ = new MarkerVisualizationProperty("Markers", this);
tasks_property_ =
new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this);
}
@ -103,7 +101,6 @@ void TaskDisplay::onInitialize()
{
Display::onInitialize();
trajectory_visual_->onInitialize(scene_node_, context_);
marker_visual_->onInitialize(scene_node_, context_);
#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
// displays are loaded before panels, hence wait a little bit for the panel to be loaded
@ -246,16 +243,6 @@ void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionCon
});
}
void TaskDisplay::clearMarkers() {
marker_visual_->clearMarkers();
}
void TaskDisplay::addMarkers(const DisplaySolutionPtr &s) {
if (!s) return;
for (size_t i=0, end = s->numSubSolutions(); i != end; ++i) {
marker_visual_->addMarkers(s->markersOfSubTrajectory(i));
}
}
void TaskDisplay::changedTaskSolutionTopic()
{

View File

@ -39,6 +39,7 @@
#pragma once
#include <rviz/display.h>
#include <moveit/visualization_tools/task_solution_visualization.h>
#ifndef Q_MOC_RUN
#include "job_queue.h"
@ -63,8 +64,6 @@ namespace moveit_rviz_plugin
{
MOVEIT_CLASS_FORWARD(DisplaySolution)
class TaskSolutionVisualization;
class MarkerVisualizationProperty;
class TaskListModel;
class TaskDisplay : public rviz::Display
@ -87,8 +86,9 @@ public:
TaskListModel& getTaskListModel() { return *task_list_model_; }
TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); }
void clearMarkers();
void addMarkers(const DisplaySolutionPtr &s);
inline void clearMarkers() { trajectory_visual_->clearMarkers(); }
inline void addMarkers(const DisplaySolutionPtr &s) { trajectory_visual_->addMarkers(s); }
protected:
void onInitialize() override;
@ -132,7 +132,6 @@ protected:
// Properties
rviz::StringProperty* robot_description_property_;
rviz::RosTopicProperty* task_solution_topic_property_;
MarkerVisualizationProperty* marker_visual_;
rviz::Property* tasks_property_;
};

View File

@ -21,6 +21,9 @@ class MarkerBase;
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene)
}
namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotState)
} }
namespace moveit_rviz_plugin {
@ -36,20 +39,48 @@ MOVEIT_CLASS_FORWARD(MarkerVisualization)
class MarkerVisualization
{
// list of all markers, attached to scene nodes in namespaces_
typedef std::pair<visualization_msgs::MarkerConstPtr, std::shared_ptr<rviz::MarkerBase>> MarkerData;
struct MarkerData {
visualization_msgs::MarkerPtr msg_;
std::shared_ptr<rviz::MarkerBase> marker_;
MarkerData(const visualization_msgs::Marker& marker);
};
struct NamespaceData {
Ogre::SceneNode* ns_node_ = nullptr;
// markers grouped by frame
std::map<std::string, Ogre::SceneNode*> frames_;
};
// list of all markers
std::deque<MarkerData> markers_;
// markers grouped by their namespace
std::map<QString, Ogre::SceneNode*> namespaces_;
std::map<std::string, NamespaceData> namespaces_;
// planning_frame_ of scene
std::string planning_frame_;
// flag indicating that markers were created
bool markers_created_ = false;
public:
MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers,
const planning_scene::PlanningScene& end_scene);
~MarkerVisualization();
void createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node);
const std::map<QString, Ogre::SceneNode*>& namespaces() const { return namespaces_; }
/// did we successfully created all markers (and scene nodes)?
bool created() const { return markers_created_; }
/// create markers (placed at planning frame of scene)
bool createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node);
/// update marker position/orientation based on frames of given scene + robot_state
void update(const planning_scene::PlanningScene &end_scene,
const moveit::core::RobotState &robot_state);
const std::map<std::string, NamespaceData>& namespaces() const { return namespaces_; }
void setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible);
private:
void update(MarkerData &data,
const planning_scene::PlanningScene &end_scene,
const moveit::core::RobotState &robot_state) const;
};
@ -67,6 +98,7 @@ class MarkerVisualizationProperty: public rviz::BoolProperty
Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to
std::map<QString, rviz::BoolProperty*> namespaces_; // rviz properties for encountered namespaces
std::list<MarkerVisualizationPtr> hosted_markers_; // list of hosted MarkerVisualization instances
rviz::BoolProperty* all_markers_at_once_;
public:
MarkerVisualizationProperty(const QString& name, Property* parent = nullptr);
@ -76,12 +108,21 @@ public:
/// remove all hosted markers from display
void clearMarkers();
/// add all markers in MarkerVisualization for display
/// add markers in MarkerVisualization for display
void addMarkers(MarkerVisualizationPtr markers);
/// update pose of all markers
void update(const planning_scene::PlanningScene &scene,
const moveit::core::RobotState &robot_state);
bool allAtOnce() const;
public Q_SLOTS:
void onEnableChanged();
void onNSEnableChanged();
void onAllAtOnceChanged();
Q_SIGNALS:
void allAtOnceChanged(bool);
};
} // namespace moveit_rviz_plugin

View File

@ -80,6 +80,7 @@ MOVEIT_CLASS_FORWARD(PlanningSceneRender)
MOVEIT_CLASS_FORWARD(DisplaySolution)
class TaskSolutionPanel;
class MarkerVisualizationProperty;
class TaskSolutionVisualization : public QObject
{
Q_OBJECT
@ -105,13 +106,18 @@ public:
planning_scene::PlanningSceneConstPtr getScene() const { return scene_; }
void showTrajectory(const moveit_task_constructor_msgs::Solution& msg);
void showTrajectory(moveit_rviz_plugin::DisplaySolutionPtr s, bool lock);
void showTrajectory(const moveit_rviz_plugin::DisplaySolutionPtr& s, bool lock);
void unlock();
void clearMarkers();
void addMarkers(const moveit_rviz_plugin::DisplaySolutionPtr &s);
public Q_SLOTS:
void interruptCurrentDisplay();
private Q_SLOTS:
void onAllAtOnceChanged(bool);
// trajectory property slots
void changedRobotVisualEnabled();
void changedRobotCollisionEnabled();
@ -142,6 +148,8 @@ protected:
PlanningSceneRenderPtr scene_render_;
// render the robot
RobotStateVisualizationPtr robot_render_;
// render markers
MarkerVisualizationProperty* marker_visual_;
// Handle colouring of robot
void setRobotColor(rviz::Robot* robot, const QColor& color);

View File

@ -13,6 +13,7 @@
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
#include <tf2_msgs/TF2Error.h>
#include <ros/console.h>
#include <eigen_conversions/eigen_msg.h>
#include <OgreSceneManager.h>
@ -56,51 +57,38 @@ rviz::MarkerBase* createMarker(int marker_type, rviz::DisplayContext* context, O
}
}
namespace {
// express marker pose relative to planning frame (of end scene)
bool toPlanningFrame(visualization_msgs::Marker &marker,
const planning_scene::PlanningScene &scene)
// create MarkerData with nil marker_ pointer, just with a copy of message
MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::Marker& marker)
{
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;
}
msg_.reset(new visualization_msgs::Marker(marker));
msg_->header.stamp = ros::Time();
msg_->frame_locked = false;
}
MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::Marker> &markers,
const planning_scene::PlanningScene &end_scene)
{
planning_frame_ = end_scene.getPlanningFrame();
// 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));
if (!end_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());
continue; // ignore markers with unknown frame
}
// remember marker message
markers_.emplace_back(marker);
// remember namespace name
namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr));
namespaces_.insert(std::make_pair(marker.ns, NamespaceData()));
}
}
MarkerVisualization::~MarkerVisualization()
{
for (const auto& pair : namespaces_) {
if (pair.second)
pair.second->getCreator()->destroySceneNode(pair.second);
if (pair.second.ns_node_)
pair.second.ns_node_->getCreator()->destroySceneNode(pair.second.ns_node_);
}
}
@ -114,56 +102,124 @@ void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible)
void MarkerVisualization::setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible)
{
auto it = namespaces_.find(ns);
auto it = namespaces_.find(ns.toStdString());
if (it == namespaces_.end())
return;
setVisibility(it->second, parent_scene_node, visible);
setVisibility(it->second.ns_node_, parent_scene_node, visible);
}
void MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::SceneNode *parent_scene_node)
bool MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::SceneNode *parent_scene_node)
{
std::string planning_frame;
if (markers_created_)
return true; // already called before
// fetch transform from planning_frame_ to rviz' fixed frame
const std::string& fixed_frame = context->getFrameManager()->getFixedFrame();
Ogre::Quaternion quat;
Ogre::Vector3 pos;
for (MarkerData& data : markers_) {
if (data.second) continue;
auto ns_it = namespaces_.find(QString::fromStdString(data.first->ns));
Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before!
if (ns_it->second == nullptr) // create scene node for this namespace
ns_it->second = parent_scene_node->getCreator()->createSceneNode();
data.second.reset(createMarker(data.first->type, context, ns_it->second));
if (data.second) {
// rviz::MarkerBase::setMessage() initializes the marker
data.second->setMessage(data.first);
// ... and sets its position + orientation relative to rviz' fixed frame
// however, we want the marker to be placed w.r.t. planning frame = msg.header.frame_id
Q_ASSERT(!data.first->header.frame_id.empty());
if (planning_frame.empty()) { // determine transform once
planning_frame = data.first->header.frame_id;
// transform from fixed frame to planning_frame
tf::TransformListener* tf = context->getFrameManager()->getTFClient();
tf::StampedTransform tm;
tf->lookupTransform(context->getFrameManager()->getFixedFrame(), planning_frame, ros::Time(), tm);
auto q = tm.getRotation();
auto p = tm.getOrigin();
quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z());
pos = Ogre::Vector3(p.x(), p.y(), p.z());
} else {
Q_ASSERT(data.first->header.frame_id == planning_frame);
}
data.second->setOrientation(quat * data.second->getOrientation());
data.second->setPosition(quat * (data.second->getPosition() - pos));
try {
tf::TransformListener* tf = context->getFrameManager()->getTFClient();
std::string error_msg;
if (!tf->canTransform(planning_frame_, fixed_frame, ros::Time(), &error_msg)) {
ROS_WARN_STREAM_NAMED("MarkerVisualization", error_msg);
return false; // frame transform not (yet) available
}
tf::StampedTransform tm;
tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm);
auto q = tm.getRotation();
auto p = tm.getOrigin();
quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z());
pos = Ogre::Vector3(p.x(), p.y(), p.z());
} catch (const tf2::TransformException& e) {
ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what());
return false;
}
for (MarkerData& data : markers_) {
if (data.marker_) continue;
auto ns_it = namespaces_.find(data.msg_->ns);
Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before!
if (ns_it->second.ns_node_ == nullptr) // create scene node for this namespace
ns_it->second.ns_node_ = parent_scene_node->getCreator()->createSceneNode();
Ogre::SceneNode* node = ns_it->second.ns_node_;
// create a scene node for all markers with given frame name
auto frame_it = ns_it->second.frames_.insert(std::make_pair(data.msg_->header.frame_id, nullptr)).first;
if (frame_it->second == nullptr)
frame_it->second = node->createChildSceneNode();
node = frame_it->second;
data.marker_.reset(createMarker(data.msg_->type, context, node));
if (!data.marker_) continue; // failed to create marker
// setMessage() initializes the marker, placing it at the message-specified frame
// w.r.t. rviz' current fixed frame. However, we want to place the marker w.r.t.
// the planning frame of the planning scene!
// Hence, temporarily modify the message-specified frame to planning_frame_
const std::string msg_frame = data.msg_->header.frame_id;
data.msg_->header.frame_id = planning_frame_;
data.marker_->setMessage(data.msg_);
data.msg_->header.frame_id = msg_frame;
// ... and subsequently revert any transform between rviz' fixed frame and planning_frame_
data.marker_->setOrientation(quat * data.marker_->getOrientation());
data.marker_->setPosition(quat * data.marker_->getPosition() + pos);
}
markers_created_ = true;
return true;
}
void MarkerVisualization::update(MarkerData& data,
const planning_scene::PlanningScene &scene,
const moveit::core::RobotState &robot_state) const
{
Q_ASSERT(scene.getPlanningFrame() == planning_frame_);
const visualization_msgs::Marker& marker = *data.msg_;
if (marker.header.frame_id == scene.getPlanningFrame())
return; // no need to transform nodes placed at planning frame
// fetch base pose from robot_state / scene
Eigen::Affine3d pose;
if (robot_state.knowsFrameTransform(marker.header.frame_id))
pose = robot_state.getFrameTransform(marker.header.frame_id);
else if (scene.knowsFrameTransform(marker.header.frame_id))
pose = scene.getFrameTransform(marker.header.frame_id);
else {
ROS_WARN_ONCE_NAMED("MarkerVisualization",
"unknown frame '%s' for solution marker in namespace '%s'",
marker.header.frame_id.c_str(), marker.ns.c_str());
return; // ignore markers with unknown frame
}
auto ns_it = namespaces_.find(marker.ns);
Q_ASSERT(ns_it != namespaces_.end()); // we have added all namespaces before
auto frame_it = ns_it->second.frames_.find(marker.header.frame_id);
Q_ASSERT(frame_it != ns_it->second.frames_.end()); // we have created all of them
const Eigen::Quaterniond q = (Eigen::Quaterniond)pose.linear();
const Eigen::Vector3d& p = pose.translation();
frame_it->second->setOrientation(Ogre::Quaternion(q.w(), q.x(), q.y(), q.z()));
frame_it->second->setPosition(Ogre::Vector3(p.x(), p.y(), p.z()));
}
void MarkerVisualization::update(const planning_scene::PlanningScene &end_scene,
const moveit::core::RobotState &robot_state)
{
for (MarkerData& data : markers_)
update(data, end_scene, robot_state);
}
MarkerVisualizationProperty::MarkerVisualizationProperty(const QString &name, rviz::Property *parent)
: rviz::BoolProperty(name, true, "Enable/disable markers", parent)
{
all_markers_at_once_ = new rviz::BoolProperty("All at once?", false, "Show all markers of multiple subsolutions at once?",
this, SLOT(onAllAtOnceChanged()), this);
connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged()));
}
@ -194,24 +250,42 @@ void MarkerVisualizationProperty::addMarkers(MarkerVisualizationPtr markers)
// remember that those markers are hosted
hosted_markers_.push_back(markers);
// create markers if not yet done
if (!markers->created() && !markers->createMarkers(context_, marker_scene_node_))
return; // if markers not created, nothing to do here
// attach all scene nodes from markers
for (const auto& pair : markers->namespaces()) {
QString ns = QString::fromStdString(pair.first);
// create sub property for newly encountered namespace, enabling visibility by default
auto ns_it = namespaces_.insert(std::make_pair(pair.first, nullptr)).first;
auto ns_it = namespaces_.insert(std::make_pair(ns, nullptr)).first;
if (ns_it->second == nullptr) {
ns_it->second = new rviz::BoolProperty(pair.first, true, "Show/hide markers of this namespace", this,
ns_it->second = new rviz::BoolProperty(ns, true, "Show/hide markers of this namespace", this,
SLOT(onNSEnableChanged()), this);
}
if (!pair.second) // invalid scene node indicates that we still need to create the rviz markers
markers->createMarkers(context_, marker_scene_node_);
Q_ASSERT(pair.second);
Q_ASSERT(pair.second.ns_node_); // nodes should have been created in createMarkers()
if (ns_it->second->getBool())
marker_scene_node_->addChild(pair.second);
marker_scene_node_->addChild(pair.second.ns_node_);
}
}
void MarkerVisualizationProperty::update(const planning_scene::PlanningScene &scene,
const moveit::core::RobotState &robot_state)
{
for (const auto& markers : hosted_markers_) {
if (!markers->created())
if (!markers->createMarkers(context_, marker_scene_node_))
continue;
markers->update(scene, robot_state);
}
}
bool MarkerVisualizationProperty::allAtOnce() const
{
return all_markers_at_once_->getBool();
}
void MarkerVisualizationProperty::onEnableChanged()
{
setVisibility(marker_scene_node_, parent_scene_node_, getBool());
@ -227,4 +301,9 @@ void MarkerVisualizationProperty::onNSEnableChanged()
markers->setVisible(ns, marker_scene_node_, visible);
}
void MarkerVisualizationProperty::onAllAtOnceChanged()
{
Q_EMIT allAtOnceChanged(all_markers_at_once_->getBool());
}
} // namespace moveit_rviz_plugin

View File

@ -36,6 +36,7 @@
#include <moveit/visualization_tools/display_solution.h>
#include <moveit/visualization_tools/task_solution_visualization.h>
#include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/visualization_tools/task_solution_panel.h>
#include <moveit/rviz_plugin_render_tools/planning_scene_render.h>
@ -152,6 +153,9 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi
octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR);
octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR);
marker_visual_ = new MarkerVisualizationProperty("Markers", parent);
connect(marker_visual_, SIGNAL(allAtOnceChanged(bool)), this, SLOT(onAllAtOnceChanged(bool)));
}
TaskSolutionVisualization::~TaskSolutionVisualization()
@ -165,6 +169,8 @@ TaskSolutionVisualization::~TaskSolutionVisualization()
if (slider_dock_panel_)
delete slider_dock_panel_;
delete marker_visual_;
if (main_scene_node_)
main_scene_node_->getCreator()->destroySceneNode(main_scene_node_);
}
@ -188,6 +194,8 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::
scene_render_.reset(new PlanningSceneRender(main_scene_node_, context_, RobotStateVisualizationPtr()));
scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
marker_visual_->onInitialize(main_scene_node_, context_);
rviz::WindowManagerInterface* window_context = context_->getWindowManager();
if (window_context)
{
@ -330,6 +338,18 @@ void TaskSolutionVisualization::interruptCurrentDisplay()
drop_displaying_solution_ = true;
}
void TaskSolutionVisualization::onAllAtOnceChanged(bool all_at_once)
{
if (!displaying_solution_)
return;
clearMarkers();
if (all_at_once)
addMarkers(displaying_solution_);
else if (current_state_ >= 0)
renderWayPoint(current_state_, -1);
}
float TaskSolutionVisualization::getStateDisplayTime()
{
std::string tm = state_display_time_property_->getStdString();
@ -458,6 +478,11 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
displaying_solution_->indexPair(previous_index).first != idx_pair.first) {
// switch to new stage: show new planning scene
renderPlanningScene (scene);
// switch to markers of next sub trajectory?
if (!marker_visual_->allAtOnce()) {
marker_visual_->clearMarkers();
marker_visual_->addMarkers(displaying_solution_->markersOfSubTrajectory(idx_pair.first));
}
Q_EMIT activeStageChanged(displaying_solution_->creatorId(idx_pair));
}
robot_state = displaying_solution_->getWayPointPtr(idx_pair);
@ -473,6 +498,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
planning_scene::ObjectColorMap color_map;
scene->getKnownObjectColors(color_map);
robot_render_->update(robot_state, color, color_map);
marker_visual_->update(*scene, *robot_state);
if (slider_panel_ && index >= 0)
slider_panel_->setSliderPosition(index);
@ -502,7 +528,7 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msg
showTrajectory(s, false);
}
void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s, bool lock_display)
void TaskSolutionVisualization::showTrajectory(const DisplaySolutionPtr& s, bool lock_display)
{
if (lock_display || !s->empty()) {
boost::mutex::scoped_lock lock(display_solution_mutex_);
@ -516,7 +542,22 @@ void TaskSolutionVisualization::showTrajectory(DisplaySolutionPtr s, bool lock_d
void TaskSolutionVisualization::unlock()
{
locked_ = false;
locked_ = false;
}
void TaskSolutionVisualization::clearMarkers()
{
marker_visual_->clearMarkers();
}
void TaskSolutionVisualization::addMarkers(const moveit_rviz_plugin::DisplaySolutionPtr &s)
{
if (!s || (!marker_visual_->allAtOnce() && s->numSubSolutions() > 1))
return;
for (size_t i = 0, end = s->numSubSolutions(); i != end; ++i) {
marker_visual_->addMarkers(s->markersOfSubTrajectory(i));
}
}
void TaskSolutionVisualization::changedRobotColor()