task_solution_visualization.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Robert Haschke */

#include <moveit/visualization_tools/display_solution.h>
#include <moveit/visualization_tools/task_solution_visualization.h>
#include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/visualization_tools/task_solution_panel.h>

#include <moveit/rviz_plugin_render_tools/planning_scene_render.h>
#include <moveit/rviz_plugin_render_tools/robot_state_visualization.h>
#include <moveit/rviz_plugin_render_tools/planning_link_updater.h>

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/trajectory_tools.h>

#include <rviz/robot/robot.h>
#include <rviz/robot/robot_link.h>
#include <rviz/properties/property.h>
#include <rviz/properties/int_property.h>
#include <rviz/properties/string_property.h>
#include <rviz/properties/bool_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/enum_property.h>
#include <rviz/properties/editable_enum_property.h>
#include <rviz/properties/color_property.h>
#include <rviz/display.h>
#include <rviz/display_context.h>
#include <rviz/window_manager_interface.h>
#include <rviz/panel_dock_widget.h>

#include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string/trim.hpp>

namespace moveit_rviz_plugin
{
TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display)
  : display_(display)
{
  // trajectory properties
  interrupt_display_property_ = new rviz::BoolProperty("Interrupt Display", false,
                                                       "Immediately show newly planned trajectory, "
                                                       "interrupting the currently displayed one.",
                                                       parent);

  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(changedTrail()), this);

  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",
                                                                parent);
  state_display_time_property_->addOptionStd("REALTIME");
  state_display_time_property_->addOptionStd("0.05 s");
  state_display_time_property_->addOptionStd("0.1 s");
  state_display_time_property_->addOptionStd("0.5 s");

  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(changedTrail()), this);
  trail_step_size_property_->setMin(1);


  // 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);

  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);

  marker_visual_ = new MarkerVisualizationProperty("Markers", parent);
  connect(marker_visual_, SIGNAL(allAtOnceChanged(bool)), this, SLOT(onAllAtOnceChanged(bool)));
}

TaskSolutionVisualization::~TaskSolutionVisualization()
{
  clearTrail();
  next_solution_to_display_.reset();
  displaying_solution_.reset();

  scene_render_.reset();
  robot_render_.reset();
  if (slider_dock_panel_)
    delete slider_dock_panel_;

  delete marker_visual_;

  if (main_scene_node_)
    main_scene_node_->getCreator()->destroySceneNode(main_scene_node_);
}

void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context)
{
  // Save pointers for later use
  parent_scene_node_ = scene_node;
  context_ = context;
  main_scene_node_ = parent_scene_node_->getCreator()->createSceneNode();
  trail_scene_node_ = parent_scene_node_->getCreator()->createSceneNode();

  // Load trajectory robot
  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(main_scene_node_, context_, RobotStateVisualizationPtr()));
  scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());

  marker_visual_->onInitialize(main_scene_node_, context_);

  rviz::WindowManagerInterface* window_context = context_->getWindowManager();
  if (window_context)
  {
    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 (slider_dock_panel_)
    slider_dock_panel_->setWindowTitle(name + " - Slider");
}

void TaskSolutionVisualization::onRobotModelLoaded(robot_model::RobotModelConstPtr robot_model)
{
  // Error check
  if (!robot_model)
  {
    ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found");
    return;
  }

  scene_.reset(new planning_scene::PlanningScene(robot_model));

  robot_render_->load(*robot_model->getURDF());  // load rviz robot
  enabledRobotColor();  // force-refresh to account for saved display configuration
}

void TaskSolutionVisualization::reset()
{
  clearTrail();
  next_solution_to_display_.reset();
  displaying_solution_.reset();
  current_state_ = -1;
  if (slider_panel_)
    slider_panel_->update(-1);

  robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
  robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
  scene_render_->getGeometryNode()->setVisible(scene_enabled_property_->getBool());

  if (main_scene_node_->getParent())
      parent_scene_node_->removeChild(main_scene_node_);
}

