mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
marker visualization
This commit is contained in:
parent
f7ae7756cc
commit
30f6ade636
@ -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();
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
|
||||
@ -219,6 +219,8 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode
|
||||
return;
|
||||
|
||||
display->showTrajectory(solution);
|
||||
display->clearMarkers();
|
||||
display->showMarkers(solution);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
171
visualization/visualization_tools/src/marker_visualization.cpp
Normal file
171
visualization/visualization_tools/src/marker_visualization.cpp
Normal 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
|
||||
@ -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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user