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 DisplaySolution
{
/// number of overall steps
size_t steps_;
/// end scenes for each sub trajectory
std::vector<planning_scene::PlanningSceneConstPtr> scene_;
/// sub trajectories, might be empty
std::vector<robot_trajectory::RobotTrajectoryPtr> trajectory_;
/// optional name of the trajectory
std::vector<std::string> 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;

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
{
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;
}
}

View File

@ -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;
}