From 12095405f4e21ce7d3c09e57401f97be2e35454d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 12 Nov 2017 21:08:07 +0100 Subject: [PATCH] DisplaySolution: make scene representing the end state - start state is accessible via scene->getParent() - allows to show the final state --- .../moveit/visualization_tools/display_solution.h | 6 ++++++ .../visualization_tools/src/display_solution.cpp | 11 ++++++----- .../src/task_solution_visualization.cpp | 6 ++++-- 3 files changed, 16 insertions(+), 7 deletions(-) diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index f912ea98..eb7e0048 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -20,9 +20,13 @@ MOVEIT_CLASS_FORWARD(DisplaySolution) /** Class representing a task solution for display */ class DisplaySolution { + /// number of overall steps size_t steps_; + /// end scenes for each sub trajectory std::vector scene_; + /// sub trajectories, might be empty std::vector trajectory_; + /// optional name of the trajectory std::vector name_; public: @@ -43,6 +47,8 @@ public: } const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const; const planning_scene::PlanningSceneConstPtr& scene(size_t index) const { + if (index == steps_) + return scene_.back(); return scene(indexPair(index)); } const std::string& name(const IndexPair& idx_pair) const; diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index a1428d50..3e57b83f 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -30,7 +30,8 @@ const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPai const planning_scene::PlanningSceneConstPtr &DisplaySolution::scene(const IndexPair &idx_pair) const { - return scene_[idx_pair.first]; + // start scene is parent of end scene + return scene_[idx_pair.first]->getParent(); } const std::string &DisplaySolution::name(const IndexPair &idx_pair) const @@ -50,16 +51,16 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningSceneConstPtr steps_ = 0; size_t i = 0; for (const auto& sub : msg.sub_trajectory) { - scene_[i] = ref_scene; trajectory_[i].reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), "")); trajectory_[i]->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory); - name_[i] = sub.name; steps_ += trajectory_[i]->getWayPointCount(); - // create new reference scene based of diffs - ref_scene = ref_scene->diff(); ref_scene->setPlanningSceneDiffMsg(sub.scene_diff); + scene_[i] = ref_scene; + + // create new reference scene for next iteration + ref_scene = ref_scene->diff(); ++i; } } diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index e1afa7c1..72129541 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -413,6 +413,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) current_state_ = slider_panel_->getSliderPosition(); else ++current_state_; + int waypoint_count = displaying_solution_->getWayPointCount(); if (current_state_ < waypoint_count) { @@ -429,8 +430,9 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) (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); + // ensure to render end state + renderWayPoint(waypoint_count - 1, previous_state); + renderPlanningScene(displaying_solution_->scene(waypoint_count)); } current_state_time_ = 0.0f; }