reworked TaskSolution visualization

- code simplification
- allow slider interaction at any time
- keep last frame visible if animation finished
- animating_ is true iff animation is running
  (previously it was also used to indicate the end of an animation cycle)
This commit is contained in:
Robert Haschke 2018-04-10 14:35:42 +02:00
parent 24e8b95203
commit 962fe2a42d
5 changed files with 80 additions and 67 deletions

View File

@ -108,7 +108,7 @@ 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_) if (index >= steps_)
return data_.back().scene_; return data_.back().scene_;
return scene(indexPair(index)); return scene(indexPair(index));
} }

View File

@ -88,7 +88,6 @@ protected:
QPushButton* button_; QPushButton* button_;
bool paused_; bool paused_;
int last_way_point_;
}; };
} // namespace moveit_rviz_plugin } // namespace moveit_rviz_plugin

View File

@ -158,7 +158,7 @@ protected:
DisplaySolutionPtr displaying_solution_; DisplaySolutionPtr displaying_solution_;
DisplaySolutionPtr next_solution_to_display_; DisplaySolutionPtr next_solution_to_display_;
std::vector<rviz::Robot*> trail_; std::vector<rviz::Robot*> trail_;
bool animating_ = false; bool animating_ = false; // auto-progressing the current waypoint?
bool drop_displaying_solution_ = false; bool drop_displaying_solution_ = false;
bool locked_ = false; bool locked_ = false;
int current_state_ = -1; int current_state_ = -1;

View File

@ -94,14 +94,13 @@ void TaskSolutionPanel::update(int way_point_count)
{ {
int max_way_point = std::max(0, way_point_count - 1); int max_way_point = std::max(0, way_point_count - 1);
slider_->setEnabled(way_point_count != 0); slider_->setEnabled(way_point_count >= 0);
button_->setEnabled(way_point_count != 0); button_->setEnabled(way_point_count >= 0);
last_way_point_ = max_way_point;
paused_ = false; paused_ = false;
slider_->setSliderPosition(0);
slider_->setMaximum(max_way_point); slider_->setMaximum(max_way_point);
maximum_label_->setText(QString::number(max_way_point)); maximum_label_->setText(way_point_count >= 0 ? QString::number(max_way_point) : "");
slider_->setSliderPosition(0);
} }
void TaskSolutionPanel::pauseButton(bool pause) void TaskSolutionPanel::pauseButton(bool pause)
@ -115,7 +114,7 @@ void TaskSolutionPanel::pauseButton(bool pause)
{ {
button_->setText("Pause"); button_->setText("Pause");
paused_ = false; paused_ = false;
if (slider_->sliderPosition() == last_way_point_) if (slider_->sliderPosition() == slider_->maximum())
slider_->setSliderPosition(0); slider_->setSliderPosition(0);
} }
} }
@ -127,7 +126,10 @@ void TaskSolutionPanel::setSliderPosition(int position)
void TaskSolutionPanel::sliderValueChanged(int value) void TaskSolutionPanel::sliderValueChanged(int value)
{ {
minimum_label_->setText(QString::number(value)); QString text;
if (!slider_->isEnabled()) text = "";
else text = QString::number(value);
minimum_label_->setText(text);
} }
void TaskSolutionPanel::buttonClicked() void TaskSolutionPanel::buttonClicked()

View File

