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)),
|
connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)),
|
||||||
task_list_model_.get(), SLOT(highlightStage(size_t)));
|
task_list_model_.get(), SLOT(highlightStage(size_t)));
|
||||||
|
|
||||||
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);
|
||||||
}
|
}
|
||||||
@ -103,7 +101,6 @@ 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_);
|
|
||||||
|
|
||||||
#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
|
#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
|
// 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()
|
void TaskDisplay::changedTaskSolutionTopic()
|
||||||
{
|
{
|
||||||
|
|||||||
@ -39,6 +39,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <rviz/display.h>
|
#include <rviz/display.h>
|
||||||
|
#include <moveit/visualization_tools/task_solution_visualization.h>
|
||||||
|
|
||||||
#ifndef Q_MOC_RUN
|
#ifndef Q_MOC_RUN
|
||||||
#include "job_queue.h"
|
#include "job_queue.h"
|
||||||
@ -63,8 +64,6 @@ namespace moveit_rviz_plugin
|
|||||||
{
|
{
|
||||||
|
|
||||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||||
class TaskSolutionVisualization;
|
|
||||||
class MarkerVisualizationProperty;
|
|
||||||
class TaskListModel;
|
class TaskListModel;
|
||||||
|
|
||||||
class TaskDisplay : public rviz::Display
|
class TaskDisplay : public rviz::Display
|
||||||
@ -87,8 +86,9 @@ public:
|
|||||||
|
|
||||||
TaskListModel& getTaskListModel() { return *task_list_model_; }
|
TaskListModel& getTaskListModel() { return *task_list_model_; }
|
||||||
TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); }
|
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:
|
protected:
|
||||||
void onInitialize() override;
|
void onInitialize() override;
|
||||||
@ -132,7 +132,6 @@ 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_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|||||||
@ -80,6 +80,7 @@ MOVEIT_CLASS_FORWARD(PlanningSceneRender)
|
|||||||
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||||
|
|
||||||
class TaskSolutionPanel;
|
class TaskSolutionPanel;
|
||||||
|
class MarkerVisualizationProperty;
|
||||||
class TaskSolutionVisualization : public QObject
|
class TaskSolutionVisualization : public QObject
|
||||||
{
|
{
|
||||||
Q_OBJECT
|
Q_OBJECT
|
||||||
@ -105,9 +106,12 @@ public:
|
|||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr getScene() const { return scene_; }
|
planning_scene::PlanningSceneConstPtr getScene() const { return scene_; }
|
||||||
void showTrajectory(const moveit_task_constructor_msgs::Solution& msg);
|
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 unlock();
|
||||||
|
|
||||||
|
void clearMarkers();
|
||||||
|
void addMarkers(const moveit_rviz_plugin::DisplaySolutionPtr &s);
|
||||||
|
|
||||||
public Q_SLOTS:
|
public Q_SLOTS:
|
||||||
void interruptCurrentDisplay();
|
void interruptCurrentDisplay();
|
||||||
|
|
||||||
@ -142,6 +146,8 @@ protected:
|
|||||||
PlanningSceneRenderPtr scene_render_;
|
PlanningSceneRenderPtr scene_render_;
|
||||||
// render the robot
|
// render the robot
|
||||||
RobotStateVisualizationPtr robot_render_;
|
RobotStateVisualizationPtr robot_render_;
|
||||||
|
// render markers
|
||||||
|
MarkerVisualizationProperty* marker_visual_;
|
||||||
|
|
||||||
// Handle colouring of robot
|
// Handle colouring of robot
|
||||||
void setRobotColor(rviz::Robot* robot, const QColor& color);
|
void setRobotColor(rviz::Robot* robot, const QColor& color);
|
||||||
|
|||||||
@ -36,6 +36,7 @@
|
|||||||
|
|
||||||
#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>
|
||||||
|
#include <moveit/visualization_tools/marker_visualization.h>
|
||||||
#include <moveit/visualization_tools/task_solution_panel.h>
|
#include <moveit/visualization_tools/task_solution_panel.h>
|
||||||
|
|
||||||
#include <moveit/rviz_plugin_render_tools/planning_scene_render.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("Z-Axis", OCTOMAP_Z_AXIS_COLOR);
|
||||||
octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR);
|
octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR);
|
||||||
|
|
||||||
|
marker_visual_ = new MarkerVisualizationProperty("Markers", parent);
|
||||||
}
|
}
|
||||||
|
|
||||||
TaskSolutionVisualization::~TaskSolutionVisualization()
|
TaskSolutionVisualization::~TaskSolutionVisualization()
|
||||||
@ -165,6 +168,8 @@ TaskSolutionVisualization::~TaskSolutionVisualization()
|
|||||||
if (slider_dock_panel_)
|
if (slider_dock_panel_)
|
||||||
delete slider_dock_panel_;
|
delete slider_dock_panel_;
|
||||||
|
|
||||||
|
delete marker_visual_;
|
||||||
|
|
||||||
if (main_scene_node_)
|
if (main_scene_node_)
|
||||||
main_scene_node_->getCreator()->destroySceneNode(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_.reset(new PlanningSceneRender(main_scene_node_, context_, RobotStateVisualizationPtr()));
|
||||||
scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
|
scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
|
||||||
|
|
||||||
|
marker_visual_->onInitialize(main_scene_node_, context_);
|
||||||
|
|
||||||
rviz::WindowManagerInterface* window_context = context_->getWindowManager();
|
rviz::WindowManagerInterface* window_context = context_->getWindowManager();
|
||||||
if (window_context)
|
if (window_context)
|
||||||
{
|
{
|
||||||
@ -473,6 +480,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
|
|||||||
planning_scene::ObjectColorMap color_map;
|
planning_scene::ObjectColorMap color_map;
|
||||||
scene->getKnownObjectColors(color_map);
|
scene->getKnownObjectColors(color_map);
|
||||||
robot_render_->update(robot_state, color, color_map);
|
robot_render_->update(robot_state, color, color_map);
|
||||||
|
marker_visual_->update(*scene, *robot_state);
|
||||||
|
|
||||||
if (slider_panel_ && index >= 0)
|
if (slider_panel_ && index >= 0)
|
||||||
slider_panel_->setSliderPosition(index);
|
slider_panel_->setSliderPosition(index);
|
||||||
@ -502,7 +510,7 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msg
|
|||||||
showTrajectory(s, false);
|
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()) {
|
if (lock_display || !s->empty()) {
|
||||||
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
||||||
@ -519,6 +527,19 @@ 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()
|
void TaskSolutionVisualization::changedRobotColor()
|
||||||
{
|
{
|
||||||
if (enable_robot_color_property_->getBool())
|
if (enable_robot_color_property_->getBool())
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user