fixed visibility update of scene nodes

All scene nodes are hidden if
- there is no solution to display
- or if animation finished and neither the panel nor the trail are enabled

Hiding scene nodes is done by detaching them from their parent
This commit is contained in:
Robert Haschke 2017-11-14 15:53:46 +01:00
parent d5627c0c39
commit 428bdd7d0c
3 changed files with 71 additions and 38 deletions

View File

@ -34,8 +34,7 @@
/* Author: Yannick Jonetzko */ /* Author: Yannick Jonetzko */
#ifndef MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TASK_SOLUTION_PANEL_ #pragma once
#define MOVEIT_TRAJECTORY_RVIZ_PLUGIN_TASK_SOLUTION_PANEL_
#ifndef Q_MOC_RUN #ifndef Q_MOC_RUN
#include <ros/ros.h> #include <ros/ros.h>
@ -93,5 +92,3 @@ protected:
}; };
} // namespace moveit_rviz_plugin } // namespace moveit_rviz_plugin
#endif

View File

@ -126,6 +126,7 @@ private Q_SLOTS:
void renderCurrentScene(); void renderCurrentScene();
protected: protected:
void setVisibility(Ogre::SceneNode* node, Ogre::SceneNode* parent, bool visible);
float getStateDisplayTime(); float getStateDisplayTime();
void clearTrail(); void clearTrail();
void renderWayPoint(size_t index, int previous_index); void renderWayPoint(size_t index, int previous_index);
@ -153,7 +154,9 @@ protected:
// Pointers from parent display that we save // Pointers from parent display that we save
rviz::Display* display_; // the parent display that this class populates 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_; rviz::DisplayContext* context_;
TaskSolutionPanel* slider_panel_ = nullptr; TaskSolutionPanel* slider_panel_ = nullptr;
rviz::PanelDockWidget* slider_dock_panel_ = nullptr; rviz::PanelDockWidget* slider_dock_panel_ = nullptr;

View File

