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 "meta_task_list_model.h"
|
||||||
#include <moveit/task_constructor/introspection.h>
|
#include <moveit/task_constructor/introspection.h>
|
||||||
#include <moveit/visualization_tools/task_solution_visualization.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_task_constructor_msgs/GetSolution.h>
|
||||||
|
|
||||||
#include <moveit/rdf_loader/rdf_loader.h>
|
#include <moveit/rdf_loader/rdf_loader.h>
|
||||||
@ -77,6 +79,8 @@ TaskDisplay::TaskDisplay() : Display()
|
|||||||
|
|
||||||
trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
|
trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
|
||||||
|
|
||||||
|
marker_visual_ = new MarkerVisualizationProperty("Markers", this);
|
||||||
|
|
||||||
tasks_property_ =
|
tasks_property_ =
|
||||||
new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this);
|
new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this);
|
||||||
}
|
}
|
||||||
@ -89,6 +93,7 @@ void TaskDisplay::onInitialize()
|
|||||||
{
|
{
|
||||||
Display::onInitialize();
|
Display::onInitialize();
|
||||||
trajectory_visual_->onInitialize(scene_node_, context_);
|
trajectory_visual_->onInitialize(scene_node_, context_);
|
||||||
|
marker_visual_->onInitialize(scene_node_, context_);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskDisplay::loadRobotModel()
|
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;
|
const std::string id = event.getPublisherName() + "/" + msg->task_id;
|
||||||
mainloop_jobs_.addJob([this, id, msg]() {
|
mainloop_jobs_.addJob([this, id, msg]() {
|
||||||
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
|
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
|
||||||
trajectory_visual_->showTrajectory(s);
|
if (s) trajectory_visual_->showTrajectory(s);
|
||||||
return;
|
return;
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
@ -189,6 +194,17 @@ void TaskDisplay::showTrajectory(const DisplaySolutionPtr &s) const {
|
|||||||
trajectory_visual_->showTrajectory(s);
|
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()
|
void TaskDisplay::changedTaskSolutionTopic()
|
||||||
{
|
{
|
||||||
task_description_sub.shutdown();
|
task_description_sub.shutdown();
|
||||||
|
|||||||
@ -64,6 +64,7 @@ namespace moveit_rviz_plugin
|
|||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||||
class TaskSolutionVisualization;
|
class TaskSolutionVisualization;
|
||||||
|
class MarkerVisualizationProperty;
|
||||||
class TaskListModel;
|
class TaskListModel;
|
||||||
|
|
||||||
class TaskDisplay : public rviz::Display
|
class TaskDisplay : public rviz::Display
|
||||||
@ -87,6 +88,8 @@ public:
|
|||||||
|
|
||||||
TaskListModel& getTaskListModel() { return *task_list_model_; }
|
TaskListModel& getTaskListModel() { return *task_list_model_; }
|
||||||
void showTrajectory(const DisplaySolutionPtr& s) const;
|
void showTrajectory(const DisplaySolutionPtr& s) const;
|
||||||
|
void clearMarkers();
|
||||||
|
void showMarkers(const DisplaySolutionPtr &s);
|
||||||
|
|
||||||
private Q_SLOTS:
|
private Q_SLOTS:
|
||||||
/**
|
/**
|
||||||
@ -123,6 +126,7 @@ protected:
|
|||||||
// Properties
|
// Properties
|
||||||
rviz::StringProperty* robot_description_property_;
|
rviz::StringProperty* robot_description_property_;
|
||||||
rviz::RosTopicProperty* task_solution_topic_property_;
|
rviz::RosTopicProperty* task_solution_topic_property_;
|
||||||
|
MarkerVisualizationProperty* marker_visual_;
|
||||||
rviz::Property* tasks_property_;
|
rviz::Property* tasks_property_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -219,6 +219,8 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode
|
|||||||
return;
|
return;
|
||||||
|
|
||||||
display->showTrajectory(solution);
|
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
|
set(HEADERS
|
||||||
${PROJECT_INCLUDE}/display_solution.h
|
${PROJECT_INCLUDE}/display_solution.h
|
||||||
|
${PROJECT_INCLUDE}/marker_visualization.h
|
||||||
${PROJECT_INCLUDE}/task_solution_panel.h
|
${PROJECT_INCLUDE}/task_solution_panel.h
|
||||||
${PROJECT_INCLUDE}/task_solution_visualization.h
|
${PROJECT_INCLUDE}/task_solution_visualization.h
|
||||||
)
|
)
|
||||||
@ -12,6 +13,7 @@ add_library(${MOVEIT_LIB_NAME}
|
|||||||
${HEADERS}
|
${HEADERS}
|
||||||
|
|
||||||
src/display_solution.cpp
|
src/display_solution.cpp
|
||||||
|
src/marker_visualization.cpp
|
||||||
src/task_solution_panel.cpp
|
src/task_solution_panel.cpp
|
||||||
src/task_solution_visualization.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}
|
target_link_libraries(${MOVEIT_LIB_NAME}
|
||||||
${catkin_LIBRARIES}
|
${catkin_LIBRARIES}
|
||||||
|
${rviz_DEFAULT_PLUGIN_LIBRARIES}
|
||||||
${OGRE_LIBRARIES}
|
${OGRE_LIBRARIES}
|
||||||
${QT_LIBRARIES}
|
${QT_LIBRARIES}
|
||||||
${Boost_LIBRARIES}
|
${Boost_LIBRARIES}
|
||||||
|
|||||||
@ -48,10 +48,18 @@ MOVEIT_CLASS_FORWARD(PlanningScene)
|
|||||||
namespace robot_trajectory {
|
namespace robot_trajectory {
|
||||||
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
MOVEIT_CLASS_FORWARD(RobotTrajectory)
|
||||||
}
|
}
|
||||||
|
namespace Ogre
|
||||||
|
{
|
||||||
|
class SceneNode;
|
||||||
|
}
|
||||||
|
namespace rviz {
|
||||||
|
class DisplayContext;
|
||||||
|
}
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||||
|
MOVEIT_CLASS_FORWARD(MarkerVisualization)
|
||||||
|
|
||||||
/** Class representing a task solution for display */
|
/** Class representing a task solution for display */
|
||||||
class DisplaySolution
|
class DisplaySolution
|
||||||
@ -68,6 +76,8 @@ class DisplaySolution
|
|||||||
robot_trajectory::RobotTrajectoryPtr trajectory_;
|
robot_trajectory::RobotTrajectoryPtr trajectory_;
|
||||||
/// optional name of the trajectory
|
/// optional name of the trajectory
|
||||||
std::string name_;
|
std::string name_;
|
||||||
|
/// rviz markers
|
||||||
|
MarkerVisualizationPtr markers_;
|
||||||
};
|
};
|
||||||
std::vector<Data> data_;
|
std::vector<Data> data_;
|
||||||
|
|
||||||
@ -76,6 +86,8 @@ public:
|
|||||||
/// create DisplaySolution for given sub trajectory of master
|
/// create DisplaySolution for given sub trajectory of master
|
||||||
DisplaySolution(const DisplaySolution& master, uint32_t sub);
|
DisplaySolution(const DisplaySolution& master, uint32_t sub);
|
||||||
|
|
||||||
|
size_t numSubSolutions() const { return data_.size(); }
|
||||||
|
|
||||||
size_t getWayPointCount() const { return steps_; }
|
size_t getWayPointCount() const { return steps_; }
|
||||||
bool empty() const { return steps_ == 0; }
|
bool empty() const { return steps_ == 0; }
|
||||||
|
|
||||||
@ -101,6 +113,13 @@ public:
|
|||||||
const std::string& name(size_t index) const {
|
const std::string& name(size_t index) const {
|
||||||
return name(indexPair(index));
|
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,
|
void setFromMessage(const planning_scene::PlanningScenePtr &start_scene,
|
||||||
const moveit_task_constructor_msgs::Solution& msg);
|
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 */
|
/* Author: Robert Haschke */
|
||||||
|
|
||||||
#include <moveit/visualization_tools/display_solution.h>
|
#include <moveit/visualization_tools/display_solution.h>
|
||||||
|
#include <moveit/visualization_tools/marker_visualization.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||||
#include <ros/console.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_;
|
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,
|
void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene,
|
||||||
const moveit_task_constructor_msgs::Solution &msg)
|
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
|
// create new reference scene for next iteration
|
||||||
ref_scene = ref_scene->diff();
|
ref_scene = ref_scene->diff();
|
||||||
|
|
||||||
|
if (sub.markers.size())
|
||||||
|
data_[i].markers_.reset(new MarkerVisualization(sub.markers));
|
||||||
|
else
|
||||||
|
data_[i].markers_.reset();
|
||||||
++i;
|
++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.
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
*********************************************************************/
|
*********************************************************************/
|
||||||
|
|
||||||
/* Author: Dave Coleman, Robert Haschke */
|
/* Author: Robert Haschke */
|
||||||
|
|
||||||
#include <moveit/visualization_tools/display_solution.h>
|
#include <moveit/visualization_tools/display_solution.h>
|
||||||
#include <moveit/visualization_tools/task_solution_visualization.h>
|
#include <moveit/visualization_tools/task_solution_visualization.h>
|
||||||
@ -164,6 +164,9 @@ TaskSolutionVisualization::~TaskSolutionVisualization()
|
|||||||
robot_render_.reset();
|
robot_render_.reset();
|
||||||
if (slider_dock_panel_)
|
if (slider_dock_panel_)
|
||||||
delete 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)
|
void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user