diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index eb627c54..67fa1f18 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -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() { diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index 53e0c3a8..fd4e70fc 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -39,6 +39,7 @@ #pragma once #include +#include #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_; }; diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index f4f70cd1..737bbdf6 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -80,6 +80,7 @@ MOVEIT_CLASS_FORWARD(PlanningSceneRender) MOVEIT_CLASS_FORWARD(DisplaySolution) class TaskSolutionPanel; +class MarkerVisualizationProperty; class TaskSolutionVisualization : public QObject { Q_OBJECT @@ -105,9 +106,12 @@ 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(); @@ -142,6 +146,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); diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 5560ea7a..05e12e42 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -152,6 +153,8 @@ 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); } TaskSolutionVisualization::~TaskSolutionVisualization() @@ -165,6 +168,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 +193,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) { @@ -473,6 +480,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 +510,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 +524,20 @@ 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) return; + for (size_t i=0, end = s->numSubSolutions(); i != end; ++i) { + marker_visual_->addMarkers(s->markersOfSubTrajectory(i)); + } } void TaskSolutionVisualization::changedRobotColor()