mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
allow marker pose updates during trajectory display
moved marker_visual_ from TaskDisplay to TaskSolutionVisualization
This commit is contained in:
parent
714117a48a
commit
b316a2e9c4
@ -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()
|
||||
{
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user