DisplaySolution: make scene representing the end state

- start state is accessible via scene->getParent()
- allows to show the final state
This commit is contained in:
Robert Haschke 2017-11-12 21:08:07 +01:00
parent 67b5a99d8d
commit 12095405f4
3 changed files with 16 additions and 7 deletions

View File

@ -20,9 +20,13 @@ MOVEIT_CLASS_FORWARD(DisplaySolution)
/** Class representing a task solution for display */ /** Class representing a task solution for display */
class DisplaySolution class DisplaySolution
{ {
/// number of overall steps
size_t steps_; size_t steps_;
/// end scenes for each sub trajectory
std::vector<planning_scene::PlanningSceneConstPtr> scene_; std::vector<planning_scene::PlanningSceneConstPtr> scene_;
/// sub trajectories, might be empty
std::vector<robot_trajectory::RobotTrajectoryPtr> trajectory_; std::vector<robot_trajectory::RobotTrajectoryPtr> trajectory_;
/// optional name of the trajectory
std::vector<std::string> name_; std::vector<std::string> name_;
public: public:
@ -43,6 +47,8 @@ public:
} }
const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const; const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const;
const planning_scene::PlanningSceneConstPtr& scene(size_t index) const { const planning_scene::PlanningSceneConstPtr& scene(size_t index) const {
if (index == steps_)
return scene_.back();
return scene(indexPair(index)); return scene(indexPair(index));
} }
const std::string& name(const IndexPair& idx_pair) const; const std::string& name(const IndexPair& idx_pair) const;

View File

@ -30,7 +30,8 @@ const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPai
const planning_scene::PlanningSceneConstPtr &DisplaySolution::scene(const IndexPair &idx_pair) const 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 const std::string &DisplaySolution::name(const IndexPair &idx_pair) const
@ -50,16 +51,16 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningSceneConstPtr
steps_ = 0; steps_ = 0;
size_t i = 0; size_t i = 0;
for (const auto& sub : msg.sub_trajectory) { for (const auto& sub : msg.sub_trajectory) {
scene_[i] = ref_scene;
trajectory_[i].reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), "")); trajectory_[i].reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), ""));
trajectory_[i]->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory); trajectory_[i]->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory);
name_[i] = sub.name; name_[i] = sub.name;
steps_ += trajectory_[i]->getWayPointCount(); steps_ += trajectory_[i]->getWayPointCount();
// create new reference scene based of diffs
ref_scene = ref_scene->diff();
ref_scene->setPlanningSceneDiffMsg(sub.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; ++i;
} }
} }

View File

@ -413,6 +413,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
current_state_ = slider_panel_->getSliderPosition(); current_state_ = slider_panel_->getSliderPosition();
else else
++current_state_; ++current_state_;
int waypoint_count = displaying_solution_->getWayPointCount(); int waypoint_count = displaying_solution_->getWayPointCount();
if (current_state_ < waypoint_count) if (current_state_ < waypoint_count)
{ {
@ -429,8 +430,9 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
(slider_panel_ && slider_panel_->isVisible())); (slider_panel_ && slider_panel_->isVisible()));
if (!loop_display_property_->getBool() && slider_panel_) if (!loop_display_property_->getBool() && slider_panel_)
slider_panel_->pauseButton(true); slider_panel_->pauseButton(true);
// ensure to render last frame // ensure to render end state
renderWayPoint(displaying_solution_->getWayPointCount() - 1, previous_state); renderWayPoint(waypoint_count - 1, previous_state);
renderPlanningScene(displaying_solution_->scene(waypoint_count));
} }
current_state_time_ = 0.0f; current_state_time_ = 0.0f;
} }