display scene corresponding to SubTrajectory

This commit is contained in:
Robert Haschke 2017-11-11 16:11:25 +01:00
parent 8129caa614
commit c1d676589a
2 changed files with 67 additions and 75 deletions

View File

@ -65,7 +65,6 @@ class PanelDockWidget;
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) }
@ -76,6 +75,7 @@ namespace moveit_rviz_plugin
MOVEIT_CLASS_FORWARD(RobotStateVisualization)
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
MOVEIT_CLASS_FORWARD(PlanningSceneRender)
MOVEIT_CLASS_FORWARD(DisplaySolution)
class TaskSolutionPanel;
class TaskSolutionVisualization : public QObject
@ -129,6 +129,7 @@ private Q_SLOTS:
protected:
float getStateDisplayTime();
void clearTrail();
void renderWayPoint(size_t index, int previous_index);
void renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene);
// render the planning scene
@ -140,8 +141,8 @@ protected:
void setRobotColor(rviz::Robot* robot, const QColor& color);
void unsetRobotColor(rviz::Robot* robot);
robot_trajectory::RobotTrajectoryPtr displaying_solution_;
robot_trajectory::RobotTrajectoryPtr solution_to_display_;
DisplaySolutionPtr displaying_solution_;
DisplaySolutionPtr solution_to_display_;
std::vector<rviz::Robot*> trail_;
bool animating_ = false;
bool drop_displaying_solution_ = false;

View File

