mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
cleanup TaskSolutionDisplay
This commit is contained in:
parent
042d33f331
commit
82758abc65
@ -37,6 +37,11 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
#include "task_solution_display.h"
|
#include "task_solution_display.h"
|
||||||
|
|
||||||
|
#include <moveit_task_constructor/visualization_tools/task_solution_visualization.h>
|
||||||
|
#include <moveit/rdf_loader/rdf_loader.h>
|
||||||
|
#include <moveit/robot_model/robot_model.h>
|
||||||
|
|
||||||
#include <rviz/properties/string_property.h>
|
#include <rviz/properties/string_property.h>
|
||||||
|
|
||||||
namespace moveit_rviz_plugin
|
namespace moveit_rviz_plugin
|
||||||
|
|||||||
@ -41,10 +41,9 @@
|
|||||||
|
|
||||||
#include <rviz/display.h>
|
#include <rviz/display.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/macros/class_forward.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
namespace rviz
|
namespace rviz
|
||||||
@ -52,12 +51,16 @@ namespace rviz
|
|||||||
class StringProperty;
|
class StringProperty;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } }
|
||||||
|
namespace rdf_loader { MOVEIT_CLASS_FORWARD(RDFLoader) }
|
||||||
|
|
||||||
namespace moveit_rviz_plugin
|
namespace moveit_rviz_plugin
|
||||||
{
|
{
|
||||||
|
|
||||||
|
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
|
||||||
class TaskSolutionDisplay : public rviz::Display
|
class TaskSolutionDisplay : public rviz::Display
|
||||||
{
|
{
|
||||||
Q_OBJECT
|
Q_OBJECT
|
||||||
// friend class TaskSolutionVisualization; // allow the visualization class to access the display
|
|
||||||
|
|
||||||
public:
|
public:
|
||||||
TaskSolutionDisplay();
|
TaskSolutionDisplay();
|
||||||
@ -87,8 +90,7 @@ protected:
|
|||||||
|
|
||||||
// Load robot model
|
// Load robot model
|
||||||
rdf_loader::RDFLoaderPtr rdf_loader_;
|
rdf_loader::RDFLoaderPtr rdf_loader_;
|
||||||
robot_model::RobotModelConstPtr robot_model_;
|
moveit::core::RobotModelConstPtr robot_model_;
|
||||||
robot_state::RobotStatePtr robot_state_;
|
|
||||||
bool load_robot_model_; // for delayed robot initialization
|
bool load_robot_model_; // for delayed robot initialization
|
||||||
|
|
||||||
// Properties
|
// Properties
|
||||||
|
|||||||
@ -38,23 +38,24 @@
|
|||||||
#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_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 <moveit_task_constructor/Solution.h>
|
||||||
#include <rviz/panel_dock_widget.h>
|
#include <QObject>
|
||||||
|
#include <boost/thread/mutex.hpp>
|
||||||
|
|
||||||
#ifndef Q_MOC_RUN
|
#ifndef Q_MOC_RUN
|
||||||
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
|
#include <ros/node_handle.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_task_constructor/Solution.h>
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
namespace Ogre
|
||||||
|
{
|
||||||
|
class SceneNode;
|
||||||
|
}
|
||||||
|
|
||||||
namespace rviz
|
namespace rviz
|
||||||
{
|
{
|
||||||
|
class Display;
|
||||||
|
class DisplayContext;
|
||||||
class Robot;
|
class Robot;
|
||||||
class Shape;
|
|
||||||
class Property;
|
class Property;
|
||||||
class IntProperty;
|
class IntProperty;
|
||||||
class StringProperty;
|
class StringProperty;
|
||||||
@ -63,13 +64,22 @@ class FloatProperty;
|
|||||||
class RosTopicProperty;
|
class RosTopicProperty;
|
||||||
class EditableEnumProperty;
|
class EditableEnumProperty;
|
||||||
class ColorProperty;
|
class ColorProperty;
|
||||||
class MovableText;
|
class PanelDockWidget;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
namespace moveit { namespace core {
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotState)
|
||||||
|
} }
|
||||||
|
namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory) }
|
||||||
|
|
||||||
namespace moveit_rviz_plugin
|
namespace moveit_rviz_plugin
|
||||||
{
|
{
|
||||||
|
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotStateVisualization)
|
||||||
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
|
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
|
||||||
|
|
||||||
|
class TaskSolutionPanel;
|
||||||
class TaskSolutionVisualization : public QObject
|
class TaskSolutionVisualization : public QObject
|
||||||
{
|
{
|
||||||
Q_OBJECT
|
Q_OBJECT
|
||||||
@ -77,11 +87,11 @@ class TaskSolutionVisualization : public QObject
|
|||||||
public:
|
public:
|
||||||
/**
|
/**
|
||||||
* \brief Playback a trajectory from a planned path
|
* \brief Playback a trajectory from a planned path
|
||||||
* \param widget - either a rviz::Display or rviz::Property
|
* \param parent - either a rviz::Display or rviz::Property
|
||||||
* \param display - the rviz::Display from the parent
|
* \param display - the rviz::Display from the parent
|
||||||
* \return true on success
|
* \return true on success
|
||||||
*/
|
*/
|
||||||
TaskSolutionVisualization(rviz::Property* widget, rviz::Display* display);
|
TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display);
|
||||||
|
|
||||||
virtual ~TaskSolutionVisualization();
|
virtual ~TaskSolutionVisualization();
|
||||||
|
|
||||||
@ -89,7 +99,7 @@ public:
|
|||||||
virtual void reset();
|
virtual void reset();
|
||||||
|
|
||||||
void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, ros::NodeHandle update_nh);
|
void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context, ros::NodeHandle update_nh);
|
||||||
void onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model);
|
void onRobotModelLoaded(moveit::core::RobotModelConstPtr robot_model);
|
||||||
void onEnable();
|
void onEnable();
|
||||||
void onDisable();
|
void onDisable();
|
||||||
void setName(const QString& name);
|
void setName(const QString& name);
|
||||||
@ -135,23 +145,23 @@ protected:
|
|||||||
robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_;
|
robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_;
|
||||||
std::vector<rviz::Robot*> trajectory_trail_;
|
std::vector<rviz::Robot*> trajectory_trail_;
|
||||||
ros::Subscriber trajectory_topic_sub_;
|
ros::Subscriber trajectory_topic_sub_;
|
||||||
bool animating_path_;
|
bool animating_path_ = false;
|
||||||
bool drop_displaying_trajectory_;
|
bool drop_displaying_trajectory_ = false;
|
||||||
int current_state_;
|
int current_state_ = -1;
|
||||||
float current_state_time_;
|
float current_state_time_;
|
||||||
boost::mutex update_trajectory_message_;
|
boost::mutex update_trajectory_message_;
|
||||||
|
|
||||||
robot_model::RobotModelConstPtr robot_model_;
|
moveit::core::RobotModelConstPtr robot_model_;
|
||||||
robot_state::RobotStatePtr robot_state_;
|
moveit::core::RobotStatePtr robot_state_;
|
||||||
|
|
||||||
// Pointers from parent display taht we save
|
// Pointers from parent display taht we save
|
||||||
rviz::Display* display_; // the parent display that this class populates
|
rviz::Display* display_; // the parent display that this class populates
|
||||||
rviz::Property* widget_;
|
rviz::Property* parent_;
|
||||||
Ogre::SceneNode* scene_node_;
|
Ogre::SceneNode* scene_node_;
|
||||||
rviz::DisplayContext* context_;
|
rviz::DisplayContext* context_;
|
||||||
ros::NodeHandle update_nh_;
|
ros::NodeHandle update_nh_;
|
||||||
TaskSolutionPanel* trajectory_slider_panel_;
|
TaskSolutionPanel* trajectory_slider_panel_ = nullptr;
|
||||||
rviz::PanelDockWidget* trajectory_slider_dock_panel_;
|
rviz::PanelDockWidget* trajectory_slider_dock_panel_ = nullptr;
|
||||||
|
|
||||||
// Properties
|
// Properties
|
||||||
rviz::BoolProperty* display_path_visual_enabled_property_;
|
rviz::BoolProperty* display_path_visual_enabled_property_;
|
||||||
|
|||||||
@ -34,15 +34,20 @@
|
|||||||
|
|
||||||
/* Author: Dave Coleman */
|
/* Author: Dave Coleman */
|
||||||
|
|
||||||
#include <boost/algorithm/string.hpp>
|
|
||||||
#include <boost/algorithm/string/replace.hpp>
|
|
||||||
|
|
||||||
#include <moveit_task_constructor/visualization_tools/task_solution_visualization.h>
|
#include <moveit_task_constructor/visualization_tools/task_solution_visualization.h>
|
||||||
|
#include <moveit_task_constructor/visualization_tools/task_solution_panel.h>
|
||||||
|
#include <moveit_task_constructor/introspection.h>
|
||||||
|
|
||||||
|
#include <moveit/rviz_plugin_render_tools/robot_state_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>
|
||||||
#include <rviz/robot/robot.h>
|
|
||||||
|
|
||||||
|
#include <moveit/robot_model/robot_model.h>
|
||||||
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||||
|
#include <moveit/trajectory_processing/trajectory_tools.h>
|
||||||
|
|
||||||
|
#include <rviz/robot/robot.h>
|
||||||
|
#include <rviz/robot/robot_link.h>
|
||||||
#include <rviz/properties/property.h>
|
#include <rviz/properties/property.h>
|
||||||
#include <rviz/properties/int_property.h>
|
#include <rviz/properties/int_property.h>
|
||||||
#include <rviz/properties/string_property.h>
|
#include <rviz/properties/string_property.h>
|
||||||
@ -51,47 +56,42 @@
|
|||||||
#include <rviz/properties/ros_topic_property.h>
|
#include <rviz/properties/ros_topic_property.h>
|
||||||
#include <rviz/properties/editable_enum_property.h>
|
#include <rviz/properties/editable_enum_property.h>
|
||||||
#include <rviz/properties/color_property.h>
|
#include <rviz/properties/color_property.h>
|
||||||
#include <moveit/trajectory_processing/trajectory_tools.h>
|
#include <rviz/display.h>
|
||||||
#include <rviz/robot/robot_link.h>
|
|
||||||
#include <rviz/display_context.h>
|
#include <rviz/display_context.h>
|
||||||
#include <rviz/window_manager_interface.h>
|
#include <rviz/window_manager_interface.h>
|
||||||
|
#include <rviz/panel_dock_widget.h>
|
||||||
|
|
||||||
namespace moveit_rviz_plugin
|
namespace moveit_rviz_plugin
|
||||||
{
|
{
|
||||||
TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* widget, rviz::Display* display)
|
TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display)
|
||||||
: display_(display)
|
: display_(display)
|
||||||
, widget_(widget)
|
, parent_(parent)
|
||||||
, animating_path_(false)
|
|
||||||
, drop_displaying_trajectory_(false)
|
|
||||||
, current_state_(-1)
|
|
||||||
, trajectory_slider_panel_(NULL)
|
|
||||||
, trajectory_slider_dock_panel_(NULL)
|
|
||||||
{
|
{
|
||||||
trajectory_topic_property_ =
|
trajectory_topic_property_ =
|
||||||
new rviz::RosTopicProperty("Trajectory Topic", "/move_group/display_planned_path",
|
new rviz::RosTopicProperty("Trajectory Topic", DEFAULT_TASK_SOLUTION_TOPIC,
|
||||||
ros::message_traits::datatype<moveit_task_constructor::Solution>(),
|
ros::message_traits::datatype<moveit_task_constructor::Solution>(),
|
||||||
"The topic on which the moveit_task_constructor::Solution messages are received", widget,
|
"The topic on which the moveit_task_constructor::Solution messages are received", parent,
|
||||||
SLOT(changedTrajectoryTopic()), this);
|
SLOT(changedTrajectoryTopic()), this);
|
||||||
|
|
||||||
display_path_visual_enabled_property_ =
|
display_path_visual_enabled_property_ =
|
||||||
new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for "
|
new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for "
|
||||||
"visualisation purposes should be displayed",
|
"visualisation purposes should be displayed",
|
||||||
widget, SLOT(changedDisplayPathVisualEnabled()), this);
|
parent, SLOT(changedDisplayPathVisualEnabled()), this);
|
||||||
|
|
||||||
display_path_collision_enabled_property_ =
|
display_path_collision_enabled_property_ =
|
||||||
new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined "
|
new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined "
|
||||||
"for collision detection purposes should be displayed",
|
"for collision detection purposes should be displayed",
|
||||||
widget, SLOT(changedDisplayPathCollisionEnabled()), this);
|
parent, SLOT(changedDisplayPathCollisionEnabled()), this);
|
||||||
|
|
||||||
robot_path_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links",
|
robot_path_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links",
|
||||||
widget, SLOT(changedRobotPathAlpha()), this);
|
parent, SLOT(changedRobotPathAlpha()), this);
|
||||||
robot_path_alpha_property_->setMin(0.0);
|
robot_path_alpha_property_->setMin(0.0);
|
||||||
robot_path_alpha_property_->setMax(1.0);
|
robot_path_alpha_property_->setMax(1.0);
|
||||||
|
|
||||||
state_display_time_property_ = new rviz::EditableEnumProperty("State Display Time", "0.05 s",
|
state_display_time_property_ = new rviz::EditableEnumProperty("State Display Time", "0.05 s",
|
||||||
"The amount of wall-time to wait in between displaying "
|
"The amount of wall-time to wait in between displaying "
|
||||||
"states along a received trajectory path",
|
"states along a received trajectory path",
|
||||||
widget, SLOT(changedStateDisplayTime()), this);
|
parent, SLOT(changedStateDisplayTime()), this);
|
||||||
state_display_time_property_->addOptionStd("REALTIME");
|
state_display_time_property_->addOptionStd("REALTIME");
|
||||||
state_display_time_property_->addOptionStd("0.05 s");
|
state_display_time_property_->addOptionStd("0.05 s");
|
||||||
state_display_time_property_->addOptionStd("0.1 s");
|
state_display_time_property_->addOptionStd("0.1 s");
|
||||||
@ -99,25 +99,25 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* widget, rvi
|
|||||||
|
|
||||||
loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path "
|
loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path "
|
||||||
"is to be animated in a loop",
|
"is to be animated in a loop",
|
||||||
widget, SLOT(changedLoopDisplay()), this);
|
parent, SLOT(changedLoopDisplay()), this);
|
||||||
|
|
||||||
trail_display_property_ =
|
trail_display_property_ =
|
||||||
new rviz::BoolProperty("Show Trail", false, "Show a path trail", widget, SLOT(changedShowTrail()), this);
|
new rviz::BoolProperty("Show Trail", false, "Show a path trail", parent, SLOT(changedShowTrail()), this);
|
||||||
|
|
||||||
trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, "Specifies the step size of the samples "
|
trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, "Specifies the step size of the samples "
|
||||||
"shown in the trajectory trail.",
|
"shown in the trajectory trail.",
|
||||||
widget, SLOT(changedTrailStepSize()), this);
|
parent, SLOT(changedTrailStepSize()), this);
|
||||||
trail_step_size_property_->setMin(1);
|
trail_step_size_property_->setMin(1);
|
||||||
|
|
||||||
interrupt_display_property_ = new rviz::BoolProperty(
|
interrupt_display_property_ = new rviz::BoolProperty(
|
||||||
"Interrupt Display", false,
|
"Interrupt Display", false,
|
||||||
"Immediately show newly planned trajectory, interrupting the currently displayed one.", widget);
|
"Immediately show newly planned trajectory, interrupting the currently displayed one.", parent);
|
||||||
|
|
||||||
robot_color_property_ = new rviz::ColorProperty(
|
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", parent, SLOT(changedRobotColor()), this);
|
||||||
|
|
||||||
enable_robot_color_property_ = new rviz::BoolProperty(
|
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", parent, SLOT(enabledRobotColor()), this);
|
||||||
}
|
}
|
||||||
|
|
||||||
TaskSolutionVisualization::~TaskSolutionVisualization()
|
TaskSolutionVisualization::~TaskSolutionVisualization()
|
||||||
@ -140,7 +140,7 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::
|
|||||||
update_nh_ = update_nh;
|
update_nh_ = update_nh;
|
||||||
|
|
||||||
// Load trajectory robot
|
// Load trajectory robot
|
||||||
display_path_robot_.reset(new RobotStateVisualization(scene_node_, context_, "Planned Path", widget_));
|
display_path_robot_.reset(new RobotStateVisualization(scene_node_, context_, "Planned Path", parent_));
|
||||||
display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
|
display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool());
|
||||||
display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
|
display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
|
||||||
display_path_robot_->setVisible(false);
|
display_path_robot_->setVisible(false);
|
||||||
@ -385,7 +385,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
|||||||
}
|
}
|
||||||
else if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible())
|
else if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible())
|
||||||
{
|
{
|
||||||
if (trajectory_slider_panel_->getSliderPosition() == displaying_trajectory_message_->getWayPointCount() - 1)
|
if (trajectory_slider_panel_->getSliderPosition() == (int)displaying_trajectory_message_->getWayPointCount() - 1)
|
||||||
{ // show the last waypoint if the slider is enabled
|
{ // show the last waypoint if the slider is enabled
|
||||||
display_path_robot_->update(
|
display_path_robot_->update(
|
||||||
displaying_trajectory_message_->getWayPointPtr(displaying_trajectory_message_->getWayPointCount() - 1));
|
displaying_trajectory_message_->getWayPointPtr(displaying_trajectory_message_->getWayPointCount() - 1));
|
||||||
@ -419,7 +419,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
|||||||
else
|
else
|
||||||
++current_state_;
|
++current_state_;
|
||||||
int waypoint_count = displaying_trajectory_message_->getWayPointCount();
|
int waypoint_count = displaying_trajectory_message_->getWayPointCount();
|
||||||
if ((std::size_t)current_state_ < waypoint_count)
|
if (current_state_ < waypoint_count)
|
||||||
{
|
{
|
||||||
if (trajectory_slider_panel_)
|
if (trajectory_slider_panel_)
|
||||||
trajectory_slider_panel_->setSliderPosition(current_state_);
|
trajectory_slider_panel_->setSliderPosition(current_state_);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user