cleanup TaskSolutionDisplay

This commit is contained in:
Robert Haschke 2017-10-28 01:21:35 +02:00
parent 042d33f331
commit 82758abc65
4 changed files with 72 additions and 55 deletions

View File

@ -37,6 +37,11 @@
*/
#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>
namespace moveit_rviz_plugin

View File

@ -41,10 +41,9 @@
#include <rviz/display.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>
#include <moveit/macros/class_forward.h>
#endif
namespace rviz
@ -52,12 +51,16 @@ namespace rviz
class StringProperty;
}
namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } }
namespace rdf_loader { MOVEIT_CLASS_FORWARD(RDFLoader) }
namespace moveit_rviz_plugin
{
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
class TaskSolutionDisplay : public rviz::Display
{
Q_OBJECT
// friend class TaskSolutionVisualization; // allow the visualization class to access the display
public:
TaskSolutionDisplay();
@ -87,8 +90,7 @@ protected:
// Load robot model
rdf_loader::RDFLoaderPtr rdf_loader_;
robot_model::RobotModelConstPtr robot_model_;
robot_state::RobotStatePtr robot_state_;
moveit::core::RobotModelConstPtr robot_model_;
bool load_robot_model_; // for delayed robot initialization
// Properties

View File

@ -38,23 +38,24 @@
#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_VISUALIZATION
#include <moveit/macros/class_forward.h>
#include <rviz/display.h>
#include <rviz/panel_dock_widget.h>
#include <moveit_task_constructor/Solution.h>
#include <QObject>
#include <boost/thread/mutex.hpp>
#ifndef Q_MOC_RUN
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.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>
#include <ros/node_handle.h>
#endif
namespace Ogre
{
class SceneNode;
}
namespace rviz
{
class Display;
class DisplayContext;
class Robot;
class Shape;
class Property;
class IntProperty;
class StringProperty;
@ -63,13 +64,22 @@ class FloatProperty;
class RosTopicProperty;
class EditableEnumProperty;
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
{
MOVEIT_CLASS_FORWARD(RobotStateVisualization)
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
class TaskSolutionPanel;
class TaskSolutionVisualization : public QObject
{
Q_OBJECT
@ -77,11 +87,11 @@ class TaskSolutionVisualization : public QObject
public:
/**
* \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
* \return true on success
*/
TaskSolutionVisualization(rviz::Property* widget, rviz::Display* display);
TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display);
virtual ~TaskSolutionVisualization();
@ -89,7 +99,7 @@ public:
virtual void reset();
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 onDisable();
void setName(const QString& name);
@ -135,23 +145,23 @@ protected:
robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_;
std::vector<rviz::Robot*> trajectory_trail_;
ros::Subscriber trajectory_topic_sub_;
bool animating_path_;
bool drop_displaying_trajectory_;
int current_state_;
bool animating_path_ = false;
bool drop_displaying_trajectory_ = false;
int current_state_ = -1;
float current_state_time_;
boost::mutex update_trajectory_message_;
robot_model::RobotModelConstPtr robot_model_;
robot_state::RobotStatePtr robot_state_;
moveit::core::RobotModelConstPtr robot_model_;
moveit::core::RobotStatePtr robot_state_;
// Pointers from parent display taht we save
rviz::Display* display_; // the parent display that this class populates
rviz::Property* widget_;
rviz::Property* parent_;
Ogre::SceneNode* scene_node_;
rviz::DisplayContext* context_;
ros::NodeHandle update_nh_;
TaskSolutionPanel* trajectory_slider_panel_;
rviz::PanelDockWidget* trajectory_slider_dock_panel_;
TaskSolutionPanel* trajectory_slider_panel_ = nullptr;
rviz::PanelDockWidget* trajectory_slider_dock_panel_ = nullptr;
// Properties
rviz::BoolProperty* display_path_visual_enabled_property_;

View File

@ -34,15 +34,20 @@
/* 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_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/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/int_property.h>
#include <rviz/properties/string_property.h>
@ -51,47 +56,42 @@
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/editable_enum_property.h>
#include <rviz/properties/color_property.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
#include <rviz/robot/robot_link.h>
#include <rviz/display.h>
#include <rviz/display_context.h>
#include <rviz/window_manager_interface.h>
#include <rviz/panel_dock_widget.h>
namespace moveit_rviz_plugin
{
TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* widget, rviz::Display* display)
TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display)
: display_(display)
, widget_(widget)
, animating_path_(false)
, drop_displaying_trajectory_(false)
, current_state_(-1)
, trajectory_slider_panel_(NULL)
, trajectory_slider_dock_panel_(NULL)
, parent_(parent)
{
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>(),
"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);
display_path_visual_enabled_property_ =
new rviz::BoolProperty("Show Robot Visual", true, "Indicates whether the geometry of the robot as defined for "
"visualisation purposes should be displayed",
widget, SLOT(changedDisplayPathVisualEnabled()), this);
parent, SLOT(changedDisplayPathVisualEnabled()), this);
display_path_collision_enabled_property_ =
new rviz::BoolProperty("Show Robot Collision", false, "Indicates whether the geometry of the robot as defined "
"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",
widget, SLOT(changedRobotPathAlpha()), this);
parent, SLOT(changedRobotPathAlpha()), this);
robot_path_alpha_property_->setMin(0.0);
robot_path_alpha_property_->setMax(1.0);
state_display_time_property_ = new rviz::EditableEnumProperty("State Display Time", "0.05 s",
"The amount of wall-time to wait in between displaying "
"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("0.05 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 "
"is to be animated in a loop",
widget, SLOT(changedLoopDisplay()), this);
parent, SLOT(changedLoopDisplay()), this);
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 "
"shown in the trajectory trail.",
widget, SLOT(changedTrailStepSize()), this);
parent, SLOT(changedTrailStepSize()), this);
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);
"Immediately show newly planned trajectory, interrupting the currently displayed one.", parent);
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(
"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()
@ -140,7 +140,7 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::
update_nh_ = update_nh;
// 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_->setCollisionVisible(display_path_collision_enabled_property_->getBool());
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())
{
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
display_path_robot_->update(
displaying_trajectory_message_->getWayPointPtr(displaying_trajectory_message_->getWayPointCount() - 1));
@ -419,7 +419,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
else
++current_state_;
int waypoint_count = displaying_trajectory_message_->getWayPointCount();
if ((std::size_t)current_state_ < waypoint_count)
if (current_state_ < waypoint_count)
{
if (trajectory_slider_panel_)
trajectory_slider_panel_->setSliderPosition(current_state_);