mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
TaskSolutionVisualization: added scene, renamed vars
This commit is contained in:
parent
81126f5ef6
commit
8129caa614
@ -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", "",
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user