adaptions for TaskSolutionDisplay

This commit is contained in:
eirtech 2017-10-26 12:26:48 +02:00 committed by Robert Haschke
parent 18cc780407
commit 042d33f331
11 changed files with 102 additions and 97 deletions

View File

@ -1,6 +1,9 @@
# id of associated task # id of associated task
string task_id string task_id
# id of robot model
string model_id
# set of all sub solutions involved # set of all sub solutions involved
SubSolution[] sub_solution SubSolution[] sub_solution

View File

@ -1,6 +1,6 @@
<library path="libmoveit_task_solution_rviz_plugin"> <library path="libmoveit_task_solution_rviz_plugin">
<class name="moveit_task_constructor/Task Solution Display" <class name="moveit_task_constructor/Task Solution Display"
type="moveit_rviz_plugin::TaskSolution" type="moveit_rviz_plugin::TaskSolutionDisplay"
base_class_type="rviz::Display"> base_class_type="rviz::Display">
<description> <description>
Animate task solution trajectories Animate task solution trajectories

View File

@ -9,6 +9,7 @@ target_link_libraries(${MOVEIT_LIB_NAME}_core
moveit_task_visualization_tools moveit_task_visualization_tools
${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES}
) )
target_include_directories(${MOVEIT_LIB_NAME}_core PUBLIC include)
# Plugin Initializer # Plugin Initializer
add_library(${MOVEIT_LIB_NAME} plugin_init.cpp) add_library(${MOVEIT_LIB_NAME} plugin_init.cpp)

View File

@ -35,6 +35,6 @@
/* Author: Dave Coleman */ /* Author: Dave Coleman */
#include <class_loader/class_loader.h> #include <class_loader/class_loader.h>
#include <moveit/trajectory_rviz_plugin/trajectory_display.h> #include <task_solution_display.h>
CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::TrajectoryDisplay, rviz::Display) CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::TaskSolutionDisplay, rviz::Display)

View File

