allow marker pose updates during trajectory display

moved marker_visual_ from TaskDisplay to TaskSolutionVisualization
This commit is contained in:
Robert Haschke 2018-03-25 20:32:48 +02:00
parent 714117a48a
commit b316a2e9c4
4 changed files with 34 additions and 21 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

@ -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);

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,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()