update trail in both directions:

- show new steps if current_state_ advanced
- hide steps if current_state_ reverted
This commit is contained in:
Robert Haschke 2018-04-11 00:40:37 +02:00
parent 534985bdbb
commit cd88b22469

View File

@ -267,9 +267,7 @@ void TaskSolutionVisualization::changedLoopDisplay()
void TaskSolutionVisualization::changedTrail() void TaskSolutionVisualization::changedTrail()
{ {
clearTrail(); clearTrail();
DisplaySolutionPtr t = next_solution_to_display_; DisplaySolutionPtr t = displaying_solution_;
if (!t)
t = displaying_solution_;
if (!t || !trail_display_property_->getBool()) { if (!t || !trail_display_property_->getBool()) {
setVisibility(trail_scene_node_, main_scene_node_, false); setVisibility(trail_scene_node_, main_scene_node_, false);
@ -280,8 +278,7 @@ void TaskSolutionVisualization::changedTrail()
setVisibility(trail_scene_node_, main_scene_node_, true); setVisibility(trail_scene_node_, main_scene_node_, true);
int stepsize = trail_step_size_property_->getInt(); int stepsize = trail_step_size_property_->getInt();
// always include last trajectory point trail_.resize(t->getWayPointCount() / stepsize);
trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize));
for (std::size_t i = 0; i < trail_.size(); i++) for (std::size_t i = 0; i < trail_.size(); i++)
{ {
int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point
@ -466,11 +463,14 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
renderWayPoint(current_state_, previous_state); renderWayPoint(current_state_, previous_state);
// render trail up to current_state_ // show / hide trail between start .. end
int stepsize = trail_step_size_property_->getInt(); int stepsize = trail_step_size_property_->getInt();
for (int i = std::max(0, previous_state / stepsize), int start = std::max(0, previous_state / stepsize);
end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) int end = current_state_ / stepsize;
trail_[i]->setVisible(true); bool show = start <= end;
if (!show) std::swap(start, end);
end = std::min<int>(end, trail_.size());
for (; start < end; ++start) trail_[start]->setVisible(show);
setVisibility(); setVisibility();
} }