diff --git a/msg/Solution.msg b/msg/Solution.msg index f2a5e1af..eb232ce6 100644 --- a/msg/Solution.msg +++ b/msg/Solution.msg @@ -1,6 +1,9 @@ # id of associated task string task_id +# id of robot model +string model_id + # set of all sub solutions involved SubSolution[] sub_solution diff --git a/task_solution_rviz_plugin_description.xml b/task_solution_rviz_plugin_description.xml index 6a5d75cc..16c6a847 100644 --- a/task_solution_rviz_plugin_description.xml +++ b/task_solution_rviz_plugin_description.xml @@ -1,6 +1,6 @@ Animate task solution trajectories diff --git a/visualization/task_solution_display/CMakeLists.txt b/visualization/task_solution_display/CMakeLists.txt index a8af839e..f59586c6 100644 --- a/visualization/task_solution_display/CMakeLists.txt +++ b/visualization/task_solution_display/CMakeLists.txt @@ -9,6 +9,7 @@ target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_task_visualization_tools ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} ) +target_include_directories(${MOVEIT_LIB_NAME}_core PUBLIC include) # Plugin Initializer add_library(${MOVEIT_LIB_NAME} plugin_init.cpp) diff --git a/visualization/task_solution_display/plugin_init.cpp b/visualization/task_solution_display/plugin_init.cpp index 9a23562e..70af2520 100644 --- a/visualization/task_solution_display/plugin_init.cpp +++ b/visualization/task_solution_display/plugin_init.cpp @@ -35,6 +35,6 @@ /* Author: Dave Coleman */ #include -#include +#include -CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::TrajectoryDisplay, rviz::Display) +CLASS_LOADER_REGISTER_CLASS(moveit_rviz_plugin::TaskSolutionDisplay, rviz::Display) diff --git a/visualization/task_solution_display/task_solution_display.cpp b/visualization/task_solution_display/task_solution_display.cpp index 655f0c08..7191bdc0 100644 --- a/visualization/task_solution_display/task_solution_display.cpp +++ b/visualization/task_solution_display/task_solution_display.cpp @@ -33,7 +33,7 @@ *********************************************************************/ /* 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" @@ -41,29 +41,29 @@ 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 // planning plugin) robot_description_property_ = new rviz::StringProperty( - "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", - this, SLOT(changedRobotDescription()), this); + "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", + 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(); trajectory_visual_->onInitialize(scene_node_, context_, update_nh_); } -void TrajectoryDisplay::loadRobotModel() +void TaskSolutionDisplay::loadRobotModel() { load_robot_model_ = false; rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); @@ -85,26 +85,26 @@ void TrajectoryDisplay::loadRobotModel() trajectory_visual_->onEnable(); } -void TrajectoryDisplay::reset() +void TaskSolutionDisplay::reset() { Display::reset(); loadRobotModel(); trajectory_visual_->reset(); } -void TrajectoryDisplay::onEnable() +void TaskSolutionDisplay::onEnable() { Display::onEnable(); load_robot_model_ = true; // allow loading of robot model in update() } -void TrajectoryDisplay::onDisable() +void TaskSolutionDisplay::onDisable() { Display::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); @@ -114,13 +114,13 @@ void TrajectoryDisplay::update(float wall_dt, float ros_dt) trajectory_visual_->update(wall_dt, ros_dt); } -void TrajectoryDisplay::setName(const QString& name) +void TaskSolutionDisplay::setName(const QString& name) { BoolProperty::setName(name); trajectory_visual_->setName(name); } -void TrajectoryDisplay::changedRobotDescription() +void TaskSolutionDisplay::changedRobotDescription() { if (isEnabled()) reset(); diff --git a/visualization/task_solution_display/task_solution_display.h b/visualization/task_solution_display/task_solution_display.h index e160c942..4322abdb 100644 --- a/visualization/task_solution_display/task_solution_display.h +++ b/visualization/task_solution_display/task_solution_display.h @@ -33,15 +33,15 @@ *********************************************************************/ /* 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 -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_DISPLAY +#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_DISPLAY +#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_DISPLAY #include -#include +#include #ifndef Q_MOC_RUN #include #include @@ -54,15 +54,15 @@ class StringProperty; namespace moveit_rviz_plugin { -class TrajectoryDisplay : public rviz::Display +class TaskSolutionDisplay : public rviz::Display { 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: - TrajectoryDisplay(); + TaskSolutionDisplay(); - virtual ~TrajectoryDisplay(); + virtual ~TaskSolutionDisplay(); void loadRobotModel(); @@ -83,7 +83,7 @@ private Q_SLOTS: protected: // The trajectory playback component - TrajectoryVisualizationPtr trajectory_visual_; + TaskSolutionVisualizationPtr trajectory_visual_; // Load robot model rdf_loader::RDFLoaderPtr rdf_loader_; diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 80bc1803..4b4fcb80 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -18,6 +18,7 @@ target_link_libraries(${MOVEIT_LIB_NAME} ${QT_LIBRARIES} ${Boost_LIBRARIES} ) +target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include) install(DIRECTORY include/ DESTINATION include) diff --git a/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_panel.h b/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_panel.h index 1d02c06a..4f0d691f 100644 --- a/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_panel.h +++ b/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_panel.h @@ -34,8 +34,8 @@ /* Author: Yannick Jonetzko */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TRAJECTORY_PANEL_ -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TRAJECTORY_PANEL_ +#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TASK_SOLUTION_PANEL_ +#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TASK_SOLUTION_PANEL_ #ifndef Q_MOC_RUN #include @@ -49,14 +49,14 @@ namespace moveit_rviz_plugin { -class TrajectoryPanel : public rviz::Panel +class TaskSolutionPanel : public rviz::Panel { Q_OBJECT public: - TrajectoryPanel(QWidget* parent = 0); + TaskSolutionPanel(QWidget* parent = 0); - virtual ~TrajectoryPanel(); + virtual ~TaskSolutionPanel(); void onInitialize(); void onEnable(); diff --git a/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_visualization.h index ecefd7aa..208b57b6 100644 --- a/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_visualization.h @@ -34,8 +34,8 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_VISUALIZATION -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TRAJECTORY_VISUALIZATION +#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLITION_VISUALIZATION +#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_VISUALIZATION #include #include @@ -43,12 +43,12 @@ #ifndef Q_MOC_RUN #include -#include +#include #include #include #include #include -#include +#include #endif namespace rviz @@ -68,9 +68,9 @@ class MovableText; namespace moveit_rviz_plugin { -MOVEIT_CLASS_FORWARD(TrajectoryVisualization); +MOVEIT_CLASS_FORWARD(TaskSolutionVisualization) -class TrajectoryVisualization : public QObject +class TaskSolutionVisualization : public QObject { Q_OBJECT @@ -81,9 +81,9 @@ public: * \param display - the rviz::Display from the parent * \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 reset(); @@ -120,7 +120,7 @@ protected: /** * \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(); void clearTrajectoryTrail(); @@ -150,7 +150,7 @@ protected: Ogre::SceneNode* scene_node_; rviz::DisplayContext* context_; ros::NodeHandle update_nh_; - TrajectoryPanel* trajectory_slider_panel_; + TaskSolutionPanel* trajectory_slider_panel_; rviz::PanelDockWidget* trajectory_slider_dock_panel_; // Properties diff --git a/visualization/visualization_tools/src/task_solution_panel.cpp b/visualization/visualization_tools/src/task_solution_panel.cpp index f08be50d..abb0217c 100644 --- a/visualization/visualization_tools/src/task_solution_panel.cpp +++ b/visualization/visualization_tools/src/task_solution_panel.cpp @@ -34,20 +34,20 @@ /* Author: Yannick Jonetzko */ -#include +#include #include 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_->setTickInterval(1); @@ -79,20 +79,20 @@ void TrajectoryPanel::onInitialize() parentWidget()->setVisible(false); } -void TrajectoryPanel::onEnable() +void TaskSolutionPanel::onEnable() { show(); parentWidget()->show(); } -void TrajectoryPanel::onDisable() +void TaskSolutionPanel::onDisable() { hide(); paused_ = false; 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); @@ -106,7 +106,7 @@ void TrajectoryPanel::update(int way_point_count) maximum_label_->setText(QString::number(max_way_point)); } -void TrajectoryPanel::pauseButton(bool pause) +void TaskSolutionPanel::pauseButton(bool 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); } -void TrajectoryPanel::sliderValueChanged(int value) +void TaskSolutionPanel::sliderValueChanged(int value) { minimum_label_->setText(QString::number(value)); } -void TrajectoryPanel::buttonClicked() +void TaskSolutionPanel::buttonClicked() { if (paused_) pauseButton(false); diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index b9bddef5..69984499 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include #include #include @@ -58,7 +58,7 @@ namespace moveit_rviz_plugin { -TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::Display* display) +TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* widget, rviz::Display* display) : display_(display) , widget_(widget) , animating_path_(false) @@ -69,8 +69,8 @@ TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::D { trajectory_topic_property_ = new rviz::RosTopicProperty("Trajectory Topic", "/move_group/display_planned_path", - ros::message_traits::datatype(), - "The topic on which the moveit_msgs::DisplayTrajectory messages are received", widget, + ros::message_traits::datatype(), + "The topic on which the moveit_task_constructor::Solution messages are received", widget, SLOT(changedTrajectoryTopic()), this); display_path_visual_enabled_property_ = @@ -110,17 +110,17 @@ TrajectoryVisualization::TrajectoryVisualization(rviz::Property* widget, rviz::D trail_step_size_property_->setMin(1); interrupt_display_property_ = new rviz::BoolProperty( - "Interrupt Display", false, - "Immediately show newly planned trajectory, interrupting the currently displayed one.", widget); + "Interrupt Display", false, + "Immediately show newly planned trajectory, interrupting the currently displayed one.", widget); robot_color_property_ = new rviz::ColorProperty( - "Robot Color", QColor(150, 50, 150), "The color of the animated robot", widget, SLOT(changedRobotColor()), this); + "Robot Color", QColor(150, 50, 150), "The color of the animated robot", widget, SLOT(changedRobotColor()), this); enable_robot_color_property_ = new rviz::BoolProperty( - "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(); trajectory_message_to_display_.reset(); @@ -131,8 +131,8 @@ TrajectoryVisualization::~TrajectoryVisualization() delete trajectory_slider_dock_panel_; } -void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, - ros::NodeHandle update_nh) +void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, + ros::NodeHandle update_nh) { // Save pointers for later use scene_node_ = scene_node; @@ -148,7 +148,7 @@ void TrajectoryVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::Di rviz::WindowManagerInterface* window_context = context_->getWindowManager(); if (window_context) { - trajectory_slider_panel_ = new TrajectoryPanel(window_context->getParentWindow()); + trajectory_slider_panel_ = new TaskSolutionPanel(window_context->getParentWindow()); trajectory_slider_dock_panel_ = window_context->addPane(display_->getName() + " - Slider", trajectory_slider_panel_); 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_) 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; // Error check 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; } @@ -184,7 +184,7 @@ void TrajectoryVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr enabledRobotColor(); // force-refresh to account for saved display configuration } -void TrajectoryVisualization::reset() +void TaskSolutionVisualization::reset() { clearTrajectoryTrail(); trajectory_message_to_display_.reset(); @@ -196,21 +196,21 @@ void TrajectoryVisualization::reset() display_path_robot_->setVisible(false); } -void TrajectoryVisualization::clearTrajectoryTrail() +void TaskSolutionVisualization::clearTrajectoryTrail() { for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) delete trajectory_trail_[i]; trajectory_trail_.clear(); } -void TrajectoryVisualization::changedLoopDisplay() +void TaskSolutionVisualization::changedLoopDisplay() { display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_); if (loop_display_property_->getBool() && trajectory_slider_panel_) trajectory_slider_panel_->pauseButton(false); } -void TrajectoryVisualization::changedShowTrail() +void TaskSolutionVisualization::changedShowTrail() { clearTrajectoryTrail(); @@ -241,30 +241,30 @@ void TrajectoryVisualization::changedShowTrail() } } -void TrajectoryVisualization::changedTrailStepSize() +void TaskSolutionVisualization::changedTrailStepSize() { if (trail_display_property_->getBool()) changedShowTrail(); } -void TrajectoryVisualization::changedRobotPathAlpha() +void TaskSolutionVisualization::changedRobotPathAlpha() { display_path_robot_->setAlpha(robot_path_alpha_property_->getFloat()); for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) trajectory_trail_[i]->setAlpha(robot_path_alpha_property_->getFloat()); } -void TrajectoryVisualization::changedTrajectoryTopic() +void TaskSolutionVisualization::changedTrajectoryTopic() { trajectory_topic_sub_.shutdown(); if (!trajectory_topic_property_->getStdString().empty()) { 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()) { @@ -275,11 +275,11 @@ void TrajectoryVisualization::changedDisplayPathVisualEnabled() } } -void TrajectoryVisualization::changedStateDisplayTime() +void TaskSolutionVisualization::changedStateDisplayTime() { } -void TrajectoryVisualization::changedDisplayPathCollisionEnabled() +void TaskSolutionVisualization::changedDisplayPathCollisionEnabled() { if (display_->isEnabled()) { @@ -290,7 +290,7 @@ void TrajectoryVisualization::changedDisplayPathCollisionEnabled() } } -void TrajectoryVisualization::onEnable() +void TaskSolutionVisualization::onEnable() { changedRobotPathAlpha(); // set alpha property @@ -307,7 +307,7 @@ void TrajectoryVisualization::onEnable() changedTrajectoryTopic(); // load topic at startup if default used } -void TrajectoryVisualization::onDisable() +void TaskSolutionVisualization::onDisable() { display_path_robot_->setVisible(false); for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) @@ -318,7 +318,7 @@ void TrajectoryVisualization::onDisable() trajectory_slider_panel_->onDisable(); } -void TrajectoryVisualization::interruptCurrentDisplay() +void TaskSolutionVisualization::interruptCurrentDisplay() { // update() starts a new trajectory as soon as it is available // interrupting may cause the newly received trajectory to interrupt @@ -327,7 +327,7 @@ void TrajectoryVisualization::interruptCurrentDisplay() animating_path_ = false; } -float TrajectoryVisualization::getStateDisplayTime() +float TaskSolutionVisualization::getStateDisplayTime() { std::string tm = state_display_time_property_->getStdString(); if (tm == "REALTIME") @@ -349,12 +349,12 @@ float TrajectoryVisualization::getStateDisplayTime() } } -void TrajectoryVisualization::dropTrajectory() +void TaskSolutionVisualization::dropTrajectory() { 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_) { @@ -388,7 +388,7 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt) if (trajectory_slider_panel_->getSliderPosition() == displaying_trajectory_message_->getWayPointCount() - 1) { // show the last waypoint if the slider is enabled display_path_robot_->update( - displaying_trajectory_message_->getWayPointPtr(displaying_trajectory_message_->getWayPointCount() - 1)); + displaying_trajectory_message_->getWayPointPtr(displaying_trajectory_message_->getWayPointCount() - 1)); } else animating_path_ = true; @@ -426,8 +426,8 @@ void TrajectoryVisualization::update(float wall_dt, float ros_dt) display_path_robot_->update(displaying_trajectory_message_->getWayPointPtr(current_state_)); for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) trajectory_trail_[i]->setVisible( - std::min(waypoint_count - 1, static_cast(i) * trail_step_size_property_->getInt()) <= - current_state_); + std::min(waypoint_count - 1, static_cast(i) * trail_step_size_property_->getInt()) <= + current_state_); } else { @@ -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 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; } @@ -458,16 +458,16 @@ void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::Displ trajectory_message_to_display_.reset(); 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()) { - t->setRobotTrajectoryMsg(*robot_state_, msg->trajectory_start, msg->trajectory[i]); + t->setRobotTrajectoryMsg(*robot_state_, msg->sub_trajectory[i].trajectory); } else { 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); } } @@ -481,13 +481,13 @@ void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::Displ } } -void TrajectoryVisualization::changedRobotColor() +void TaskSolutionVisualization::changedRobotColor() { if (enable_robot_color_property_->getBool()) setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor()); } -void TrajectoryVisualization::enabledRobotColor() +void TaskSolutionVisualization::enabledRobotColor() { if (enable_robot_color_property_->getBool()) setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor()); @@ -495,19 +495,19 @@ void TrajectoryVisualization::enabledRobotColor() unsetRobotColor(&(display_path_robot_->getRobot())); } -void TrajectoryVisualization::unsetRobotColor(rviz::Robot* robot) +void TaskSolutionVisualization::unsetRobotColor(rviz::Robot* robot) { for (auto& link : robot->getLinks()) 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()) 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_) return;