diff --git a/visualization/motion_planning_tasks/task_display.cpp b/visualization/motion_planning_tasks/task_display.cpp index 1a37d174..ac5ec874 100644 --- a/visualization/motion_planning_tasks/task_display.cpp +++ b/visualization/motion_planning_tasks/task_display.cpp @@ -53,9 +53,9 @@ namespace moveit_rviz_plugin TaskDisplay::TaskDisplay() : Display() { - 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_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); task_solution_topic_property_ = new rviz::RosTopicProperty("Task Solution Topic", "", 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 d0a7496f..75c22183 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 @@ -34,18 +34,13 @@ /* Author: Dave Coleman */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLITION_VISUALIZATION -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN__TASK_SOLUTION_VISUALIZATION +#pragma once #include #include #include #include -#ifndef Q_MOC_RUN -#include -#endif - namespace Ogre { class SceneNode; @@ -62,6 +57,7 @@ class StringProperty; class BoolProperty; class FloatProperty; class RosTopicProperty; +class EnumProperty; class EditableEnumProperty; class ColorProperty; class PanelDockWidget; @@ -71,6 +67,7 @@ namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) MOVEIT_CLASS_FORWARD(RobotState) } } +namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) } namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory) } namespace moveit_rviz_plugin @@ -78,6 +75,7 @@ namespace moveit_rviz_plugin MOVEIT_CLASS_FORWARD(RobotStateVisualization) MOVEIT_CLASS_FORWARD(TaskSolutionVisualization) +MOVEIT_CLASS_FORWARD(PlanningSceneRender) class TaskSolutionPanel; class TaskSolutionVisualization : public QObject @@ -111,65 +109,76 @@ public Q_SLOTS: void interruptCurrentDisplay(); private Q_SLOTS: - - /** - * \brief Slot Event Functions - */ - void changedDisplayPathVisualEnabled(); - void changedDisplayPathCollisionEnabled(); - void changedRobotPathAlpha(); + // trajectory property slots + void changedRobotVisualEnabled(); + void changedRobotCollisionEnabled(); + void changedRobotAlpha(); void changedLoopDisplay(); void changedShowTrail(); void changedTrailStepSize(); void changedStateDisplayTime(); void changedRobotColor(); void enabledRobotColor(); - void trajectorySliderPanelVisibilityChange(bool enable); + void changedAttachedBodyColor(); + void sliderPanelVisibilityChange(bool enable); + + // planning scene property slots + void changedSceneEnabled(); + void renderCurrentScene(); protected: float getStateDisplayTime(); - void clearTrajectoryTrail(); + void clearTrail(); + void renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene); - // Handles actually drawing the robot along motion plans - RobotStateVisualizationPtr display_path_robot_; + // render the planning scene + PlanningSceneRenderPtr scene_render_; + // render the robot + RobotStateVisualizationPtr robot_render_; // Handle colouring of robot void setRobotColor(rviz::Robot* robot, const QColor& color); void unsetRobotColor(rviz::Robot* robot); - robot_trajectory::RobotTrajectoryPtr displaying_trajectory_message_; - robot_trajectory::RobotTrajectoryPtr trajectory_message_to_display_; - std::vector trajectory_trail_; - bool animating_path_ = false; - bool drop_displaying_trajectory_ = false; + robot_trajectory::RobotTrajectoryPtr displaying_solution_; + robot_trajectory::RobotTrajectoryPtr solution_to_display_; + std::vector trail_; + bool animating_ = false; + bool drop_displaying_solution_ = false; int current_state_ = -1; float current_state_time_; - boost::mutex update_trajectory_message_; + boost::mutex display_solution_mutex_; - moveit::core::RobotModelConstPtr robot_model_; - moveit::core::RobotStatePtr robot_state_; + planning_scene::PlanningScenePtr scene_; - // Pointers from parent display taht we save + // Pointers from parent display that we save rviz::Display* display_; // the parent display that this class populates - rviz::Property* parent_; Ogre::SceneNode* scene_node_; rviz::DisplayContext* context_; - TaskSolutionPanel* trajectory_slider_panel_ = nullptr; - rviz::PanelDockWidget* trajectory_slider_dock_panel_ = nullptr; + TaskSolutionPanel* slider_panel_ = nullptr; + rviz::PanelDockWidget* slider_dock_panel_ = nullptr; + + // Trajectory Properties + rviz::Property* robot_property_; + rviz::BoolProperty* robot_visual_enabled_property_; + rviz::BoolProperty* robot_collision_enabled_property_; + rviz::FloatProperty* robot_alpha_property_; + rviz::ColorProperty* robot_color_property_; + rviz::BoolProperty* enable_robot_color_property_; - // Properties - rviz::BoolProperty* display_path_visual_enabled_property_; - rviz::BoolProperty* display_path_collision_enabled_property_; rviz::EditableEnumProperty* state_display_time_property_; - rviz::FloatProperty* robot_path_alpha_property_; rviz::BoolProperty* loop_display_property_; rviz::BoolProperty* trail_display_property_; rviz::BoolProperty* interrupt_display_property_; - rviz::ColorProperty* robot_color_property_; - rviz::BoolProperty* enable_robot_color_property_; rviz::IntProperty* trail_step_size_property_; + + // PlanningScene Properties + rviz::BoolProperty* scene_enabled_property_; + rviz::FloatProperty* scene_alpha_property_; + rviz::ColorProperty* scene_color_property_; + rviz::ColorProperty* attached_body_color_property_; + rviz::EnumProperty* octree_render_property_; + rviz::EnumProperty* octree_coloring_property_; }; } // namespace moveit_rviz_plugin - -#endif diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 12217898..b6d30ff0 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -34,13 +34,13 @@ /* Author: Dave Coleman, Robert Haschke */ +#include #include #include - +#include #include #include -#include #include #include @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -65,22 +66,19 @@ namespace moveit_rviz_plugin { TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display) : display_(display) - , parent_(parent) { - 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", - parent, SLOT(changedDisplayPathVisualEnabled()), this); + // trajectory properties + interrupt_display_property_ = new rviz::BoolProperty("Interrupt Display", false, + "Immediately show newly planned trajectory, " + "interrupting the currently displayed one.", + parent); - 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", - parent, SLOT(changedDisplayPathCollisionEnabled()), this); + loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, + "Indicates whether the last received path is to be animated in a loop", + parent, SLOT(changedLoopDisplay()), this); - robot_path_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", - parent, SLOT(changedRobotPathAlpha()), this); - robot_path_alpha_property_->setMin(0.0); - robot_path_alpha_property_->setMax(1.0); + trail_display_property_ = new rviz::BoolProperty("Show Trail", false, "Show a path trail", + parent, SLOT(changedShowTrail()), this); state_display_time_property_ = new rviz::EditableEnumProperty("State Display Time", "0.05 s", "The amount of wall-time to wait in between displaying " @@ -91,38 +89,78 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi state_display_time_property_->addOptionStd("0.1 s"); state_display_time_property_->addOptionStd("0.5 s"); - loop_display_property_ = new rviz::BoolProperty("Loop Animation", false, "Indicates whether the last received path " - "is to be animated in a loop", - parent, SLOT(changedLoopDisplay()), this); - - trail_display_property_ = - 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.", + trail_step_size_property_ = new rviz::IntProperty("Trail Step Size", 1, + "Specifies the step size of the samples shown in the trajectory trail.", 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.", parent); - robot_color_property_ = new rviz::ColorProperty( - "Robot Color", QColor(150, 50, 150), "The color of the animated robot", parent, SLOT(changedRobotColor()), this); + // robot properties + robot_property_ = new rviz::Property("Robot", QString(), QString(), parent); + robot_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", + robot_property_, SLOT(changedRobotVisualEnabled()), this); - enable_robot_color_property_ = new rviz::BoolProperty( - "Color Enabled", false, "Specifies whether robot coloring is enabled", parent, SLOT(enabledRobotColor()), this); + robot_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", + robot_property_, SLOT(changedRobotCollisionEnabled()), this); + + robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", + robot_property_, SLOT(changedRobotAlpha()), this); + robot_alpha_property_->setMin(0.0); + robot_alpha_property_->setMax(1.0); + + robot_color_property_ = new rviz::ColorProperty("Fixed Robot Color", QColor(150, 50, 150), "The color of the animated robot", + robot_property_, SLOT(changedRobotColor()), this); + + enable_robot_color_property_ = new rviz::BoolProperty("Use Fixed Robot Color", false, + "Specifies whether the fixed robot color should be used." + " If not, the original color is used.", + robot_property_, SLOT(enabledRobotColor()), this); + + + // planning scene properties + scene_enabled_property_ = new rviz::BoolProperty("Scene", true, "Show Planning Scene", parent, + SLOT(changedSceneEnabled()), this); + + scene_alpha_property_ = new rviz::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry", + scene_enabled_property_, SLOT(renderCurrentScene()), this); + scene_alpha_property_->setMin(0.0); + scene_alpha_property_->setMax(1.0); + + scene_color_property_ = new rviz::ColorProperty("Scene Color", QColor(50, 230, 50), + "The color for the planning scene obstacles (if a color is not defined)", + scene_enabled_property_, SLOT(renderCurrentScene()), this); + + attached_body_color_property_ = new rviz::ColorProperty("Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies", + scene_enabled_property_, SLOT(changedAttachedBodyColor()), this); + + octree_render_property_ = new rviz::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.", + scene_enabled_property_, SLOT(renderCurrentScene()), this); + + octree_render_property_->addOption("Occupied Voxels", OCTOMAP_OCCUPIED_VOXELS); + octree_render_property_->addOption("Free Voxels", OCTOMAP_FREE_VOXELS); + octree_render_property_->addOption("All Voxels", OCTOMAP_FREE_VOXELS | OCTOMAP_OCCUPIED_VOXELS); + + octree_coloring_property_ = new rviz::EnumProperty("Voxel Coloring", "Z-Axis", "Select voxel coloring mode", + scene_enabled_property_, SLOT(renderCurrentScene()), this); + + octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR); + octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR); } TaskSolutionVisualization::~TaskSolutionVisualization() { - clearTrajectoryTrail(); - trajectory_message_to_display_.reset(); - displaying_trajectory_message_.reset(); + clearTrail(); + solution_to_display_.reset(); + displaying_solution_.reset(); - display_path_robot_.reset(); - if (trajectory_slider_dock_panel_) - delete trajectory_slider_dock_panel_; + scene_render_.reset(); + robot_render_.reset(); + if (slider_dock_panel_) + delete slider_dock_panel_; } void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context) @@ -132,104 +170,102 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz:: context_ = context; // Load trajectory robot - display_path_robot_.reset(new RobotStateVisualization(scene_node_, context_, "Solution Trajectory", 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); + robot_render_.reset(new RobotStateVisualization(scene_node_, context_, "Solution Trajectory", robot_property_)); + robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); + robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); + robot_render_->setVisible(false); + + scene_render_.reset(new PlanningSceneRender(scene_node_, context_, robot_render_)); + scene_render_->getGeometryNode()->setVisible(false); rviz::WindowManagerInterface* window_context = context_->getWindowManager(); if (window_context) { - 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()); - connect(trajectory_slider_dock_panel_, SIGNAL(visibilityChanged(bool)), this, - SLOT(trajectorySliderPanelVisibilityChange(bool))); - trajectory_slider_panel_->onInitialize(); + slider_panel_ = new TaskSolutionPanel(window_context->getParentWindow()); + slider_dock_panel_ = + window_context->addPane(display_->getName() + " - Slider", slider_panel_); + slider_dock_panel_->setIcon(display_->getIcon()); + connect(slider_dock_panel_, SIGNAL(visibilityChanged(bool)), this, + SLOT(sliderPanelVisibilityChange(bool))); + slider_panel_->onInitialize(); } } void TaskSolutionVisualization::setName(const QString& name) { - if (trajectory_slider_dock_panel_) - trajectory_slider_dock_panel_->setWindowTitle(name + " - Slider"); + if (slider_dock_panel_) + slider_dock_panel_->setWindowTitle(name + " - Slider"); } void TaskSolutionVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model) { - robot_model_ = robot_model; - // Error check - if (!robot_model_) + if (!robot_model) { ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found"); return; } - // Load robot state - robot_state_.reset(new robot_state::RobotState(robot_model_)); - robot_state_->setToDefaultValues(); + scene_.reset(new planning_scene::PlanningScene(robot_model)); - // Load rviz robot - display_path_robot_->load(*robot_model_->getURDF()); + robot_render_->load(*robot_model->getURDF()); // load rviz robot enabledRobotColor(); // force-refresh to account for saved display configuration } void TaskSolutionVisualization::reset() { - clearTrajectoryTrail(); - trajectory_message_to_display_.reset(); - displaying_trajectory_message_.reset(); - animating_path_ = false; + clearTrail(); + solution_to_display_.reset(); + displaying_solution_.reset(); + animating_ = false; - 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); + robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); + robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); + robot_render_->setVisible(false); } -void TaskSolutionVisualization::clearTrajectoryTrail() +void TaskSolutionVisualization::clearTrail() { - for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) - delete trajectory_trail_[i]; - trajectory_trail_.clear(); + for (std::size_t i = 0; i < trail_.size(); ++i) + delete trail_[i]; + trail_.clear(); } 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); + robot_render_->setVisible(display_->isEnabled() && displaying_solution_ && animating_); + if (loop_display_property_->getBool() && slider_panel_) + slider_panel_->pauseButton(false); } void TaskSolutionVisualization::changedShowTrail() { - clearTrajectoryTrail(); + clearTrail(); if (!trail_display_property_->getBool()) return; - robot_trajectory::RobotTrajectoryPtr t = trajectory_message_to_display_; + robot_trajectory::RobotTrajectoryPtr t = solution_to_display_; if (!t) - t = displaying_trajectory_message_; + t = displaying_solution_; if (!t) return; int stepsize = trail_step_size_property_->getInt(); // always include last trajectory point - trajectory_trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize)); - for (std::size_t i = 0; i < trajectory_trail_.size(); i++) + trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize)); + for (std::size_t i = 0; i < trail_.size(); i++) { int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point rviz::Robot* r = new rviz::Robot(scene_node_, context_, "Trail Robot " + boost::lexical_cast(i), NULL); - r->load(*robot_model_->getURDF()); - r->setVisualVisible(display_path_visual_enabled_property_->getBool()); - r->setCollisionVisible(display_path_collision_enabled_property_->getBool()); - r->setAlpha(robot_path_alpha_property_->getFloat()); + r->load(*scene_->getRobotModel()->getURDF()); + r->setVisualVisible(robot_visual_enabled_property_->getBool()); + r->setCollisionVisible(robot_collision_enabled_property_->getBool()); + r->setAlpha(robot_alpha_property_->getFloat()); r->update(PlanningLinkUpdater(t->getWayPointPtr(waypoint_i))); if (enable_robot_color_property_->getBool()) setRobotColor(r, robot_color_property_->getColor()); - r->setVisible(display_->isEnabled() && (!animating_path_ || waypoint_i <= current_state_)); - trajectory_trail_[i] = r; + r->setVisible(display_->isEnabled() && (!animating_ || waypoint_i <= current_state_)); + trail_[i] = r; } } @@ -239,21 +275,32 @@ void TaskSolutionVisualization::changedTrailStepSize() changedShowTrail(); } -void TaskSolutionVisualization::changedRobotPathAlpha() +void TaskSolutionVisualization::changedRobotAlpha() { - 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()); + robot_render_->setAlpha(robot_alpha_property_->getFloat()); + for (std::size_t i = 0; i < trail_.size(); ++i) + trail_[i]->setAlpha(robot_alpha_property_->getFloat()); } -void TaskSolutionVisualization::changedDisplayPathVisualEnabled() +void TaskSolutionVisualization::changedRobotVisualEnabled() { if (display_->isEnabled()) { - display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool()); - display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_); - for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) - trajectory_trail_[i]->setVisualVisible(display_path_visual_enabled_property_->getBool()); + robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); + robot_render_->setVisible(display_->isEnabled() && displaying_solution_ && animating_); + for (std::size_t i = 0; i < trail_.size(); ++i) + trail_[i]->setVisualVisible(robot_visual_enabled_property_->getBool()); + } +} + +void TaskSolutionVisualization::changedRobotCollisionEnabled() +{ + if (display_->isEnabled()) + { + robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); + robot_render_->setVisible(display_->isEnabled() && displaying_solution_ && animating_); + for (std::size_t i = 0; i < trail_.size(); ++i) + trail_[i]->setCollisionVisible(robot_collision_enabled_property_->getBool()); } } @@ -261,41 +308,35 @@ void TaskSolutionVisualization::changedStateDisplayTime() { } -void TaskSolutionVisualization::changedDisplayPathCollisionEnabled() -{ - if (display_->isEnabled()) - { - display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool()); - display_path_robot_->setVisible(display_->isEnabled() && displaying_trajectory_message_ && animating_path_); - for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) - trajectory_trail_[i]->setCollisionVisible(display_path_collision_enabled_property_->getBool()); - } -} - void TaskSolutionVisualization::onEnable() { - changedRobotPathAlpha(); // set alpha property + changedRobotAlpha(); // set alpha property + changedSceneEnabled(); // show/hide planning scene - display_path_robot_->setVisualVisible(display_path_visual_enabled_property_->getBool()); - display_path_robot_->setCollisionVisible(display_path_collision_enabled_property_->getBool()); - display_path_robot_->setVisible(displaying_trajectory_message_ && animating_path_); - for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) + robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); + robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); + robot_render_->setVisible(displaying_solution_ && animating_); + for (std::size_t i = 0; i < trail_.size(); ++i) { - trajectory_trail_[i]->setVisualVisible(display_path_visual_enabled_property_->getBool()); - trajectory_trail_[i]->setCollisionVisible(display_path_collision_enabled_property_->getBool()); - trajectory_trail_[i]->setVisible(true); + trail_[i]->setVisualVisible(robot_visual_enabled_property_->getBool()); + trail_[i]->setCollisionVisible(robot_collision_enabled_property_->getBool()); + trail_[i]->setVisible(true); } } void TaskSolutionVisualization::onDisable() { - display_path_robot_->setVisible(false); - for (std::size_t i = 0; i < trajectory_trail_.size(); ++i) - trajectory_trail_[i]->setVisible(false); - displaying_trajectory_message_.reset(); - animating_path_ = false; - if (trajectory_slider_panel_) - trajectory_slider_panel_->onDisable(); + if (scene_render_) + scene_render_->getGeometryNode()->setVisible(false); + + robot_render_->setVisible(false); + for (std::size_t i = 0; i < trail_.size(); ++i) + trail_[i]->setVisible(false); + + displaying_solution_.reset(); + animating_ = false; + if (slider_panel_) + slider_panel_->onDisable(); } void TaskSolutionVisualization::interruptCurrentDisplay() @@ -304,7 +345,7 @@ void TaskSolutionVisualization::interruptCurrentDisplay() // interrupting may cause the newly received trajectory to interrupt // hence, only interrupt when current_state_ already advanced past first if (current_state_ > 0) - animating_path_ = false; + animating_ = false; } float TaskSolutionVisualization::getStateDisplayTime() @@ -331,90 +372,90 @@ float TaskSolutionVisualization::getStateDisplayTime() void TaskSolutionVisualization::dropTrajectory() { - drop_displaying_trajectory_ = true; + drop_displaying_solution_ = true; } void TaskSolutionVisualization::update(float wall_dt, float ros_dt) { - if (drop_displaying_trajectory_) + if (drop_displaying_solution_) { - animating_path_ = false; - displaying_trajectory_message_.reset(); - display_path_robot_->setVisible(false); - trajectory_slider_panel_->update(0); - drop_displaying_trajectory_ = false; + animating_ = false; + displaying_solution_.reset(); + robot_render_->setVisible(false); + slider_panel_->update(0); + drop_displaying_solution_ = false; } - if (!animating_path_) + if (!animating_) { // finished last animation? - boost::mutex::scoped_lock lock(update_trajectory_message_); + boost::mutex::scoped_lock lock(display_solution_mutex_); // new trajectory available to display? - if (trajectory_message_to_display_ && !trajectory_message_to_display_->empty()) + if (solution_to_display_ && !solution_to_display_->empty()) { - animating_path_ = true; - displaying_trajectory_message_ = trajectory_message_to_display_; + animating_ = true; + displaying_solution_ = solution_to_display_; changedShowTrail(); - if (trajectory_slider_panel_) - trajectory_slider_panel_->update(trajectory_message_to_display_->getWayPointCount()); + if (slider_panel_) + slider_panel_->update(solution_to_display_->getWayPointCount()); } - else if (displaying_trajectory_message_) + else if (displaying_solution_) { if (loop_display_property_->getBool()) { // do loop? -> start over too - animating_path_ = true; + animating_ = true; } - else if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible()) + else if (slider_panel_ && slider_panel_->isVisible()) { - if (trajectory_slider_panel_->getSliderPosition() == (int)displaying_trajectory_message_->getWayPointCount() - 1) + if (slider_panel_->getSliderPosition() == (int)displaying_solution_->getWayPointCount() - 1) { // show the last waypoint if the slider is enabled - display_path_robot_->update( - displaying_trajectory_message_->getWayPointPtr(displaying_trajectory_message_->getWayPointCount() - 1)); + robot_render_->update( + displaying_solution_->getWayPointPtr(displaying_solution_->getWayPointCount() - 1)); } else - animating_path_ = true; + animating_ = true; } } - trajectory_message_to_display_.reset(); + solution_to_display_.reset(); - if (animating_path_) + if (animating_) { current_state_ = -1; current_state_time_ = std::numeric_limits::infinity(); - display_path_robot_->update(displaying_trajectory_message_->getFirstWayPointPtr()); - display_path_robot_->setVisible(display_->isEnabled()); - if (trajectory_slider_panel_) - trajectory_slider_panel_->setSliderPosition(0); + robot_render_->update(displaying_solution_->getFirstWayPointPtr()); + robot_render_->setVisible(display_->isEnabled()); + if (slider_panel_) + slider_panel_->setSliderPosition(0); } } - if (animating_path_) + if (animating_) { float tm = getStateDisplayTime(); if (tm < 0.0) // if we should use realtime - tm = displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1); + tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1); if (current_state_time_ > tm) { - if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible() && trajectory_slider_panel_->isPaused()) - current_state_ = trajectory_slider_panel_->getSliderPosition(); + if (slider_panel_ && slider_panel_->isVisible() && slider_panel_->isPaused()) + current_state_ = slider_panel_->getSliderPosition(); else ++current_state_; - int waypoint_count = displaying_trajectory_message_->getWayPointCount(); + int waypoint_count = displaying_solution_->getWayPointCount(); if (current_state_ < waypoint_count) { - if (trajectory_slider_panel_) - trajectory_slider_panel_->setSliderPosition(current_state_); - 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( + if (slider_panel_) + slider_panel_->setSliderPosition(current_state_); + robot_render_->update(displaying_solution_->getWayPointPtr(current_state_)); + for (std::size_t i = 0; i < trail_.size(); ++i) + trail_[i]->setVisible( std::min(waypoint_count - 1, static_cast(i) * trail_step_size_property_->getInt()) <= current_state_); } else { - animating_path_ = false; // animation finished - display_path_robot_->setVisible(loop_display_property_->getBool()); - if (!loop_display_property_->getBool() && trajectory_slider_panel_) - trajectory_slider_panel_->pauseButton(true); + animating_ = false; // animation finished + robot_render_->setVisible(loop_display_property_->getBool()); + if (!loop_display_property_->getBool() && slider_panel_) + slider_panel_->pauseButton(true); } current_state_time_ = 0.0f; } @@ -422,32 +463,45 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) } } +void TaskSolutionVisualization::renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene) +{ + if (!scene_render_ || !scene_enabled_property_->getBool()) + return; + + QColor color = scene_color_property_->getColor(); + rviz::Color env_color(color.redF(), color.greenF(), color.blueF()); + color = attached_body_color_property_->getColor(); + rviz::Color attached_color(color.redF(), color.greenF(), color.blueF()); + + scene_render_->renderPlanningScene(scene, env_color, attached_color, + static_cast(octree_render_property_->getOptionInt()), + static_cast(octree_coloring_property_->getOptionInt()), + scene_alpha_property_->getFloat()); +} + void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::Solution& msg) { // Error check - if (!robot_state_ || !robot_model_) - { - ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot state or robot model loaded"); + if (!scene_) return; - } - if (msg.start_scene.robot_model_name != robot_model_->getName()) + if (msg.start_scene.robot_model_name != scene_->getRobotModel()->getName()) ROS_WARN("Received a trajectory to display for model '%s' but model '%s' was expected", msg.start_scene.robot_model_name .c_str(), - robot_model_->getName().c_str()); + scene_->getRobotModel()->getName().c_str()); - trajectory_message_to_display_.reset(); + scene_->setPlanningSceneMsg(msg.start_scene); - robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(robot_model_, "")); + robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(scene_->getRobotModel(), "")); for (std::size_t i = 0; i < msg.sub_trajectory.size(); ++i) { if (t->empty()) { - t->setRobotTrajectoryMsg(*robot_state_, msg.sub_trajectory[i].trajectory); + t->setRobotTrajectoryMsg(scene_->getCurrentState(), msg.sub_trajectory[i].trajectory); } else { - robot_trajectory::RobotTrajectory tmp(robot_model_, ""); + robot_trajectory::RobotTrajectory tmp(scene_->getRobotModel(), ""); tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg.sub_trajectory[i].trajectory); t->append(tmp, 0.0); } @@ -455,8 +509,8 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::So if (!t->empty()) { - boost::mutex::scoped_lock lock(update_trajectory_message_); - trajectory_message_to_display_.swap(t); + boost::mutex::scoped_lock lock(display_solution_mutex_); + solution_to_display_.swap(t); if (interrupt_display_property_->getBool()) interruptCurrentDisplay(); } @@ -465,15 +519,20 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::So void TaskSolutionVisualization::changedRobotColor() { if (enable_robot_color_property_->getBool()) - setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor()); + setRobotColor(&(robot_render_->getRobot()), robot_color_property_->getColor()); } void TaskSolutionVisualization::enabledRobotColor() { if (enable_robot_color_property_->getBool()) - setRobotColor(&(display_path_robot_->getRobot()), robot_color_property_->getColor()); + setRobotColor(&(robot_render_->getRobot()), robot_color_property_->getColor()); else - unsetRobotColor(&(display_path_robot_->getRobot())); + unsetRobotColor(&(robot_render_->getRobot())); +} + +void TaskSolutionVisualization::changedAttachedBodyColor() +{ + } void TaskSolutionVisualization::unsetRobotColor(rviz::Robot* robot) @@ -485,18 +544,29 @@ void TaskSolutionVisualization::unsetRobotColor(rviz::Robot* robot) 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()); + link.second->setColor(color.redF(), color.greenF(), color.blueF()); } -void TaskSolutionVisualization::trajectorySliderPanelVisibilityChange(bool enable) +void TaskSolutionVisualization::sliderPanelVisibilityChange(bool enable) { - if (!trajectory_slider_panel_) + if (!slider_panel_) return; if (enable) - trajectory_slider_panel_->onEnable(); + slider_panel_->onEnable(); else - trajectory_slider_panel_->onDisable(); + slider_panel_->onDisable(); +} + + +void TaskSolutionVisualization::changedSceneEnabled() +{ + if (scene_render_) + scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool()); +} + +void TaskSolutionVisualization::renderCurrentScene() +{ } } // namespace moveit_rviz_plugin