marker visualization

This commit is contained in:
Robert Haschke 2017-11-29 11:21:24 +01:00
parent f7ae7756cc
commit 30f6ade636
9 changed files with 299 additions and 2 deletions

View File

@ -41,6 +41,8 @@
#include "meta_task_list_model.h"
#include <moveit/task_constructor/introspection.h>
#include <moveit/visualization_tools/task_solution_visualization.h>
#include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/visualization_tools/display_solution.h>
#include <moveit_task_constructor_msgs/GetSolution.h>
#include <moveit/rdf_loader/rdf_loader.h>
@ -77,6 +79,8 @@ TaskDisplay::TaskDisplay() : Display()
trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
marker_visual_ = new MarkerVisualizationProperty("Markers", this);
tasks_property_ =
new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this);
}
@ -89,6 +93,7 @@ void TaskDisplay::onInitialize()
{
Display::onInitialize();
trajectory_visual_->onInitialize(scene_node_, context_);
marker_visual_->onInitialize(scene_node_, context_);
}
void TaskDisplay::loadRobotModel()
@ -179,7 +184,7 @@ void TaskDisplay::taskSolutionCB(const ros::MessageEvent<const moveit_task_const
const std::string id = event.getPublisherName() + "/" + msg->task_id;
mainloop_jobs_.addJob([this, id, msg]() {
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
trajectory_visual_->showTrajectory(s);
if (s) trajectory_visual_->showTrajectory(s);
return;
});
}
@ -189,6 +194,17 @@ void TaskDisplay::showTrajectory(const DisplaySolutionPtr &s) const {
trajectory_visual_->showTrajectory(s);
}
void TaskDisplay::clearMarkers() {
marker_visual_->clearMarkers();
}
void TaskDisplay::showMarkers(const DisplaySolutionPtr &s) {
if (!s) return;
for (size_t i=0, end = s->numSubSolutions(); i != end; ++i) {
marker_visual_->showMarkers(s->markersOfSubTrajectory(i));
}
}
void TaskDisplay::changedTaskSolutionTopic()
{
task_description_sub.shutdown();

View File

@ -64,6 +64,7 @@ namespace moveit_rviz_plugin
MOVEIT_CLASS_FORWARD(DisplaySolution)
class TaskSolutionVisualization;
class MarkerVisualizationProperty;
class TaskListModel;
class TaskDisplay : public rviz::Display
@ -87,6 +88,8 @@ public:
TaskListModel& getTaskListModel() { return *task_list_model_; }
void showTrajectory(const DisplaySolutionPtr& s) const;
void clearMarkers();
void showMarkers(const DisplaySolutionPtr &s);
private Q_SLOTS:
/**
@ -123,6 +126,7 @@ protected:
// Properties
rviz::StringProperty* robot_description_property_;
rviz::RosTopicProperty* task_solution_topic_property_;
MarkerVisualizationProperty* marker_visual_;
rviz::Property* tasks_property_;
};

View File

@ -219,6 +219,8 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex &current, const QMode
return;
display->showTrajectory(solution);
display->clearMarkers();
display->showMarkers(solution);
}
}

View File

@ -4,6 +4,7 @@ set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_too
set(HEADERS
${PROJECT_INCLUDE}/display_solution.h
${PROJECT_INCLUDE}/marker_visualization.h
${PROJECT_INCLUDE}/task_solution_panel.h
${PROJECT_INCLUDE}/task_solution_visualization.h
)
@ -12,6 +13,7 @@ add_library(${MOVEIT_LIB_NAME}
${HEADERS}
src/display_solution.cpp
src/marker_visualization.cpp
src/task_solution_panel.cpp
src/task_solution_visualization.cpp
)
@ -19,6 +21,7 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V
target_link_libraries(${MOVEIT_LIB_NAME}
${catkin_LIBRARIES}
${rviz_DEFAULT_PLUGIN_LIBRARIES}
${OGRE_LIBRARIES}
${QT_LIBRARIES}
${Boost_LIBRARIES}

View File

@ -48,10 +48,18 @@ MOVEIT_CLASS_FORWARD(PlanningScene)
namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory)
}
namespace Ogre
{
class SceneNode;
}
namespace rviz {
class DisplayContext;
}
namespace moveit_rviz_plugin {
MOVEIT_CLASS_FORWARD(DisplaySolution)
MOVEIT_CLASS_FORWARD(MarkerVisualization)
/** Class representing a task solution for display */
class DisplaySolution
@ -68,6 +76,8 @@ class DisplaySolution
robot_trajectory::RobotTrajectoryPtr trajectory_;
/// optional name of the trajectory
std::string name_;
/// rviz markers
MarkerVisualizationPtr markers_;
};
std::vector<Data> data_;
@ -76,6 +86,8 @@ public:
/// create DisplaySolution for given sub trajectory of master
DisplaySolution(const DisplaySolution& master, uint32_t sub);
size_t numSubSolutions() const { return data_.size(); }
size_t getWayPointCount() const { return steps_; }
bool empty() const { return steps_ == 0; }
@ -101,6 +113,13 @@ public:
const std::string& name(size_t index) const {
return name(indexPair(index));
}
const MarkerVisualizationPtr markers(const IndexPair& idx_pair) const;
const MarkerVisualizationPtr markers(size_t index) const {
return markers(indexPair(index));
}
const MarkerVisualizationPtr markersOfSubTrajectory(size_t index) const {
return data_.at(index).markers_;
}
void setFromMessage(const planning_scene::PlanningScenePtr &start_scene,
const moveit_task_constructor_msgs::Solution& msg);

