TaskSolutionVisualization: added scene, renamed vars

This commit is contained in:
Robert Haschke 2017-11-11 11:15:21 +01:00
parent 81126f5ef6
commit 8129caa614
3 changed files with 299 additions and 220 deletions

View File

@ -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", "",

View File

@ -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 <moveit/macros/class_forward.h>
#include <moveit_task_constructor/Solution.h>
#include <QObject>
#include <boost/thread/mutex.hpp>
#ifndef Q_MOC_RUN
#include <ros/node_handle.h>
#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<rviz::Robot*> trajectory_trail_;
bool animating_path_ = false;
bool drop_displaying_trajectory_ = false;
robot_trajectory::RobotTrajectoryPtr displaying_solution_;
robot_trajectory::RobotTrajectoryPtr solution_to_display_;
std::vector<rviz::Robot*> 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

View File

@ -34,13 +34,13 @@
/* Author: Dave Coleman, Robert Haschke */
#include <moveit_task_constructor/visualization_tools/display_solution.h>
#include <moveit_task_constructor/visualization_tools/task_solution_visualization.h>
#include <moveit_task_constructor/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/rviz_plugin_render_tools/robot_state_visualization.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
@ -54,6 +54,7 @@
#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>
@ -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<std::string>(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<float>::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<int>(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<OctreeVoxelRenderMode>(octree_render_property_->getOptionInt()),
static_cast<OctreeVoxelColorMode>(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