@ -235,7 +235,9 @@ void TaskSolutionVisualization::reset()
clearTrail(); clearTrail();
next_solution_to_display_.reset(); next_solution_to_display_.reset();
displaying_solution_.reset(); displaying_solution_.reset();
animating_ = false; current_state_ = -1;
if (slider_panel_)
slider_panel_->update(-1);
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool()); robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool()); robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
@ -253,9 +255,14 @@ void TaskSolutionVisualization::clearTrail()
void TaskSolutionVisualization::changedLoopDisplay() void TaskSolutionVisualization::changedLoopDisplay()
{ {
if (loop_display_property_->getBool() && slider_panel_) // restart animation if current_state_ is at end and looping got activated
if (displaying_solution_ && loop_display_property_->getBool() &&
slider_panel_ && slider_panel_->isVisible() &&
current_state_+1 >= (int)displaying_solution_->getWayPointCount()) {
current_state_ = -1;
slider_panel_->pauseButton(false); slider_panel_->pauseButton(false);
} }
}
void TaskSolutionVisualization::changedTrail() void TaskSolutionVisualization::changedTrail()
{ {
@ -325,7 +332,8 @@ void TaskSolutionVisualization::onDisable()
parent_scene_node_->removeChild(main_scene_node_); parent_scene_node_->removeChild(main_scene_node_);
displaying_solution_.reset(); displaying_solution_.reset();
animating_ = false; next_solution_to_display_.reset();
current_state_ = -1;
if (slider_panel_) { if (slider_panel_) {
slider_panel_was_visible_ = slider_panel_->isVisible(); slider_panel_was_visible_ = slider_panel_->isVisible();
slider_panel_->onDisable(); slider_panel_->onDisable();
@ -374,74 +382,86 @@ float TaskSolutionVisualization::getStateDisplayTime()
void TaskSolutionVisualization::update(float wall_dt, float ros_dt) void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
{ {
if (drop_displaying_solution_) if (drop_displaying_solution_) {
{ current_state_ = -1;
animating_ = false;
displaying_solution_.reset(); displaying_solution_.reset();
slider_panel_->update(0); if (slider_panel_) slider_panel_->update(-1);
drop_displaying_solution_ = false; drop_displaying_solution_ = false;
} }
if (!animating_) if (current_state_ < 0 || // last animation finished
{ // finished last animation (slider_panel_ && slider_panel_->isPaused())) {
boost::mutex::scoped_lock lock(display_solution_mutex_); boost::mutex::scoped_lock lock(display_solution_mutex_);
// new trajectory available to display? // new trajectory available to display?
if (next_solution_to_display_ && (!locked_ || !displaying_solution_)) { if (next_solution_to_display_ && (!locked_ || !displaying_solution_)) {
current_state_ = -1;
animating_ = true; animating_ = true;
displaying_solution_ = next_solution_to_display_; displaying_solution_ = next_solution_to_display_;
changedTrail(); changedTrail();
if (slider_panel_) if (slider_panel_)
slider_panel_->update(next_solution_to_display_->getWayPointCount()); slider_panel_->update(next_solution_to_display_->getWayPointCount());
} }
else if (displaying_solution_) { // also reset if locked_
if (loop_display_property_->getBool()) { next_solution_to_display_.reset();
animating_ = true; }
} else if (slider_panel_ && slider_panel_->isVisible()) { if (!displaying_solution_) {
if (slider_panel_->getSliderPosition() >= (int)displaying_solution_->getWayPointCount() - 1) animating_ = false;
return; // nothing more to do setVisibility();
else
animating_ = true;
} else if (locked_)
return; return;
} }
next_solution_to_display_.reset();
if (animating_)
{
// restart animation
current_state_ = -1;
trail_scene_node_->setVisible(false);
}
}
if (animating_)
{
int previous_state = current_state_; int previous_state = current_state_;
int waypoint_count = displaying_solution_->getWayPointCount(); int waypoint_count = displaying_solution_->getWayPointCount();
if (slider_panel_ && slider_panel_->isVisible()) {
animating_ = !slider_panel_->isPaused();
if (current_state_ >= 0)
// user can override current_state_ at any time with slider
current_state_ = slider_panel_->getSliderPosition();
} else if (current_state_ < waypoint_count)
animating_ = true; // auto-activate animation if slider_panel_ is hidden
QString msg = "no change";
if (animating_ && current_state_ == previous_state) {
// auto-advance current_state_ based on time progress
current_state_time_ += wall_dt; current_state_time_ += wall_dt;
float tm = getStateDisplayTime(); float tm = getStateDisplayTime();
if (slider_panel_ && slider_panel_->isVisible() && slider_panel_->isPaused()) if (current_state_ < 0) { // special case indicating restart of animation
current_state_ = slider_panel_->getSliderPosition();
else if (current_state_ < 0) { // special case indicating restart of animation
current_state_ = 0; current_state_ = 0;
current_state_time_ = 0.0; current_state_time_ = 0.0;
trail_scene_node_->setVisible(false);
msg = "restart";
} else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time } else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time
while (current_state_ < waypoint_count && while (current_state_ < waypoint_count &&
(tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) { (tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) {
current_state_time_ -= tm;
++current_state_; ++current_state_;
current_state_time_ -= tm;
} }
} else if (current_state_time_ > tm) { // fixed display time per state } else if (current_state_time_ > tm) { // fixed display time per state
current_state_time_ = 0.0;
++current_state_; ++current_state_;
current_state_time_ = 0.0;
msg = "progress";
}
} else if (current_state_ != previous_state) { // current_state_ changed from slider
current_state_time_ = 0.0;
msg = "slider";
} }
if (current_state_ >= waypoint_count) { // animation finished?
if (loop_display_property_->getBool())
current_state_ = -1; // restart in next cycle
else {
current_state_ = waypoint_count;
if (slider_panel_)
slider_panel_->pauseButton(true);
}
setVisibility();
return;
}
if (current_state_ == previous_state) if (current_state_ == previous_state)
return; return;
if (current_state_ < waypoint_count) if (current_state_ < waypoint_count) {
{
renderWayPoint(current_state_, previous_state); renderWayPoint(current_state_, previous_state);
int stepsize = trail_step_size_property_->getInt(); int stepsize = trail_step_size_property_->getInt();
@ -449,32 +469,24 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i)
trail_[i]->setVisible(true); trail_[i]->setVisible(true);
} }
else
{
animating_ = false; // animation finished
if (!loop_display_property_->getBool() && slider_panel_)
slider_panel_->pauseButton(true);
// ensure to render end state
renderWayPoint(waypoint_count, previous_state);
}
}
setVisibility(); setVisibility();
} }
void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
{ {
size_t waypoint_count = displaying_solution_->getWayPointCount();
moveit::core::RobotStateConstPtr robot_state; moveit::core::RobotStateConstPtr robot_state;
planning_scene::PlanningSceneConstPtr scene; planning_scene::PlanningSceneConstPtr scene;
if (index == displaying_solution_->getWayPointCount()) { if (index+1 >= waypoint_count) {
// render last state // render last state
scene = displaying_solution_->scene(index); scene = displaying_solution_->scene(waypoint_count);
renderPlanningScene (scene); renderPlanningScene (scene);
robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); robot_state.reset(new moveit::core::RobotState(scene->getCurrentState()));
} else { } else {
auto idx_pair = displaying_solution_->indexPair(index); auto idx_pair = displaying_solution_->indexPair(index);
scene = displaying_solution_->scene(idx_pair); scene = displaying_solution_->scene(idx_pair);
if (previous_index < 0 || if (previous_index < 0 || previous_index >= (int)waypoint_count ||
displaying_solution_->indexPair(previous_index).first != idx_pair.first) { displaying_solution_->indexPair(previous_index).first != idx_pair.first) {
// switch to new stage: show new planning scene // switch to new stage: show new planning scene
renderPlanningScene (scene); renderPlanningScene (scene);