void TaskSolutionVisualization::clearTrail()
{
  qDeleteAll(trail_);
  trail_.clear();
}

void TaskSolutionVisualization::changedLoopDisplay()
{
  // restart animation if current_state_ is at end and looping got activated
  if (displaying_solution_ && loop_display_property_->getBool() &&
      slider_panel_ && slider_panel_->isVisible() &&
      current_state_+1 >= (int)displaying_solution_->getWayPointCount()) {
    current_state_ = -1;
    slider_panel_->pauseButton(false);
  }
}

void TaskSolutionVisualization::changedTrail()
{
  clearTrail();
  DisplaySolutionPtr t = displaying_solution_;

  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();
  trail_.resize(t->getWayPointCount() / 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(trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast<std::string>(i), NULL);
    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(waypoint_i <= current_state_);
    trail_[i] = r;
  }
}

void TaskSolutionVisualization::changedRobotAlpha()
{
  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::changedRobotVisualEnabled()
{
  robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
  renderCurrentWayPoint();
  for (std::size_t i = 0; i < trail_.size(); ++i)
    trail_[i]->setVisualVisible(robot_visual_enabled_property_->getBool());
}

void TaskSolutionVisualization::changedRobotCollisionEnabled()
{
  robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
  renderCurrentWayPoint();
  for (std::size_t i = 0; i < trail_.size(); ++i)
    trail_[i]->setCollisionVisible(robot_collision_enabled_property_->getBool());
}

void TaskSolutionVisualization::onEnable()
{
  if (slider_panel_ && slider_panel_was_visible_)
    slider_panel_->onEnable();
}

void TaskSolutionVisualization::onDisable()
{
  // make all scene nodes invisible
  if (main_scene_node_->getParent())
    parent_scene_node_->removeChild(main_scene_node_);

  displaying_solution_.reset();
  next_solution_to_display_.reset();
  current_state_ = -1;
  if (slider_panel_) {
    slider_panel_was_visible_ = slider_panel_->isVisible();
    slider_panel_->onDisable();
  }
}

void TaskSolutionVisualization::interruptCurrentDisplay()
{
  if (!locked_)
    drop_displaying_solution_ = true;
}

void TaskSolutionVisualization::onAllAtOnceChanged(bool all_at_once)
{
  if (!displaying_solution_)
    return;
  clearMarkers();

  if (all_at_once)
    addMarkers(displaying_solution_);
  else if (current_state_ >= 0)
    renderCurrentWayPoint();
}

float TaskSolutionVisualization::getStateDisplayTime()
{
  std::string tm = state_display_time_property_->getStdString();
  if (tm == "REALTIME")
    return -1.0;
  else
  {
    boost::replace_all(tm, "s", "");
    boost::trim(tm);
    float t = 0.05f;
    try
    {
      t = boost::lexical_cast<float>(tm);
    }
    catch (const boost::bad_lexical_cast& ex)
    {
      state_display_time_property_->setStdString("0.05 s");
    }
    return t;
  }
}

void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
{
  if (drop_displaying_solution_) {
    current_state_ = -1;
    displaying_solution_.reset();
    if (slider_panel_) slider_panel_->update(-1);
    drop_displaying_solution_ = false;
  }
  if (current_state_ < 0 ||  // last animation finished
      (slider_panel_ && slider_panel_->isPaused())) {
    boost::mutex::scoped_lock lock(display_solution_mutex_);

    // new trajectory available to display?
    if (next_solution_to_display_ && (!locked_ || !displaying_solution_)) {
      current_state_ = -1;
      animating_ = true;
      displaying_solution_ = next_solution_to_display_;
      changedTrail();
      if (slider_panel_)
        slider_panel_->update(next_solution_to_display_->getWayPointCount());
    }
    // also reset if locked_
    next_solution_to_display_.reset();
  }
  if (!displaying_solution_) {
    animating_ = false;
    setVisibility();
    return;
  }

  int previous_state = current_state_;
  // for an empty trajectory, show the start and end state at least
  int waypoint_count = displaying_solution_->getWayPointCount();
  int max_state_index = std::max<int>(1, waypoint_count);
  if (slider_panel_ && slider_panel_->isVisible()) {
    animating_ = !slider_panel_->isPaused();
    if (current_state_ >= 0)
      // user can override current_state_ at any time with slider
      current_state_ = slider_panel_->getSliderPosition();
  } else if (current_state_ < max_state_index)
    animating_ = true;  // auto-activate animation if slider_panel_ is hidden

  if (animating_ && current_state_ == previous_state) {
    // auto-advance current_state_ based on time progress
    current_state_time_ += wall_dt;

    float tm = getStateDisplayTime();
    if (current_state_ < 0) {  // special case indicating restart of animation
      current_state_ = 0;
      current_state_time_ = 0.0;
      trail_scene_node_->setVisible(false);
    } else if (tm < 0.0) {  // using realtime: skip to next waypoint based on elapsed display time
      while (current_state_ < max_state_index &&
             (tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) {
          ++current_state_;
          current_state_time_ -= tm;
      }
    } else if (current_state_time_ > tm) {  // fixed display time per state
        ++current_state_;
      current_state_time_ = 0.0;
    }
  } else if (current_state_ != previous_state) {  // current_state_ changed from slider
    current_state_time_ = 0.0;
  }

  if ((waypoint_count > 0 && current_state_ >= max_state_index) ||
      (waypoint_count == 0 && current_state_ > max_state_index)) {  // animation finished?
    if (loop_display_property_->getBool())
      current_state_ = -1;  // restart in next cycle
    else {
      current_state_ = max_state_index;
      if (slider_panel_)
        slider_panel_->pauseButton(true);
    }
    setVisibility();
    return;
  }
  if (current_state_ == previous_state)
    return;

  renderWayPoint(current_state_, previous_state);

  // show / hide trail between start .. end
  int stepsize = trail_step_size_property_->getInt();
  int start = std::max(0, previous_state / stepsize);
  int end = current_state_ / stepsize;
  bool show = start <= end;
  if (!show) std::swap(start, end);
  end = std::min<int>(end, trail_.size());
  for (; start < end; ++start) trail_[start]->setVisible(show);

  setVisibility();
}

void TaskSolutionVisualization::renderCurrentWayPoint()
{
  if (displaying_solution_)
    renderWayPoint(current_state_, -1);
}

void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
{
  assert(index >= 0);
  size_t waypoint_count = displaying_solution_->getWayPointCount();
  moveit::core::RobotStateConstPtr robot_state;
  planning_scene::PlanningSceneConstPtr scene;
  if (index+1 >= waypoint_count) {
    if (index == 0 && waypoint_count == 0)
      // special case: render start scene
      scene = displaying_solution_->startScene();
    else // render end state
      scene = displaying_solution_->scene(waypoint_count);
    renderPlanningScene (scene);
    robot_state.reset(new moveit::core::RobotState(scene->getCurrentState()));
  } else {
    auto idx_pair = displaying_solution_->indexPair(index);
    scene = displaying_solution_->scene(idx_pair);

    if (previous_index < 0 || previous_index >= (int)waypoint_count ||
        displaying_solution_->indexPair(previous_index).first != idx_pair.first) {
      // switch to new stage: show new planning scene
      renderPlanningScene (scene);
      // switch to markers of next sub trajectory?
      if (!marker_visual_->allAtOnce()) {
          marker_visual_->clearMarkers();
          marker_visual_->addMarkers(displaying_solution_->markersOfSubTrajectory(idx_pair.first));
      }
      Q_EMIT activeStageChanged(displaying_solution_->creatorId(idx_pair));
    }
    robot_state = displaying_solution_->getWayPointPtr(idx_pair);
  }

  QColor attached_color = attached_body_color_property_->getColor();
  std_msgs::ColorRGBA color;
  color.r = attached_color.redF();
  color.g = attached_color.greenF();
  color.b = attached_color.blueF();
  color.a = 1.0f;

  planning_scene::ObjectColorMap color_map;
  scene->getKnownObjectColors(color_map);
  robot_render_->update(robot_state, color, color_map);
  marker_visual_->update(*scene, *robot_state);

  if (slider_panel_ && index >= 0)
    slider_panel_->setSliderPosition(index);
}

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<OctreeVoxelRenderMode>(octree_render_property_->getOptionInt()),
                                     static_cast<OctreeVoxelColorMode>(octree_coloring_property_->getOptionInt()),
                                     scene_alpha_property_->getFloat());
}