View File

@ -0,0 +1,68 @@
#pragma once
#include <rviz/properties/bool_property.h>
#include <moveit/macros/class_forward.h>
#include <visualization_msgs/Marker.h>
#include <deque>
#include <list>
#include <memory>
namespace Ogre
{
class SceneNode;
}
namespace rviz
{
class DisplayContext;
class MarkerBase;
}
namespace moveit_rviz_plugin {
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;
std::deque<MarkerData> markers_;
// markers grouped by their namespace
std::map<QString, Ogre::SceneNode*> namespaces_;
public:
MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers);
~MarkerVisualization();
void createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node);
const std::map<QString, Ogre::SceneNode*>& namespaces() const { return namespaces_; }
void setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible);
};
class MarkerVisualizationProperty: public rviz::BoolProperty
{
Q_OBJECT
rviz::DisplayContext* context_ = nullptr;
Ogre::SceneNode* parent_scene_node_ = nullptr; // scene node provided externally
Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to
std::map<QString, rviz::BoolProperty*> namespaces_;
std::list<MarkerVisualizationPtr> visible_markers_;
public:
MarkerVisualizationProperty(const QString& name, Property* parent = nullptr);
~MarkerVisualizationProperty();
void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context);
void clearMarkers();
void showMarkers(MarkerVisualizationPtr markers);
public Q_SLOTS:
void onEnableChanged();
void onNSEnableChanged();
};
} // namespace moveit_rviz_plugin

View File

@ -35,6 +35,7 @@
/* Author: Robert Haschke */
#include <moveit/visualization_tools/display_solution.h>
#include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <ros/console.h>
@ -83,6 +84,11 @@ const std::string &DisplaySolution::name(const IndexPair &idx_pair) const
return data_[idx_pair.first].name_;
}
const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::IndexPair &idx_pair) const
{
return data_[idx_pair.first].markers_;
}
void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene,
const moveit_task_constructor_msgs::Solution &msg)
{
@ -113,6 +119,11 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta
// create new reference scene for next iteration
ref_scene = ref_scene->diff();
if (sub.markers.size())
data_[i].markers_.reset(new MarkerVisualization(sub.markers));
else
data_[i].markers_.reset();
++i;
}
}

View File

