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 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;
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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;
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user