mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
update trail in both directions:
- show new steps if current_state_ advanced - hide steps if current_state_ reverted
This commit is contained in:
parent
534985bdbb
commit
cd88b22469
@ -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();
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user