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
|
# 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
|
||||||
|
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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)
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
@ -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_;
|
||||||
|
|||||||
@ -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)
|
||||||
|
|
||||||
|
|||||||
@ -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();
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user