From 82758abc654a6e915d1432fd5483b47b7bed412f Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 28 Oct 2017 01:21:35 +0200 Subject: [PATCH] cleanup TaskSolutionDisplay --- .../task_solution_display.cpp | 5 ++ .../task_solution_display.h | 12 ++-- .../task_solution_visualization.h | 54 ++++++++++-------- .../src/task_solution_visualization.cpp | 56 +++++++++---------- 4 files changed, 72 insertions(+), 55 deletions(-) diff --git a/visualization/task_solution_display/task_solution_display.cpp b/visualization/task_solution_display/task_solution_display.cpp index 7191bdc0..86ef26df 100644 --- a/visualization/task_solution_display/task_solution_display.cpp +++ b/visualization/task_solution_display/task_solution_display.cpp @@ -37,6 +37,11 @@ */ #include "task_solution_display.h" + +#include +#include +#include + #include namespace moveit_rviz_plugin diff --git a/visualization/task_solution_display/task_solution_display.h b/visualization/task_solution_display/task_solution_display.h index 4322abdb..abf04af8 100644 --- a/visualization/task_solution_display/task_solution_display.h +++ b/visualization/task_solution_display/task_solution_display.h @@ -41,10 +41,9 @@ #include -#include #ifndef Q_MOC_RUN #include -#include +#include #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 diff --git a/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_visualization.h index 208b57b6..983fc015 100644 --- a/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit_task_constructor/visualization_tools/task_solution_visualization.h @@ -38,23 +38,24 @@ #define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_VISUALIZATION #include -#include -#include +#include +#include +#include #ifndef Q_MOC_RUN -#include -#include -#include -#include -#include -#include -#include +#include #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 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_; diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 69984499..fe7c47b1 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -34,15 +34,20 @@ /* Author: Dave Coleman */ -#include -#include - #include +#include +#include +#include #include #include -#include +#include +#include +#include + +#include +#include #include #include #include @@ -51,47 +56,42 @@ #include #include #include -#include -#include +#include #include #include +#include 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(), - "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_);