mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
d5627c0c39
commit
428bdd7d0c
@ -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
|
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user