@ -169,20 +169,21 @@ TaskSolutionVisualization::~TaskSolutionVisualization()
void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context) void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context)
{ {
// Save pointers for later use // Save pointers for later use
scene_node_ = scene_node; parent_scene_node_ = scene_node;
context_ = context; 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 // 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_->setVisualVisible(robot_visual_enabled_property_->getBool());
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
changedRobotAlpha(); changedRobotAlpha();
enabledRobotColor(); enabledRobotColor();
robot_render_->setVisible(true); robot_render_->setVisible(true);
scene_render_.reset(new PlanningSceneRender(scene_node_, context_, RobotStateVisualizationPtr())); scene_render_.reset(new PlanningSceneRender(main_scene_node_, context_, RobotStateVisualizationPtr()));
scene_render_->getGeometryNode()->setVisible(true); scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());
rviz::WindowManagerInterface* window_context = context_->getWindowManager(); rviz::WindowManagerInterface* window_context = context_->getWindowManager();
if (window_context) if (window_context)
@ -227,7 +228,10 @@ void TaskSolutionVisualization::reset()
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
robot_render_->setCollisionVisible(robot_collision_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() void TaskSolutionVisualization::clearTrail()
@ -238,7 +242,6 @@ void TaskSolutionVisualization::clearTrail()
void TaskSolutionVisualization::changedLoopDisplay() void TaskSolutionVisualization::changedLoopDisplay()
{ {
scene_node_->setVisible(display_->isEnabled() && displaying_solution_ && animating_);
if (loop_display_property_->getBool() && slider_panel_) if (loop_display_property_->getBool() && slider_panel_)
slider_panel_->pauseButton(false); slider_panel_->pauseButton(false);
} }
@ -246,14 +249,17 @@ void TaskSolutionVisualization::changedLoopDisplay()
void TaskSolutionVisualization::changedTrail() void TaskSolutionVisualization::changedTrail()
{ {
clearTrail(); clearTrail();
if (!trail_display_property_->getBool())
return;
DisplaySolutionPtr t = solution_to_display_; DisplaySolutionPtr t = solution_to_display_;
if (!t) if (!t)
t = displaying_solution_; t = displaying_solution_;
if (!t)
if (!t || !trail_display_property_->getBool()) {
setVisibility(trail_scene_node_, main_scene_node_, false);
return; return;
}
setVisibility(main_scene_node_, parent_scene_node_, true);
setVisibility(trail_scene_node_, main_scene_node_, true);
int stepsize = trail_step_size_property_->getInt(); int stepsize = trail_step_size_property_->getInt();
// always include last trajectory point // always include last trajectory point
@ -261,7 +267,7 @@ void TaskSolutionVisualization::changedTrail()
for (std::size_t i = 0; i < trail_.size(); i++) 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 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<std::string>(i), NULL); rviz::Robot* r = new rviz::Robot(trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast<std::string>(i), NULL);
r->load(*scene_->getRobotModel()->getURDF()); r->load(*scene_->getRobotModel()->getURDF());
r->setVisualVisible(robot_visual_enabled_property_->getBool()); r->setVisualVisible(robot_visual_enabled_property_->getBool());
r->setCollisionVisible(robot_collision_enabled_property_->getBool()); r->setCollisionVisible(robot_collision_enabled_property_->getBool());
@ -297,12 +303,13 @@ void TaskSolutionVisualization::changedRobotCollisionEnabled()
void TaskSolutionVisualization::onEnable() void TaskSolutionVisualization::onEnable()
{ {
scene_node_->setVisible(displaying_solution_ && animating_);
} }
void TaskSolutionVisualization::onDisable() 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(); displaying_solution_.reset();
animating_ = false; animating_ = false;
@ -352,12 +359,11 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
{ {
animating_ = false; animating_ = false;
displaying_solution_.reset(); displaying_solution_.reset();
scene_node_->setVisible(false);
slider_panel_->update(0); slider_panel_->update(0);
drop_displaying_solution_ = false; drop_displaying_solution_ = false;
} }
if (!animating_) if (!animating_)
{ // finished last animation? { // finished last animation
boost::mutex::scoped_lock lock(display_solution_mutex_); boost::mutex::scoped_lock lock(display_solution_mutex_);
// new trajectory available to display? // new trajectory available to display?
@ -371,11 +377,9 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
} }
else if (displaying_solution_) else if (displaying_solution_)
{ {
if (loop_display_property_->getBool()) if (loop_display_property_->getBool()) {
{ // do loop? -> start over too
animating_ = true; 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) if (slider_panel_->getSliderPosition() == (int)displaying_solution_->getWayPointCount() - 1)
return; // nothing more to do return; // nothing more to do
@ -387,9 +391,10 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
if (animating_) if (animating_)
{ {
// restart animation
current_state_ = -1; current_state_ = -1;
current_state_time_ = std::numeric_limits<float>::infinity(); current_state_time_ = std::numeric_limits<float>::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) if (current_state_ < waypoint_count)
{ {
renderWayPoint(current_state_, previous_state); renderWayPoint(current_state_, previous_state);
for (std::size_t i = 0; i < trail_.size(); ++i)
trail_[i]->setVisible( int stepsize = trail_step_size_property_->getInt();
std::min(waypoint_count - 1, for (int i = std::max(0, previous_state / stepsize),
static_cast<int>(i) * trail_step_size_property_->getInt()) <= current_state_); end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i)
trail_[i]->setVisible(true);
} }
else else
{ {
animating_ = false; // animation finished animating_ = false; // animation finished
scene_node_->setVisible(loop_display_property_->getBool() ||
(slider_panel_ && slider_panel_->isVisible()));
if (!loop_display_property_->getBool() && slider_panel_) if (!loop_display_property_->getBool() && slider_panel_)
slider_panel_->pauseButton(true); slider_panel_->pauseButton(true);
// ensure to render end state // ensure to render end state
@ -431,6 +435,12 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
} }
current_state_time_ += wall_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) void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
@ -534,25 +544,48 @@ void TaskSolutionVisualization::sliderPanelVisibilityChange(bool enable)
if (!slider_panel_) if (!slider_panel_)
return; return;
if (enable) if (enable) {
// also enable display
display_->setEnabled(true);
slider_panel_->onEnable(); slider_panel_->onEnable();
else } else
slider_panel_->onDisable(); slider_panel_->onDisable();
scene_node_->setVisible(enable || animating_); setVisibility(main_scene_node_, parent_scene_node_,
display_->isEnabled() && displaying_solution_ && enable);
} }
void TaskSolutionVisualization::changedSceneEnabled() void TaskSolutionVisualization::changedSceneEnabled()
{ {
if (scene_render_) if (!scene_render_)
scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool()); return;
setVisibility(scene_render_->getGeometryNode(), main_scene_node_,
scene_enabled_property_->getBool());
} }
void TaskSolutionVisualization::renderCurrentScene() void TaskSolutionVisualization::renderCurrentScene()
{ {
if (scene_render_ && scene_enabled_property_->getBool() && current_state_ >= 0) 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 } // namespace moveit_rviz_plugin