@ -0,0 +1,171 @@
#include <moveit/visualization_tools/marker_visualization.h>
#include <rviz/default_plugin/markers/marker_base.h>
#include "rviz/default_plugin/markers/arrow_marker.h"
#include "rviz/default_plugin/markers/line_list_marker.h"
#include "rviz/default_plugin/markers/line_strip_marker.h"
#include "rviz/default_plugin/markers/mesh_resource_marker.h"
#include "rviz/default_plugin/markers/points_marker.h"
#include "rviz/default_plugin/markers/shape_marker.h"
#include "rviz/default_plugin/markers/text_view_facing_marker.h"
#include "rviz/default_plugin/markers/triangle_list_marker.h"
#include <OgreSceneManager.h>
namespace moveit_rviz_plugin {
rviz::MarkerBase* createMarker(int marker_type, rviz::DisplayContext* context, Ogre::SceneNode* node)
{
switch (marker_type) {
case visualization_msgs::Marker::CUBE:
case visualization_msgs::Marker::CYLINDER:
case visualization_msgs::Marker::SPHERE:
return new rviz::ShapeMarker(nullptr, context, node);
case visualization_msgs::Marker::ARROW:
return new rviz::ArrowMarker(nullptr, context, node);
case visualization_msgs::Marker::LINE_STRIP:
return new rviz::LineStripMarker(nullptr, context, node);
case visualization_msgs::Marker::LINE_LIST:
return new rviz::LineListMarker(nullptr, context, node);
case visualization_msgs::Marker::SPHERE_LIST:
case visualization_msgs::Marker::CUBE_LIST:
case visualization_msgs::Marker::POINTS:
return new rviz::PointsMarker(nullptr, context, node);
case visualization_msgs::Marker::TEXT_VIEW_FACING:
return new rviz::TextViewFacingMarker(nullptr, context, node);
case visualization_msgs::Marker::MESH_RESOURCE:
return new rviz::MeshResourceMarker(nullptr, context, node);
case visualization_msgs::Marker::TRIANGLE_LIST:
return new rviz::TriangleListMarker(nullptr, context, node);
default:
ROS_ERROR("Unknown marker type: %d", marker_type);
return nullptr;
}
}
MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::Marker> &markers)
{
// 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));
markers_.push_back(std::move(data));
// remember namespace name
namespaces_.insert(std::make_pair(QString::fromStdString(marker.ns), nullptr));
}
}
MarkerVisualization::~MarkerVisualization()
{
for (const auto& pair : namespaces_)
pair.second->getCreator()->destroySceneNode(pair.second);
}
void setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible)
{
if (visible && node->getParent() != parent)
parent->addChild(node);
else if (!visible && node->getParent())
node->getParent()->removeChild(node);
}
void MarkerVisualization::setVisible(const QString &ns, Ogre::SceneNode* parent_scene_node, bool visible)
{
auto it = namespaces_.find(ns);
if (it == namespaces_.end())
return;
setVisibility(it->second, parent_scene_node, visible);
}
void MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::SceneNode *parent_scene_node)
{
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) data.second->setMessage(data.first);
}
}
MarkerVisualizationProperty::MarkerVisualizationProperty(const QString &name, rviz::Property *parent)
: rviz::BoolProperty(name, true, "Enable/disable markers", parent)
{
connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged()));
}
MarkerVisualizationProperty::~MarkerVisualizationProperty()
{
if (marker_scene_node_)
marker_scene_node_->getCreator()->destroySceneNode(marker_scene_node_);
}
void MarkerVisualizationProperty::onInitialize(Ogre::SceneNode *scene_node, rviz::DisplayContext *context)
{
context_ = context;
parent_scene_node_ = scene_node;
marker_scene_node_ = parent_scene_node_->createChildSceneNode();
}
void MarkerVisualizationProperty::clearMarkers()
{
// detach all existing scene nodes
marker_scene_node_->removeAllChildren();
// clear list of memorized visible markers
visible_markers_.clear();
}
void MarkerVisualizationProperty::showMarkers(MarkerVisualizationPtr markers)
{
if (!markers) return;
// memorize that those markers are visible
visible_markers_.push_back(markers);
// attach all scene nodes from markers
for (const auto& pair : markers->namespaces()) {
// create namespace sub property
auto ns_it = namespaces_.insert(std::make_pair(pair.first, nullptr)).first;
if (ns_it->second == nullptr) {
ns_it->second = new rviz::BoolProperty(pair.first, true, "Show/hide markers of this namespace", this,
SLOT(onNSEnableChanged()), this);
}
if (!ns_it->second->getBool())
continue; // nothing to show for this namespace
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);
marker_scene_node_->addChild(pair.second);
}
}
void MarkerVisualizationProperty::onEnableChanged()
{
setVisibility(marker_scene_node_, parent_scene_node_, getBool());
}
void MarkerVisualizationProperty::onNSEnableChanged()
{
rviz::BoolProperty *ns_property = static_cast<rviz::BoolProperty*>(sender());
const QString &ns = ns_property->getName();
bool visible = ns_property->getBool();
for (const auto& markers : visible_markers_)
markers->setVisible(ns, marker_scene_node_, visible);
}
} // namespace moveit_rviz_plugin

View File

@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Dave Coleman, Robert Haschke */
/* Author: Robert Haschke */
#include <moveit/visualization_tools/display_solution.h>
#include <moveit/visualization_tools/task_solution_visualization.h>
@ -164,6 +164,9 @@ TaskSolutionVisualization::~TaskSolutionVisualization()
robot_render_.reset();
if (slider_dock_panel_)
delete slider_dock_panel_;
if (main_scene_node_)
main_scene_node_->getCreator()->destroySceneNode(main_scene_node_);
}
void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context)