@ -168,15 +168,18 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::
// Save pointers for later use
scene_node_ = scene_node;
context_ = context;
scene_node_->setVisible(false);
// Load trajectory robot
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);
changedRobotAlpha();
enabledRobotColor();
robot_render_->setVisible(true);
scene_render_.reset(new PlanningSceneRender(scene_node_, context_, robot_render_));
scene_render_->getGeometryNode()->setVisible(false);
scene_render_.reset(new PlanningSceneRender(scene_node_, context_, RobotStateVisualizationPtr()));
scene_render_->getGeometryNode()->setVisible(true);
rviz::WindowManagerInterface* window_context = context_->getWindowManager();
if (window_context)
@ -221,19 +224,18 @@ void TaskSolutionVisualization::reset()
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
robot_render_->setVisible(false);
scene_node_->setVisible(false);
}
void TaskSolutionVisualization::clearTrail()
{
for (std::size_t i = 0; i < trail_.size(); ++i)
delete trail_[i];
qDeleteAll(trail_);
trail_.clear();
}
void TaskSolutionVisualization::changedLoopDisplay()
{
robot_render_->setVisible(display_->isEnabled() && displaying_solution_ && animating_);
scene_node_->setVisible(display_->isEnabled() && displaying_solution_ && animating_);
if (loop_display_property_->getBool() && slider_panel_)
slider_panel_->pauseButton(false);
}
@ -244,7 +246,7 @@ void TaskSolutionVisualization::changedShowTrail()
if (!trail_display_property_->getBool())
return;
robot_trajectory::RobotTrajectoryPtr t = solution_to_display_;
DisplaySolutionPtr t = solution_to_display_;
if (!t)
t = displaying_solution_;
if (!t)
@ -264,7 +266,7 @@ void TaskSolutionVisualization::changedShowTrail()
r->update(PlanningLinkUpdater(t->getWayPointPtr(waypoint_i)));
if (enable_robot_color_property_->getBool())
setRobotColor(r, robot_color_property_->getColor());
r->setVisible(display_->isEnabled() && (!animating_ || waypoint_i <= current_state_));
r->setVisible(waypoint_i <= current_state_);
trail_[i] = r;
}
}
@ -284,24 +286,16 @@ void TaskSolutionVisualization::changedRobotAlpha()
void TaskSolutionVisualization::changedRobotVisualEnabled()
{
if (display_->isEnabled())
{
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());
}
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
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());
}
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
for (std::size_t i = 0; i < trail_.size(); ++i)
trail_[i]->setCollisionVisible(robot_collision_enabled_property_->getBool());
}
void TaskSolutionVisualization::changedStateDisplayTime()
@ -310,28 +304,12 @@ void TaskSolutionVisualization::changedStateDisplayTime()
void TaskSolutionVisualization::onEnable()
{
changedRobotAlpha(); // set alpha property
changedSceneEnabled(); // show/hide planning scene
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)
{
trail_[i]->setVisualVisible(robot_visual_enabled_property_->getBool());
trail_[i]->setCollisionVisible(robot_collision_enabled_property_->getBool());
trail_[i]->setVisible(true);
}
scene_node_->setVisible(displaying_solution_ && animating_);
}
void TaskSolutionVisualization::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);
scene_node_->setVisible(false);
displaying_solution_.reset();
animating_ = false;
@ -381,7 +359,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
{
animating_ = false;
displaying_solution_.reset();
robot_render_->setVisible(false);
scene_node_->setVisible(false);
slider_panel_->update(0);
drop_displaying_solution_ = false;
}
@ -407,10 +385,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
else if (slider_panel_ && slider_panel_->isVisible())
{
if (slider_panel_->getSliderPosition() == (int)displaying_solution_->getWayPointCount() - 1)
{ // show the last waypoint if the slider is enabled
robot_render_->update(
displaying_solution_->getWayPointPtr(displaying_solution_->getWayPointCount() - 1));
}
return; // nothing more to do
else
animating_ = true;
}
@ -421,10 +396,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
{
current_state_ = -1;
current_state_time_ = std::numeric_limits<float>::infinity();
robot_render_->update(displaying_solution_->getFirstWayPointPtr());
robot_render_->setVisible(display_->isEnabled());
if (slider_panel_)
slider_panel_->setSliderPosition(0);
scene_node_->setVisible(display_->isEnabled());
}
}
@ -433,6 +405,8 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
float tm = getStateDisplayTime();
if (tm < 0.0) // if we should use realtime
tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1);
int previous_state = current_state_;
if (current_state_time_ > tm)
{
if (slider_panel_ && slider_panel_->isVisible() && slider_panel_->isPaused())
@ -442,20 +416,21 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
int waypoint_count = displaying_solution_->getWayPointCount();
if (current_state_ < waypoint_count)
{
if (slider_panel_)
slider_panel_->setSliderPosition(current_state_);
robot_render_->update(displaying_solution_->getWayPointPtr(current_state_));
renderWayPoint(current_state_, previous_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_);
std::min(waypoint_count - 1,
static_cast<int>(i) * trail_step_size_property_->getInt()) <= current_state_);
}
else
{
animating_ = false; // animation finished
robot_render_->setVisible(loop_display_property_->getBool());
scene_node_->setVisible(loop_display_property_->getBool() ||
(slider_panel_ && slider_panel_->isVisible()));
if (!loop_display_property_->getBool() && slider_panel_)
slider_panel_->pauseButton(true);
// ensure to render last frame
renderWayPoint(displaying_solution_->getWayPointCount() - 1, previous_state);
}
current_state_time_ = 0.0f;
}
@ -463,6 +438,30 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
}
}
void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
{
auto idx_pair = displaying_solution_->indexPair(index);
const planning_scene::PlanningSceneConstPtr &scene = displaying_solution_->scene(idx_pair);
if (previous_index < 0 ||
displaying_solution_->indexPair(previous_index).first != idx_pair.first)
renderPlanningScene (scene);
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(displaying_solution_->getWayPointPtr(idx_pair), color, color_map);
if (slider_panel_)
slider_panel_->setSliderPosition(index);
}
void TaskSolutionVisualization::renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene)
{
if (!scene_render_ || !scene_enabled_property_->getBool())
@ -492,25 +491,13 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::So
scene_->setPlanningSceneMsg(msg.start_scene);
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(scene_->getCurrentState(), msg.sub_trajectory[i].trajectory);
}
else
{
robot_trajectory::RobotTrajectory tmp(scene_->getRobotModel(), "");
tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg.sub_trajectory[i].trajectory);
t->append(tmp, 0.0);
}
}
DisplaySolutionPtr s (new DisplaySolution);
s->setFromMessage(scene_, msg);
if (!t->empty())
if (!s->empty())
{
boost::mutex::scoped_lock lock(display_solution_mutex_);
solution_to_display_.swap(t);
solution_to_display_.swap(s);
if (interrupt_display_property_->getBool())
interruptCurrentDisplay();
}
@ -556,6 +543,8 @@ void TaskSolutionVisualization::sliderPanelVisibilityChange(bool enable)
slider_panel_->onEnable();
else
slider_panel_->onDisable();
scene_node_->setVisible(enable || animating_);
}
@ -567,6 +556,8 @@ void TaskSolutionVisualization::changedSceneEnabled()
void TaskSolutionVisualization::renderCurrentScene()
{
if (scene_render_ && scene_enabled_property_->getBool() && current_state_ >= 0)
renderPlanningScene(displaying_solution_->scene(current_state_));
}
} // namespace moveit_rviz_plugin