void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msgs::Solution& msg)
{

  DisplaySolutionPtr s (new DisplaySolution);
  s->setFromMessage(scene_, msg);
  showTrajectory(s, false);
}

void TaskSolutionVisualization::showTrajectory(const DisplaySolutionPtr& s, bool lock_display)
{
  if (lock_display || !s->empty()) {
    boost::mutex::scoped_lock lock(display_solution_mutex_);
    next_solution_to_display_ = s;
    if (lock_display)
      locked_ = drop_displaying_solution_ = true;
    else if (interrupt_display_property_->getBool())
      interruptCurrentDisplay();
  }
}

void TaskSolutionVisualization::unlock()
{
    locked_ = false;
}

void TaskSolutionVisualization::clearMarkers()
{
    marker_visual_->clearMarkers();
}

void TaskSolutionVisualization::addMarkers(const moveit_rviz_plugin::DisplaySolutionPtr &s)
{
	if (!s || (!marker_visual_->allAtOnce() && s->numSubSolutions() > 1))
		return;

	for (size_t i = 0, end = s->numSubSolutions(); i != end; ++i) {
		marker_visual_->addMarkers(s->markersOfSubTrajectory(i));
	}
}

void TaskSolutionVisualization::changedRobotColor()
{
  if (enable_robot_color_property_->getBool())
    setRobotColor(&(robot_render_->getRobot()), robot_color_property_->getColor());
}

