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
string task_id
# id of robot model
string model_id
# set of all sub solutions involved
SubSolution[] sub_solution

View File

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

View File

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

View File

@ -35,6 +35,6 @@
/* Author: Dave Coleman */
#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
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();

View File

@ -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 <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
#include <ros/ros.h>
#include <moveit/rdf_loader/rdf_loader.h>
@ -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_;

View File

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

View File

@ -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 <ros/ros.h>
@ -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();

View File

@ -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 <moveit/macros/class_forward.h>
#include <rviz/display.h>
@ -43,12 +43,12 @@
#ifndef Q_MOC_RUN
#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 <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_task_constructor/Solution.h>
#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

View File

@ -34,20 +34,20 @@
/* Author: Yannick Jonetzko */
#include <moveit/rviz_plugin_render_tools/trajectory_panel.h>
#include <moveit_task_constructor/visualization_tools/task_solution_panel.h>
#include <QHBoxLayout>
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);

View File

@ -37,7 +37,7 @@
#include <boost/algorithm/string.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/robot_state_visualization.h>
@ -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<moveit_msgs::DisplayTrajectory>(),
"The topic on which the moveit_msgs::DisplayTrajectory messages are received", widget,
ros::message_traits::datatype<moveit_task_constructor::Solution>(),
"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<int>(i) * trail_step_size_property_->getInt()) <=
current_state_);
std::min(waypoint_count - 1, static_cast<int>(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;