mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
67b5a99d8d
commit
12095405f4
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user