void TaskSolutionVisualization::enabledRobotColor()
{
  if (enable_robot_color_property_->getBool())
    setRobotColor(&(robot_render_->getRobot()), robot_color_property_->getColor());
  else
    unsetRobotColor(&(robot_render_->getRobot()));
}

void TaskSolutionVisualization::changedAttachedBodyColor()
{
  renderCurrentWayPoint();
}

void TaskSolutionVisualization::unsetRobotColor(rviz::Robot* robot)
{
  for (auto& link : robot->getLinks())
    link.second->unsetColor();
}

void TaskSolutionVisualization::setRobotColor(rviz::Robot* robot, const QColor& color)
{
  for (auto& link : robot->getLinks())
    link.second->setColor(color.redF(), color.greenF(), color.blueF());
}

void TaskSolutionVisualization::sliderPanelVisibilityChange(bool enable)
{
  if (!slider_panel_)
    return;

  if (enable) {
    // also enable display
    display_->setEnabled(true);
    slider_panel_->onEnable();
  } else
    slider_panel_->onDisable();

  setVisibility();
}


void TaskSolutionVisualization::changedSceneEnabled()
{
  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_));
}

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);
}

void TaskSolutionVisualization::setVisibility()
{
  // main scene node is visible if animation, trail, or panel is shown, or if display is locked
  setVisibility(main_scene_node_, parent_scene_node_,
                display_->isEnabled() && displaying_solution_ &&
                (animating_ || locked_ || trail_scene_node_->getParent() ||
                 (slider_panel_ && slider_panel_->isVisible())));
}

}  // namespace moveit_rviz_plugin