@ -33,7 +33,7 @@
*********************************************************************/ *********************************************************************/
/* Author: Dave Coleman /* Author: Dave Coleman
Desc: Wraps a trajectory_visualization playback class for Rviz into a stand alone display Desc: Wraps a task_solution_visualization playback class for Rviz into a stand alone display
*/ */
#include "task_solution_display.h" #include "task_solution_display.h"
@ -41,7 +41,7 @@
namespace moveit_rviz_plugin namespace moveit_rviz_plugin
{ {
TrajectoryDisplay::TrajectoryDisplay() : Display(), load_robot_model_(false) TaskSolutionDisplay::TaskSolutionDisplay() : Display(), load_robot_model_(false)
{ {
// The robot description property is only needed when using the trajectory playback standalone (not within motion // The robot description property is only needed when using the trajectory playback standalone (not within motion
// planning plugin) // planning plugin)
@ -49,21 +49,21 @@ TrajectoryDisplay::TrajectoryDisplay() : Display(), load_robot_model_(false)
"Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
this, SLOT(changedRobotDescription()), this); this, SLOT(changedRobotDescription()), this);
trajectory_visual_.reset(new TrajectoryVisualization(this, this)); trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
} }
TrajectoryDisplay::~TrajectoryDisplay() TaskSolutionDisplay::~TaskSolutionDisplay()
{ {
} }
void TrajectoryDisplay::onInitialize() void TaskSolutionDisplay::onInitialize()
{ {
Display::onInitialize(); Display::onInitialize();
trajectory_visual_->onInitialize(scene_node_, context_, update_nh_); trajectory_visual_->onInitialize(scene_node_, context_, update_nh_);
} }
void TrajectoryDisplay::loadRobotModel() void TaskSolutionDisplay::loadRobotModel()
{ {
load_robot_model_ = false; load_robot_model_ = false;
rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString()));
@ -85,26 +85,26 @@ void TrajectoryDisplay::loadRobotModel()
trajectory_visual_->onEnable(); trajectory_visual_->onEnable();
} }
void TrajectoryDisplay::reset() void TaskSolutionDisplay::reset()
{ {
Display::reset(); Display::reset();
loadRobotModel(); loadRobotModel();
trajectory_visual_->reset(); trajectory_visual_->reset();
} }
void TrajectoryDisplay::onEnable() void TaskSolutionDisplay::onEnable()
{ {
Display::onEnable(); Display::onEnable();
load_robot_model_ = true; // allow loading of robot model in update() load_robot_model_ = true; // allow loading of robot model in update()
} }
void TrajectoryDisplay::onDisable() void TaskSolutionDisplay::onDisable()
{ {
Display::onDisable(); Display::onDisable();
trajectory_visual_->onDisable(); trajectory_visual_->onDisable();
} }
void TrajectoryDisplay::update(float wall_dt, float ros_dt) void TaskSolutionDisplay::update(float wall_dt, float ros_dt)
{ {
Display::update(wall_dt, ros_dt); Display::update(wall_dt, ros_dt);
@ -114,13 +114,13 @@ void TrajectoryDisplay::update(float wall_dt, float ros_dt)
trajectory_visual_->update(wall_dt, ros_dt); trajectory_visual_->update(wall_dt, ros_dt);
} }
void TrajectoryDisplay::setName(const QString& name) void TaskSolutionDisplay::setName(const QString& name)
{ {
BoolProperty::setName(name); BoolProperty::setName(name);
trajectory_visual_->setName(name); trajectory_visual_->setName(name);
} }
void TrajectoryDisplay::changedRobotDescription() void TaskSolutionDisplay::changedRobotDescription()
{ {
if (isEnabled()) if (isEnabled())
reset(); reset();

View File

@ -33,15 +33,15 @@
*********************************************************************/ *********************************************************************/
/* Author: Dave Coleman /* Author: Dave Coleman
Desc: Wraps a trajectory_visualization playback class for Rviz into a stand alone display Desc: Wraps a task_solution_visualization playback class for Rviz into a stand alone display
*/ */
#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_DISPLAY #ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_DISPLAY
#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_DISPLAY #define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_DISPLAY
#include <rviz/display.h> #include <rviz/display.h>
#include <moveit/rviz_plugin_render_tools/trajectory_visualization.h> #include <moveit_task_constructor/visualization_tools/task_solution_visualization.h>
#ifndef Q_MOC_RUN #ifndef Q_MOC_RUN
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit/rdf_loader/rdf_loader.h> #include <moveit/rdf_loader/rdf_loader.h>
@ -54,15 +54,15 @@ class StringProperty;
namespace moveit_rviz_plugin namespace moveit_rviz_plugin
{ {
class TrajectoryDisplay : public rviz::Display class TaskSolutionDisplay : public rviz::Display
{ {
Q_OBJECT Q_OBJECT
// friend class TrajectoryVisualization; // allow the visualization class to access the display // friend class TaskSolutionVisualization; // allow the visualization class to access the display
public: public:
TrajectoryDisplay(); TaskSolutionDisplay();
virtual ~TrajectoryDisplay(); virtual ~TaskSolutionDisplay();
void loadRobotModel(); void loadRobotModel();
@ -83,7 +83,7 @@ private Q_SLOTS:
protected: protected:
// The trajectory playback component // The trajectory playback component
TrajectoryVisualizationPtr trajectory_visual_; TaskSolutionVisualizationPtr trajectory_visual_;
// Load robot model // Load robot model
rdf_loader::RDFLoaderPtr rdf_loader_; rdf_loader::RDFLoaderPtr rdf_loader_;

View File

@ -18,6 +18,7 @@ target_link_libraries(${MOVEIT_LIB_NAME}
${QT_LIBRARIES} ${QT_LIBRARIES}
${Boost_LIBRARIES} ${Boost_LIBRARIES}
) )
target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include)
install(DIRECTORY include/ DESTINATION include) install(DIRECTORY include/ DESTINATION include)

View File

@ -34,8 +34,8 @@
/* Author: Yannick Jonetzko */ /* Author: Yannick Jonetzko */
#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TRAJECTORY_PANEL_ #ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TASK_SOLUTION_PANEL_
#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TRAJECTORY_PANEL_ #define MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TASK_SOLUTION_PANEL_
#ifndef Q_MOC_RUN #ifndef Q_MOC_RUN
#include <ros/ros.h> #include <ros/ros.h>
@ -49,14 +49,14 @@
namespace moveit_rviz_plugin namespace moveit_rviz_plugin
{ {
class TrajectoryPanel : public rviz::Panel class TaskSolutionPanel : public rviz::Panel
{ {
Q_OBJECT Q_OBJECT
public: public:
TrajectoryPanel(QWidget* parent = 0); TaskSolutionPanel(QWidget* parent = 0);
virtual ~TrajectoryPanel(); virtual ~TaskSolutionPanel();
void onInitialize(); void onInitialize();
void onEnable(); void onEnable();

View File

@ -34,8 +34,8 @@
/* Author: Dave Coleman */ /* Author: Dave Coleman */
#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_VISUALIZATION #ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLITION_VISUALIZATION
#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_VISUALIZATION #define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_VISUALIZATION
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <rviz/display.h> #include <rviz/display.h>
@ -43,12 +43,12 @@
#ifndef Q_MOC_RUN #ifndef Q_MOC_RUN
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.h> #include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
#include <moveit/rviz_plugin_render_tools/trajectory_panel.h> #include <moveit_task_constructor/visualization_tools/task_solution_panel.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <moveit/robot_model/robot_model.h> #include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h> #include <moveit/robot_state/robot_state.h>
#include <moveit/robot_trajectory/robot_trajectory.h> #include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit_msgs/DisplayTrajectory.h> #include <moveit_task_constructor/Solution.h>
#endif #endif
namespace rviz namespace rviz
@ -68,9 +68,9 @@ class MovableText;
namespace moveit_rviz_plugin namespace moveit_rviz_plugin
{ {
MOVEIT_CLASS_FORWARD(TrajectoryVisualization); MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
class TrajectoryVisualization : public QObject class TaskSolutionVisualization : public QObject
{ {
Q_OBJECT Q_OBJECT
@ -81,9 +81,9 @@ public:
* \param display - the rviz::Display from the parent * \param display - the rviz::Display from the parent
* \return true on success * \return true on success
*/ */
TrajectoryVisualization(rviz::Property* widget, rviz::Display* display); TaskSolutionVisualization(rviz::Property* widget, rviz::Display* display);
virtual ~TrajectoryVisualization(); virtual ~TaskSolutionVisualization();
virtual void update(float wall_dt, float ros_dt); virtual void update(float wall_dt, float ros_dt);
virtual void reset(); virtual void reset();
@ -120,7 +120,7 @@ protected:
/** /**
* \brief ROS callback for an incoming path message * \brief ROS callback for an incoming path message
*/ */
void incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg); void incomingSolution(const moveit_task_constructor::Solution::ConstPtr& msg);
float getStateDisplayTime(); float getStateDisplayTime();
void clearTrajectoryTrail(); void clearTrajectoryTrail();
@ -150,7 +150,7 @@ protected:
Ogre::SceneNode* scene_node_; Ogre::SceneNode* scene_node_;
rviz::DisplayContext* context_; rviz::DisplayContext* context_;
ros::NodeHandle update_nh_; ros::NodeHandle update_nh_;
TrajectoryPanel* trajectory_slider_panel_; TaskSolutionPanel* trajectory_slider_panel_;
rviz::PanelDockWidget* trajectory_slider_dock_panel_; rviz::PanelDockWidget* trajectory_slider_dock_panel_;
// Properties // Properties

View File

@ -34,20 +34,20 @@
/* Author: Yannick Jonetzko */ /* Author: Yannick Jonetzko */
#include <moveit/rviz_plugin_render_tools/trajectory_panel.h> #include <moveit_task_constructor/visualization_tools/task_solution_panel.h>
#include <QHBoxLayout> #include <QHBoxLayout>
namespace moveit_rviz_plugin namespace moveit_rviz_plugin
{ {
TrajectoryPanel::TrajectoryPanel(QWidget* parent) : Panel(parent) TaskSolutionPanel::TaskSolutionPanel(QWidget* parent) : Panel(parent)
{ {
} }
TrajectoryPanel::~TrajectoryPanel() TaskSolutionPanel::~TaskSolutionPanel()
{ {
} }
void TrajectoryPanel::onInitialize() void TaskSolutionPanel::onInitialize()
{ {
slider_ = new QSlider(Qt::Horizontal); slider_ = new QSlider(Qt::Horizontal);
slider_->setTickInterval(1); slider_->setTickInterval(1);
@ -79,20 +79,20 @@ void TrajectoryPanel::onInitialize()
parentWidget()->setVisible(false); parentWidget()->setVisible(false);
} }
void TrajectoryPanel::onEnable() void TaskSolutionPanel::onEnable()
{ {
show(); show();
parentWidget()->show(); parentWidget()->show();
} }
void TrajectoryPanel::onDisable() void TaskSolutionPanel::onDisable()
{ {
hide(); hide();
paused_ = false; paused_ = false;
parentWidget()->hide(); parentWidget()->hide();
} }
void TrajectoryPanel::update(int way_point_count) void TaskSolutionPanel::update(int way_point_count)
{ {
int max_way_point = std::max(0, way_point_count - 1); int max_way_point = std::max(0, way_point_count - 1);
@ -106,7 +106,7 @@ void TrajectoryPanel::update(int way_point_count)
maximum_label_->setText(QString::number(max_way_point)); maximum_label_->setText(QString::number(max_way_point));
} }
void TrajectoryPanel::pauseButton(bool pause) void TaskSolutionPanel::pauseButton(bool pause)
{ {
if (pause) if (pause)
{ {
@ -122,17 +122,17 @@ void TrajectoryPanel::pauseButton(bool pause)
} }
} }
void TrajectoryPanel::setSliderPosition(int position) void TaskSolutionPanel::setSliderPosition(int position)
{ {
slider_->setSliderPosition(position); slider_->setSliderPosition(position);
} }
void TrajectoryPanel::sliderValueChanged(int value) void TaskSolutionPanel::sliderValueChanged(int value)
{ {
minimum_label_->setText(QString::number(value)); minimum_label_->setText(QString::number(value));
} }
void TrajectoryPanel::buttonClicked() void TaskSolutionPanel::buttonClicked()
{ {
if (paused_) if (paused_)
pauseButton(false); pauseButton(false);

View File

@ -37,7 +37,7 @@
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/replace.hpp> #include <boost/algorithm/string/replace.hpp>
#include <moveit/rviz_plugin_render_tools/trajectory_visualization.h> #include <moveit_task_constructor/visualization_tools/task_solution_visualization.h>
#include <moveit/rviz_plugin_render_tools/planning_link_updater.h> #include <moveit/rviz_plugin_render_tools/planning_link_updater.h>
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.h> #include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
@ -58,7 +58,7 @@
namespace moveit_rviz_plugin namespace moveit_rviz_plugin
{ {
TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::Display* display) TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* widget, rviz::Display* display)
: display_(display) : display_(display)
, widget_(widget) , widget_(widget)
, animating_path_(false) , animating_path_(false)
@ -69,8 +69,8 @@ TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::D
{ {
trajectory_topic_property_ = trajectory_topic_property_ =
new rviz::RosTopicProperty("Trajectory Topic", "/move_group/display_planned_path", new rviz::RosTopicProperty("Trajectory Topic", "/move_group/display_planned_path",
ros::message_traits::datatype<moveit_msgs::DisplayTrajectory>(), ros::message_traits::datatype<moveit_task_constructor::Solution>(),
"The topic on which the moveit_msgs::DisplayTrajectory messages are received", widget, "The topic on which the moveit_task_constructor::Solution messages are received", widget,
SLOT(changedTrajectoryTopic()), this); SLOT(changedTrajectoryTopic()), this);
display_path_visual_enabled_property_ = display_path_visual_enabled_property_ =
@ -120,7 +120,7 @@ TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::D
"Color Enabled", false, "Specifies whether robot coloring is enabled", widget, SLOT(enabledRobotColor()), this); "Color Enabled", false, "Specifies whether robot coloring is enabled", widget, SLOT(enabledRobotColor()), this);
} }
TrajectoryVisualization::~TrajectoryVisualization() TaskSolutionVisualization::~TaskSolutionVisualization()
{ {
clearTrajectoryTrail(); clearTrajectoryTrail();
trajectory_message_to_display_.reset(); trajectory_message_to_display_.reset();
@ -131,7 +131,7 @@ TrajectoryVisualization::~TrajectoryVisualization()
delete trajectory_slider_dock_panel_; delete trajectory_slider_dock_panel_;
} }
void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context,
ros::NodeHandle update_nh) ros::NodeHandle update_nh)
{ {
// Save pointers for later use // Save pointers for later use
@ -148,7 +148,7 @@ void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::Di
rviz::WindowManagerInterface* window_context = context_->getWindowManager(); rviz::WindowManagerInterface* window_context = context_->getWindowManager();
if (window_context) if (window_context)
{ {
trajectory_slider_panel_ = new TrajectoryPanel(window_context->getParentWindow()); trajectory_slider_panel_ = new TaskSolutionPanel(window_context->getParentWindow());
trajectory_slider_dock_panel_ = trajectory_slider_dock_panel_ =
window_context->addPane(display_->getName() + " - Slider", trajectory_slider_panel_); window_context->addPane(display_->getName() + " - Slider", trajectory_slider_panel_);
trajectory_slider_dock_panel_->setIcon(display_->getIcon()); trajectory_slider_dock_panel_->setIcon(display_->getIcon());
@ -158,20 +158,20 @@ void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::Di
} }
} }
void TrajectoryVisualization::setName(const QString& name) void TaskSolutionVisualization::setName(const QString& name)
{ {
if (trajectory_slider_dock_panel_) if (trajectory_slider_dock_panel_)
trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider"); trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider");
} }
void TrajectoryVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model) void TaskSolutionVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model)
{ {
robot_model_ = robot_model; robot_model_ = robot_model;
// Error check // Error check
if (!robot_model_) if (!robot_model_)
{ {
ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot model found"); ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found");
return; return;
} }
@ -184,7 +184,7 @@ void TrajectoryVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr
enabledRobotColor(); // force-refresh to account for saved display configuration enabledRobotColor(); // force-refresh to account for saved display configuration
} }
void TrajectoryVisualization::reset() void TaskSolutionVisualization::reset()
{ {
clearTrajectoryTrail(); clearTrajectoryTrail();
trajectory_message_to_display_.reset(); trajectory_message_to_display_.reset();
@ -196,21 +196,21 @@ void TrajectoryVisualization::reset()
display_path_robot_->setVisible(false); display_path_robot_->setVisible(false);
} }
void TrajectoryVisualization::clearTrajectoryTrail() void TaskSolutionVisualization::clearTrajectoryTrail()
{ {
for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
delete trajectory_trail_[i]; delete trajectory_trail_[i];
trajectory_trail_.clear(); trajectory_trail_.clear();
} }
void TrajectoryVisualization::changedLoopDisplay() void TaskSolutionVisualization::changedLoopDisplay()
{ {
display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_); display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_);
if (loop_display_property_->getBool() && trajectory_slider_panel_) if (loop_display_property_->getBool() && trajectory_slider_panel_)
trajectory_slider_panel_->pauseButton(false); trajectory_slider_panel_->pauseButton(false);
} }
void TrajectoryVisualization::changedShowTrail() void TaskSolutionVisualization::changedShowTrail()
{ {
clearTrajectoryTrail(); clearTrajectoryTrail();
@ -241,30 +241,30 @@ void TrajectoryVisualization::changedShowTrail()
} }
} }
void TrajectoryVisualization::changedTrailStepSize() void TaskSolutionVisualization::changedTrailStepSize()
{ {
if (trail_display_property_->getBool()) if (trail_display_property_->getBool())
changedShowTrail(); changedShowTrail();
} }
void TrajectoryVisualization::changedRobotPathAlpha() void TaskSolutionVisualization::changedRobotPathAlpha()
{ {
display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat()); display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat());
for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
trajectory_trail_[i]->setAlpha(robot_path_alpha_property_->getFloat()); trajectory_trail_[i]->setAlpha(robot_path_alpha_property_->getFloat());
} }
void TrajectoryVisualization::changedTrajectoryTopic() void TaskSolutionVisualization::changedTrajectoryTopic()
{ {
trajectory_topic_sub_.shutdown(); trajectory_topic_sub_.shutdown();
if (!trajectory_topic_property_->getStdString().empty()) if (!trajectory_topic_property_->getStdString().empty())
{ {
trajectory_topic_sub_ = update_nh_.subscribe(trajectory_topic_property_->getStdString(), 2, trajectory_topic_sub_ = update_nh_.subscribe(trajectory_topic_property_->getStdString(), 2,
&TrajectoryVisualization::incomingDisplayTrajectory, this); &TaskSolutionVisualization::incomingSolution, this);
} }
} }
void TrajectoryVisualization::changedDisplayPathVisualEnabled() void TaskSolutionVisualization::changedDisplayPathVisualEnabled()
{ {
if (display_->isEnabled()) if (display_->isEnabled())
{ {
@ -275,11 +275,11 @@ void TrajectoryVisualization::changedDisplayPathVisualEnabled()
} }
} }
void TrajectoryVisualization::changedStateDisplayTime() void TaskSolutionVisualization::changedStateDisplayTime()
{ {
} }
void TrajectoryVisualization::changedDisplayPathCollisionEnabled() void TaskSolutionVisualization::changedDisplayPathCollisionEnabled()
{ {
if (display_->isEnabled()) if (display_->isEnabled())
{ {
@ -290,7 +290,7 @@ void TrajectoryVisualization::changedDisplayPathCollisionEnabled()
} }
} }
void TrajectoryVisualization::onEnable() void TaskSolutionVisualization::onEnable()
{ {
changedRobotPathAlpha(); // set alpha property changedRobotPathAlpha(); // set alpha property
@ -307,7 +307,7 @@ void TrajectoryVisualization::onEnable()
changedTrajectoryTopic(); // load topic at startup if default used changedTrajectoryTopic(); // load topic at startup if default used
} }
void TrajectoryVisualization::onDisable() void TaskSolutionVisualization::onDisable()
{ {
display_path_robot_->setVisible(false); display_path_robot_->setVisible(false);
for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) for (std::size_t i = 0; i < trajectory_trail_.size(); ++i)
@ -318,7 +318,7 @@ void TrajectoryVisualization::onDisable()
trajectory_slider_panel_->onDisable(); trajectory_slider_panel_->onDisable();
} }
void TrajectoryVisualization::interruptCurrentDisplay() void TaskSolutionVisualization::interruptCurrentDisplay()
{ {
// update() starts a new trajectory as soon as it is available // update() starts a new trajectory as soon as it is available
// interrupting may cause the newly received trajectory to interrupt // interrupting may cause the newly received trajectory to interrupt
@ -327,7 +327,7 @@ void TrajectoryVisualization::interruptCurrentDisplay()
animating_path_ = false; animating_path_ = false;
} }
float TrajectoryVisualization::getStateDisplayTime() float TaskSolutionVisualization::getStateDisplayTime()
{ {
std::string tm = state_display_time_property_->getStdString(); std::string tm = state_display_time_property_->getStdString();
if (tm == "REALTIME") if (tm == "REALTIME")
@ -349,12 +349,12 @@ float TrajectoryVisualization::getStateDisplayTime()
} }
} }
void TrajectoryVisualization::dropTrajectory() void TaskSolutionVisualization::dropTrajectory()
{ {
drop_displaying_trajectory_ = true; drop_displaying_trajectory_ = true;
} }
void TrajectoryVisualization::update(float wall_dt, float ros_dt) void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
{ {
if (drop_displaying_trajectory_) if (drop_displaying_trajectory_)
{ {
@ -442,12 +442,12 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt)
} }
} }
void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg) void TaskSolutionVisualization::incomingSolution(const moveit_task_constructor::Solution::ConstPtr& msg)
{ {
// Error check // Error check
if (!robot_state_ || !robot_model_) if (!robot_state_ || !robot_model_)
{ {
ROS_ERROR_STREAM_NAMED("trajectory_visualization", "No robot state or robot model loaded"); ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot state or robot model loaded");
return; return;
} }
@ -458,16 +458,16 @@ void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::Displ
trajectory_message_to_display_.reset(); trajectory_message_to_display_.reset();
robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, "")); robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, ""));
for (std::size_t i = 0; i < msg->trajectory.size(); ++i) for (std::size_t i = 0; i < msg->sub_trajectory.size(); ++i)
{ {
if (t->empty()) if (t->empty())
{ {
t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]); t->setRobotTrajectoryMsg(*robot_state_, msg->sub_trajectory[i].trajectory);
} }
else else
{ {
robot_trajectory::RobotTrajectory tmp(robot_model_, ""); robot_trajectory::RobotTrajectory tmp(robot_model_, "");
tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->trajectory[i]); tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg->sub_trajectory[i].trajectory);
t->append(tmp, 0.0); t->append(tmp, 0.0);
} }
} }
@ -481,13 +481,13 @@ void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::Displ
} }
} }
void TrajectoryVisualization::changedRobotColor() void TaskSolutionVisualization::changedRobotColor()
{ {
if (enable_robot_color_property_->getBool()) if (enable_robot_color_property_->getBool())
setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor()); setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor());
} }
void TrajectoryVisualization::enabledRobotColor() void TaskSolutionVisualization::enabledRobotColor()
{ {
if (enable_robot_color_property_->getBool()) if (enable_robot_color_property_->getBool())
setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor()); setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor());
@ -495,19 +495,19 @@ void TrajectoryVisualization::enabledRobotColor()
unsetRobotColor(&(display_path_robot_->getRobot())); unsetRobotColor(&(display_path_robot_->getRobot()));
} }
void TrajectoryVisualization::unsetRobotColor(rviz::Robot* robot) void TaskSolutionVisualization::unsetRobotColor(rviz::Robot* robot)
{ {
for (auto& link : robot->getLinks()) for (auto& link : robot->getLinks())
link.second->unsetColor(); link.second->unsetColor();
} }
void TrajectoryVisualization::setRobotColor(rviz::Robot* robot, const QColor& color) void TaskSolutionVisualization::setRobotColor(rviz::Robot* robot, const QColor& color)
{ {
for (auto& link : robot->getLinks()) for (auto& link : robot->getLinks())
robot->getLink(link.first)->setColor(color.redF(), color.greenF(), color.blueF()); robot->getLink(link.first)->setColor(color.redF(), color.greenF(), color.blueF());
} }
void TrajectoryVisualization::trajectorySliderPanelVisibilityChange(bool enable) void TaskSolutionVisualization::trajectorySliderPanelVisibilityChange(bool enable)
{ {
if (!trajectory_slider_panel_) if (!trajectory_slider_panel_)
return; return;