mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
adaptions for TaskSolutionDisplay
This commit is contained in:
parent
18cc780407
commit
042d33f331
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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)
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user