diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h index 4f0d691f..cbd86807 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h @@ -34,8 +34,7 @@ /* Author: Yannick Jonetzko */ -#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TASK_SOLUTION_PANEL_ -#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TASK_SOLUTION_PANEL_ +#pragma once #ifndef Q_MOC_RUN #include @@ -93,5 +92,3 @@ protected: }; } // namespace moveit_rviz_plugin - -#endif diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index 3e57940f..6a2f9e88 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -126,6 +126,7 @@ private Q_SLOTS: void renderCurrentScene(); protected: + void setVisibility(Ogre::SceneNode* node, Ogre::SceneNode* parent, bool visible); float getStateDisplayTime(); void clearTrail(); void renderWayPoint(size_t index, int previous_index); @@ -153,7 +154,9 @@ protected: // Pointers from parent display that we save rviz::Display* display_; // the parent display that this class populates - Ogre::SceneNode* scene_node_; + Ogre::SceneNode* parent_scene_node_; // parent scene node provided by display + Ogre::SceneNode* main_scene_node_; // to be added/removed to/from scene_node_ + Ogre::SceneNode* trail_scene_node_; // to be added/removed to/from scene_node_ rviz::DisplayContext* context_; TaskSolutionPanel* slider_panel_ = nullptr; rviz::PanelDockWidget* slider_dock_panel_ = nullptr; diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 8c5297df..c1194b3e 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -169,20 +169,21 @@ TaskSolutionVisualization::~TaskSolutionVisualization() void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context) { // Save pointers for later use - scene_node_ = scene_node; + parent_scene_node_ = scene_node; context_ = context; - scene_node_->setVisible(false); + main_scene_node_ = parent_scene_node_->getCreator()->createSceneNode(); + trail_scene_node_ = parent_scene_node_->getCreator()->createSceneNode(); // Load trajectory robot - robot_render_.reset(new RobotStateVisualization(scene_node_, context_, "Solution Trajectory", robot_property_)); + robot_render_.reset(new RobotStateVisualization(main_scene_node_, context_, "Solution Trajectory", robot_property_)); robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); changedRobotAlpha(); enabledRobotColor(); robot_render_->setVisible(true); - scene_render_.reset(new PlanningSceneRender(scene_node_, context_, RobotStateVisualizationPtr())); - scene_render_->getGeometryNode()->setVisible(true); + scene_render_.reset(new PlanningSceneRender(main_scene_node_, context_, RobotStateVisualizationPtr())); + scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool()); rviz::WindowManagerInterface* window_context = context_->getWindowManager(); if (window_context) @@ -227,7 +228,10 @@ void TaskSolutionVisualization::reset() robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); - scene_node_->setVisible(false); + scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool()); + + if (main_scene_node_->getParent()) + parent_scene_node_->removeChild(main_scene_node_); } void TaskSolutionVisualization::clearTrail() @@ -238,7 +242,6 @@ void TaskSolutionVisualization::clearTrail() void TaskSolutionVisualization::changedLoopDisplay() { - scene_node_->setVisible(display_->isEnabled() && displaying_solution_ && animating_); if (loop_display_property_->getBool() && slider_panel_) slider_panel_->pauseButton(false); } @@ -246,14 +249,17 @@ void TaskSolutionVisualization::changedLoopDisplay() void TaskSolutionVisualization::changedTrail() { clearTrail(); - - if (!trail_display_property_->getBool()) - return; DisplaySolutionPtr t = solution_to_display_; if (!t) t = displaying_solution_; - if (!t) + + if (!t || !trail_display_property_->getBool()) { + setVisibility(trail_scene_node_, main_scene_node_, false); return; + } + + setVisibility(main_scene_node_, parent_scene_node_, true); + setVisibility(trail_scene_node_, main_scene_node_, true); int stepsize = trail_step_size_property_->getInt(); // always include last trajectory point @@ -261,7 +267,7 @@ void TaskSolutionVisualization::changedTrail() 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); + rviz::Robot* r = new rviz::Robot(trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast(i), NULL); r->load(*scene_->getRobotModel()->getURDF()); r->setVisualVisible(robot_visual_enabled_property_->getBool()); r->setCollisionVisible(robot_collision_enabled_property_->getBool()); @@ -297,12 +303,13 @@ void TaskSolutionVisualization::changedRobotCollisionEnabled() void TaskSolutionVisualization::onEnable() { - scene_node_->setVisible(displaying_solution_ && animating_); } void TaskSolutionVisualization::onDisable() { - scene_node_->setVisible(false); + // make all scene nodes invisible + if (main_scene_node_->getParent()) + parent_scene_node_->removeChild(main_scene_node_); displaying_solution_.reset(); animating_ = false; @@ -352,12 +359,11 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) { animating_ = false; displaying_solution_.reset(); - scene_node_->setVisible(false); slider_panel_->update(0); drop_displaying_solution_ = false; } if (!animating_) - { // finished last animation? + { // finished last animation boost::mutex::scoped_lock lock(display_solution_mutex_); // new trajectory available to display? @@ -371,11 +377,9 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) } else if (displaying_solution_) { - if (loop_display_property_->getBool()) - { // do loop? -> start over too + if (loop_display_property_->getBool()) { animating_ = true; - } - else if (slider_panel_ && slider_panel_->isVisible()) + } else if (slider_panel_ && slider_panel_->isVisible()) { if (slider_panel_->getSliderPosition() == (int)displaying_solution_->getWayPointCount() - 1) return; // nothing more to do @@ -387,9 +391,10 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) if (animating_) { + // restart animation current_state_ = -1; current_state_time_ = std::numeric_limits::infinity(); - scene_node_->setVisible(display_->isEnabled()); + trail_scene_node_->setVisible(false); } } @@ -411,16 +416,15 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) if (current_state_ < waypoint_count) { renderWayPoint(current_state_, previous_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_); + + int stepsize = trail_step_size_property_->getInt(); + for (int i = std::max(0, previous_state / stepsize), + end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) + trail_[i]->setVisible(true); } else { animating_ = false; // animation finished - scene_node_->setVisible(loop_display_property_->getBool() || - (slider_panel_ && slider_panel_->isVisible())); if (!loop_display_property_->getBool() && slider_panel_) slider_panel_->pauseButton(true); // ensure to render end state @@ -431,6 +435,12 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) } current_state_time_ += wall_dt; } + + // main scene node is visible if: animation, trail, or panel is shown + setVisibility(main_scene_node_, parent_scene_node_, + display_->isEnabled() && displaying_solution_ && + (animating_ || trail_scene_node_->getParent() || + (slider_panel_ && slider_panel_->isVisible()))); } void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) @@ -534,25 +544,48 @@ void TaskSolutionVisualization::sliderPanelVisibilityChange(bool enable) if (!slider_panel_) return; - if (enable) + if (enable) { + // also enable display + display_->setEnabled(true); slider_panel_->onEnable(); - else + } else slider_panel_->onDisable(); - scene_node_->setVisible(enable || animating_); + setVisibility(main_scene_node_, parent_scene_node_, + display_->isEnabled() && displaying_solution_ && enable); } void TaskSolutionVisualization::changedSceneEnabled() { - if (scene_render_) - scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool()); + if (!scene_render_) + return; + setVisibility(scene_render_->getGeometryNode(), main_scene_node_, + scene_enabled_property_->getBool()); } void TaskSolutionVisualization::renderCurrentScene() { if (scene_render_ && scene_enabled_property_->getBool() && current_state_ >= 0) - renderPlanningScene(displaying_solution_->scene(current_state_)); + renderPlanningScene(displaying_solution_->scene(current_state_)); +} + +void TaskSolutionVisualization::setVisibility(Ogre::SceneNode *node, Ogre::SceneNode *parent, bool visible) +{ + if (node != main_scene_node_ && !main_scene_node_->getParent()) + return; // main scene node is detached + + if (visible && node->getParent() != parent) { + parent->addChild(node); + // if main scene node became attached, also update visibility of other nodes + if (node == main_scene_node_) { + if (scene_render_) + setVisibility(scene_render_->getGeometryNode(), main_scene_node_, + scene_enabled_property_->getBool()); + setVisibility(trail_scene_node_, main_scene_node_, loop_display_property_->getBool()); + } + } else if (!visible && node->getParent()) + node->getParent()->removeChild(node); } } // namespace moveit_rviz_plugin