From 24e8b95203ba41edbad1d678c7adc54d5e7a60a9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 10 Apr 2018 14:38:02 +0200 Subject: [PATCH 01/10] TaskSolutionVisualization: solution_to_display_ -> next_solution_to_display_ --- .../task_solution_visualization.h | 2 +- .../src/task_solution_visualization.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index b5f5d32c..bb1037ac 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -156,7 +156,7 @@ protected: void unsetRobotColor(rviz::Robot* robot); DisplaySolutionPtr displaying_solution_; - DisplaySolutionPtr solution_to_display_; + DisplaySolutionPtr next_solution_to_display_; std::vector trail_; bool animating_ = false; bool drop_displaying_solution_ = false; diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 2b8e327b..908728a7 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -161,7 +161,7 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi TaskSolutionVisualization::~TaskSolutionVisualization() { clearTrail(); - solution_to_display_.reset(); + next_solution_to_display_.reset(); displaying_solution_.reset(); scene_render_.reset(); @@ -233,7 +233,7 @@ void TaskSolutionVisualization::onRobotModelLoaded(robot_model::RobotModelConstP void TaskSolutionVisualization::reset() { clearTrail(); - solution_to_display_.reset(); + next_solution_to_display_.reset(); displaying_solution_.reset(); animating_ = false; @@ -260,7 +260,7 @@ void TaskSolutionVisualization::changedLoopDisplay() void TaskSolutionVisualization::changedTrail() { clearTrail(); - DisplaySolutionPtr t = solution_to_display_; + DisplaySolutionPtr t = next_solution_to_display_; if (!t) t = displaying_solution_; @@ -386,12 +386,12 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) boost::mutex::scoped_lock lock(display_solution_mutex_); // new trajectory available to display? - if (solution_to_display_ && (!locked_ || !displaying_solution_)) { + if (next_solution_to_display_ && (!locked_ || !displaying_solution_)) { animating_ = true; - displaying_solution_ = solution_to_display_; + displaying_solution_ = next_solution_to_display_; changedTrail(); if (slider_panel_) - slider_panel_->update(solution_to_display_->getWayPointCount()); + slider_panel_->update(next_solution_to_display_->getWayPointCount()); } else if (displaying_solution_) { if (loop_display_property_->getBool()) { @@ -404,7 +404,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) } else if (locked_) return; } - solution_to_display_.reset(); + next_solution_to_display_.reset(); if (animating_) { @@ -532,7 +532,7 @@ void TaskSolutionVisualization::showTrajectory(const DisplaySolutionPtr& s, bool { if (lock_display || !s->empty()) { boost::mutex::scoped_lock lock(display_solution_mutex_); - solution_to_display_ = s; + next_solution_to_display_ = s; if (lock_display) locked_ = drop_displaying_solution_ = true; else if (interrupt_display_property_->getBool()) From 962fe2a42d099b6b39f26ae563c215d36c3011dd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 10 Apr 2018 14:35:42 +0200 Subject: [PATCH 02/10] 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) --- .../visualization_tools/display_solution.h | 2 +- .../visualization_tools/task_solution_panel.h | 1 - .../task_solution_visualization.h | 2 +- .../src/task_solution_panel.cpp | 16 ++- .../src/task_solution_visualization.cpp | 126 ++++++++++-------- 5 files changed, 80 insertions(+), 67 deletions(-) diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 2970a130..9705d3f3 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -108,7 +108,7 @@ public: } const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const; const planning_scene::PlanningSceneConstPtr& scene(size_t index) const { - if (index == steps_) + if (index >= steps_) return data_.back().scene_; return scene(indexPair(index)); } diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h index cbd86807..7f030041 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h @@ -88,7 +88,6 @@ protected: QPushButton* button_; bool paused_; - int last_way_point_; }; } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index bb1037ac..dd79b4e2 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -158,7 +158,7 @@ protected: DisplaySolutionPtr displaying_solution_; DisplaySolutionPtr next_solution_to_display_; std::vector trail_; - bool animating_ = false; + bool animating_ = false; // auto-progressing the current waypoint? bool drop_displaying_solution_ = false; bool locked_ = false; int current_state_ = -1; diff --git a/visualization/visualization_tools/src/task_solution_panel.cpp b/visualization/visualization_tools/src/task_solution_panel.cpp index 6ef190ef..e56e76e6 100644 --- a/visualization/visualization_tools/src/task_solution_panel.cpp +++ b/visualization/visualization_tools/src/task_solution_panel.cpp @@ -94,14 +94,13 @@ void TaskSolutionPanel::update(int way_point_count) { int max_way_point = std::max(0, way_point_count - 1); - slider_->setEnabled(way_point_count != 0); - button_->setEnabled(way_point_count != 0); + slider_->setEnabled(way_point_count >= 0); + button_->setEnabled(way_point_count >= 0); - last_way_point_ = max_way_point; paused_ = false; - slider_->setSliderPosition(0); 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) @@ -115,7 +114,7 @@ void TaskSolutionPanel::pauseButton(bool pause) { button_->setText("Pause"); paused_ = false; - if (slider_->sliderPosition() == last_way_point_) + if (slider_->sliderPosition() == slider_->maximum()) slider_->setSliderPosition(0); } } @@ -127,7 +126,10 @@ void TaskSolutionPanel::setSliderPosition(int position) 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() diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 908728a7..b672117e 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -235,7 +235,9 @@ void TaskSolutionVisualization::reset() clearTrail(); next_solution_to_display_.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_->setCollisionVisible(robot_collision_enabled_property_->getBool()); @@ -253,8 +255,13 @@ void TaskSolutionVisualization::clearTrail() 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); + } } void TaskSolutionVisualization::changedTrail() @@ -325,7 +332,8 @@ void TaskSolutionVisualization::onDisable() parent_scene_node_->removeChild(main_scene_node_); displaying_solution_.reset(); - animating_ = false; + next_solution_to_display_.reset(); + current_state_ = -1; if (slider_panel_) { slider_panel_was_visible_ = slider_panel_->isVisible(); slider_panel_->onDisable(); @@ -374,107 +382,111 @@ float TaskSolutionVisualization::getStateDisplayTime() void TaskSolutionVisualization::update(float wall_dt, float ros_dt) { - if (drop_displaying_solution_) - { - animating_ = false; + if (drop_displaying_solution_) { + current_state_ = -1; displaying_solution_.reset(); - slider_panel_->update(0); + if (slider_panel_) slider_panel_->update(-1); drop_displaying_solution_ = false; } - if (!animating_) - { // finished last animation + if (current_state_ < 0 || // last animation finished + (slider_panel_ && slider_panel_->isPaused())) { boost::mutex::scoped_lock lock(display_solution_mutex_); // new trajectory available to display? if (next_solution_to_display_ && (!locked_ || !displaying_solution_)) { + current_state_ = -1; animating_ = true; displaying_solution_ = next_solution_to_display_; changedTrail(); if (slider_panel_) slider_panel_->update(next_solution_to_display_->getWayPointCount()); } - else if (displaying_solution_) { - if (loop_display_property_->getBool()) { - animating_ = true; - } else if (slider_panel_ && slider_panel_->isVisible()) { - if (slider_panel_->getSliderPosition() >= (int)displaying_solution_->getWayPointCount() - 1) - return; // nothing more to do - else - animating_ = true; - } else if (locked_) - return; - } + // also reset if locked_ next_solution_to_display_.reset(); - - if (animating_) - { - // restart animation - current_state_ = -1; - trail_scene_node_->setVisible(false); - } + } + if (!displaying_solution_) { + animating_ = false; + setVisibility(); + return; } - if (animating_) - { - int previous_state = current_state_; - int waypoint_count = displaying_solution_->getWayPointCount(); + int previous_state = current_state_; + 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; float tm = getStateDisplayTime(); - if (slider_panel_ && slider_panel_->isVisible() && slider_panel_->isPaused()) - current_state_ = slider_panel_->getSliderPosition(); - else if (current_state_ < 0) { // special case indicating restart of animation + if (current_state_ < 0) { // special case indicating restart of animation current_state_ = 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 while (current_state_ < waypoint_count && (tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) { - current_state_time_ -= tm; ++current_state_; + current_state_time_ -= tm; } } else if (current_state_time_ > tm) { // fixed display time per state + ++current_state_; current_state_time_ = 0.0; - ++current_state_; + msg = "progress"; } + } else if (current_state_ != previous_state) { // current_state_ changed from slider + current_state_time_ = 0.0; + msg = "slider"; + } - if (current_state_ == previous_state) - return; - - if (current_state_ < waypoint_count) - { - renderWayPoint(current_state_, previous_state); - - int stepsize = trail_step_size_property_->getInt(); - for (int i = std::max(0, previous_state / stepsize), - end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) - trail_[i]->setVisible(true); - } - else - { - animating_ = false; // animation finished - if (!loop_display_property_->getBool() && slider_panel_) + 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); - // ensure to render end state - renderWayPoint(waypoint_count, previous_state); } + setVisibility(); + return; + } + if (current_state_ == previous_state) + return; + + if (current_state_ < waypoint_count) { + renderWayPoint(current_state_, previous_state); + + int stepsize = trail_step_size_property_->getInt(); + for (int i = std::max(0, previous_state / stepsize), + end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) + trail_[i]->setVisible(true); } setVisibility(); } void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) { + size_t waypoint_count = displaying_solution_->getWayPointCount(); moveit::core::RobotStateConstPtr robot_state; planning_scene::PlanningSceneConstPtr scene; - if (index == displaying_solution_->getWayPointCount()) { + if (index+1 >= waypoint_count) { // render last state - scene = displaying_solution_->scene(index); + scene = displaying_solution_->scene(waypoint_count); renderPlanningScene (scene); robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); } else { auto idx_pair = displaying_solution_->indexPair(index); 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) { // switch to new stage: show new planning scene renderPlanningScene (scene); From 534985bdbb9dd2e267f1428a66e6d5736e6b5669 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 14 Apr 2018 09:34:35 +0200 Subject: [PATCH 03/10] show start / end scene for empty trajectories --- .../visualization_tools/display_solution.h | 1 + .../visualization_tools/task_solution_panel.h | 1 + .../src/task_solution_panel.cpp | 9 +++-- .../src/task_solution_visualization.cpp | 35 +++++++++++-------- 4 files changed, 29 insertions(+), 17 deletions(-) diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 9705d3f3..fcb19e30 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -106,6 +106,7 @@ public: const moveit::core::RobotStatePtr& getWayPointPtr(size_t index) const { return getWayPointPtr(indexPair(index)); } + const planning_scene::PlanningSceneConstPtr& startScene() const { return start_scene_; } const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const; const planning_scene::PlanningSceneConstPtr& scene(size_t index) const { if (index >= steps_) diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h index 7f030041..ca873ea4 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h @@ -88,6 +88,7 @@ protected: QPushButton* button_; bool paused_; + bool empty_; }; } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/src/task_solution_panel.cpp b/visualization/visualization_tools/src/task_solution_panel.cpp index e56e76e6..4e59b236 100644 --- a/visualization/visualization_tools/src/task_solution_panel.cpp +++ b/visualization/visualization_tools/src/task_solution_panel.cpp @@ -41,6 +41,7 @@ namespace moveit_rviz_plugin { TaskSolutionPanel::TaskSolutionPanel(QWidget* parent) : Panel(parent) { + empty_ = false; } TaskSolutionPanel::~TaskSolutionPanel() @@ -92,14 +93,15 @@ void TaskSolutionPanel::onDisable() void TaskSolutionPanel::update(int way_point_count) { - int max_way_point = std::max(0, way_point_count - 1); + int max_way_point = std::max(1, way_point_count - 1); + empty_ = (way_point_count == 0); slider_->setEnabled(way_point_count >= 0); button_->setEnabled(way_point_count >= 0); paused_ = false; slider_->setMaximum(max_way_point); - maximum_label_->setText(way_point_count >= 0 ? QString::number(max_way_point) : ""); + maximum_label_->setText(way_point_count >= 0 ? QString::number(way_point_count) : ""); slider_->setSliderPosition(0); } @@ -128,7 +130,8 @@ void TaskSolutionPanel::sliderValueChanged(int value) { QString text; if (!slider_->isEnabled()) text = ""; - else text = QString::number(value); + else if (empty_) text = value == 0 ? "S" : "E"; + else text = QString::number(value+1); minimum_label_->setText(text); } diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index b672117e..303368e0 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -411,13 +411,15 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) } int previous_state = current_state_; + // for an empty trajectory, show the start and end state at least int waypoint_count = displaying_solution_->getWayPointCount(); + int max_state_index = std::max(1, waypoint_count); 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) + } else if (current_state_ < max_state_index) animating_ = true; // auto-activate animation if slider_panel_ is hidden QString msg = "no change"; @@ -432,7 +434,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) trail_scene_node_->setVisible(false); msg = "restart"; } else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time - while (current_state_ < waypoint_count && + while (current_state_ < max_state_index && (tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) { ++current_state_; current_state_time_ -= tm; @@ -447,11 +449,12 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) msg = "slider"; } - if (current_state_ >= waypoint_count) { // animation finished? + if ((waypoint_count > 0 && current_state_ >= max_state_index) || + (waypoint_count == 0 && current_state_ > max_state_index)) { // animation finished? if (loop_display_property_->getBool()) current_state_ = -1; // restart in next cycle else { - current_state_ = waypoint_count; + current_state_ = max_state_index; if (slider_panel_) slider_panel_->pauseButton(true); } @@ -461,27 +464,31 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) if (current_state_ == previous_state) return; - if (current_state_ < waypoint_count) { - renderWayPoint(current_state_, previous_state); + renderWayPoint(current_state_, previous_state); + + // render trail up to current_state_ + int stepsize = trail_step_size_property_->getInt(); + for (int i = std::max(0, previous_state / stepsize), + end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) + trail_[i]->setVisible(true); - int stepsize = trail_step_size_property_->getInt(); - for (int i = std::max(0, previous_state / stepsize), - end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) - trail_[i]->setVisible(true); - } setVisibility(); } void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) { + assert(index >= 0); size_t waypoint_count = displaying_solution_->getWayPointCount(); moveit::core::RobotStateConstPtr robot_state; planning_scene::PlanningSceneConstPtr scene; if (index+1 >= waypoint_count) { - // render last state + if (index == 0 && waypoint_count == 0) + // special case: render start scene + scene = displaying_solution_->startScene(); + else // render end state scene = displaying_solution_->scene(waypoint_count); - renderPlanningScene (scene); - robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); + renderPlanningScene (scene); + robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); } else { auto idx_pair = displaying_solution_->indexPair(index); scene = displaying_solution_->scene(idx_pair); From cd88b224697a782415ccc78244da2fa3378c7feb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 11 Apr 2018 00:40:37 +0200 Subject: [PATCH 04/10] update trail in both directions: - show new steps if current_state_ advanced - hide steps if current_state_ reverted --- .../src/task_solution_visualization.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 303368e0..189a1993 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -267,9 +267,7 @@ void TaskSolutionVisualization::changedLoopDisplay() void TaskSolutionVisualization::changedTrail() { clearTrail(); - DisplaySolutionPtr t = next_solution_to_display_; - if (!t) - t = displaying_solution_; + DisplaySolutionPtr t = displaying_solution_; if (!t || !trail_display_property_->getBool()) { setVisibility(trail_scene_node_, main_scene_node_, false); @@ -280,8 +278,7 @@ void TaskSolutionVisualization::changedTrail() setVisibility(trail_scene_node_, main_scene_node_, true); int stepsize = trail_step_size_property_->getInt(); - // always include last trajectory point - trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize)); + trail_.resize(t->getWayPointCount() / stepsize); 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 @@ -466,11 +463,14 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt) renderWayPoint(current_state_, previous_state); - // render trail up to current_state_ + // show / hide trail between start .. end int stepsize = trail_step_size_property_->getInt(); - for (int i = std::max(0, previous_state / stepsize), - end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i) - trail_[i]->setVisible(true); + int start = std::max(0, previous_state / stepsize); + int end = current_state_ / stepsize; + bool show = start <= end; + if (!show) std::swap(start, end); + end = std::min(end, trail_.size()); + for (; start < end; ++start) trail_[start]->setVisible(show); setVisibility(); } From ade42456b86b08593340862de0d88c3d6949f272 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 23 Feb 2018 17:36:16 +0100 Subject: [PATCH 05/10] list of random todos --- core/include/moveit/task_constructor/task.h | 3 +++ core/src/stages/compute_ik.cpp | 3 +++ core/src/stages/move_relative.cpp | 2 ++ core/src/stages/move_to.cpp | 3 ++- 4 files changed, 10 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 04e754dc..6ac3fced 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -64,6 +64,7 @@ MOVEIT_CLASS_FORWARD(Task) */ class Task : protected WrapperBase { public: + // TODO: move into MoveIt! core static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model, const std::string &ns = "move_group", const std::string &planning_plugin_param_name = "planning_plugin", @@ -81,6 +82,7 @@ public: /// load robot model from given parameter void loadRobotModel(const std::string& robot_description = "robot_description"); + // TODO: use Stage::insert as well? void add(Stage::pointer &&stage); void clear() override; @@ -111,6 +113,7 @@ public: /// publish all top-level solutions void publishAllSolutions(bool wait = true); + // TODO: convenient access to arbitrary stage by name /// access stage tree ContainerBase *stages(); const ContainerBase *stages() const; diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 1281675e..7bfea6c6 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -116,6 +116,7 @@ typedef std::vector> IKSolutions; namespace { +// TODO: provide callback methods in PlanningScene class / probably not very useful here though... bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Affine3d pose, const robot_model::LinkModel* link) { @@ -215,6 +216,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff(); + // TODO: this should not be necessary in my opinion // enforced initialization from interface ensures that new target_pose is read properties().performInitFrom(INTERFACE, s.start()->properties(), true); const auto& props = properties(); @@ -376,6 +378,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) spawn(InterfaceState(scene), std::move(solution)); } + // TODO: magic constant should be a property instead ("current_seed_only", or equivalent) if (!succeeded && max_ik_solutions == 1) break; // first and only attempt failed } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 0f83286d..239b3ac4 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -241,10 +241,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning // add an arrow marker visualization_msgs::Marker m; + // TODO: make "marker" a common property of all stages m.ns = props.get("marker_ns"); if (!m.ns.empty()) { m.header.frame_id = scene->getPlanningFrame(); if (linear_norm > 1e-3) { + // TODO: arrow could be split into "valid" and "invalid" part rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN : rviz_marker_tools::RED); rviz_marker_tools::makeArrow(m, linear_norm); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 2c6b66d3..78978caa 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -48,7 +48,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan , planner_(planner) { auto& p = properties(); - p.declare("timeout", 10.0, "planning timeout"); + p.declare("timeout", 10.0, "planning timeout"); // TODO: make this private in Stage p.declare("group", "name of planning group"); p.declare("link", "", "link to move (default is tip of jmg)"); @@ -184,6 +184,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP return success; } +// TODO: move these as default implementation to PropagateEitherWay? bool MoveTo::computeForward(const InterfaceState &from){ planning_scene::PlanningScenePtr to; SubTrajectory trajectory; From 6d6f185870f768167b52aff309119cea2e925713 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 9 Apr 2018 21:41:34 +0200 Subject: [PATCH 06/10] feedback --- core/include/moveit/task_constructor/task.h | 4 ++-- core/src/stages/compute_ik.cpp | 8 ++++++-- core/src/stages/move_relative.cpp | 4 ++-- core/src/stages/move_to.cpp | 5 +++-- 4 files changed, 13 insertions(+), 8 deletions(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 6ac3fced..db2bb8af 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -64,7 +64,7 @@ MOVEIT_CLASS_FORWARD(Task) */ class Task : protected WrapperBase { public: - // TODO: move into MoveIt! core + // +1 TODO: move into MoveIt! core static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model, const std::string &ns = "move_group", const std::string &planning_plugin_param_name = "planning_plugin", @@ -113,7 +113,7 @@ public: /// publish all top-level solutions void publishAllSolutions(bool wait = true); - // TODO: convenient access to arbitrary stage by name + // +1 TODO: convenient access to arbitrary stage by name. traverse hierarchy using / separator? /// access stage tree ContainerBase *stages(); const ContainerBase *stages() const; diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 7bfea6c6..bbd2ff81 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -116,7 +116,8 @@ typedef std::vector> IKSolutions; namespace { -// TODO: provide callback methods in PlanningScene class / probably not very useful here though... +// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though... +// move into MoveIt! core bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Affine3d pose, const robot_model::LinkModel* link) { @@ -216,7 +217,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s) assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff(); - // TODO: this should not be necessary in my opinion + // -1 TODO: this should not be necessary in my opinion: Why do you think so? + // It is, because the properties on the interface might change from call to call... // enforced initialization from interface ensures that new target_pose is read properties().performInitFrom(INTERFACE, s.start()->properties(), true); const auto& props = properties(); @@ -379,6 +381,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s) } // TODO: magic constant should be a property instead ("current_seed_only", or equivalent) + // Yeah, you are right, these are two different semantic concepts: + // One could also have multiple IK solutions derived from the same seed if (!succeeded && max_ik_solutions == 1) break; // first and only attempt failed } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 239b3ac4..efe1d5ce 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -241,12 +241,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning // add an arrow marker visualization_msgs::Marker m; - // TODO: make "marker" a common property of all stages + // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" m.ns = props.get("marker_ns"); if (!m.ns.empty()) { m.header.frame_id = scene->getPlanningFrame(); if (linear_norm > 1e-3) { - // TODO: arrow could be split into "valid" and "invalid" part + // +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder) rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN : rviz_marker_tools::RED); rviz_marker_tools::makeArrow(m, linear_norm); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 78978caa..7b916f6f 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -48,7 +48,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan , planner_(planner) { auto& p = properties(); - p.declare("timeout", 10.0, "planning timeout"); // TODO: make this private in Stage + p.declare("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage p.declare("group", "name of planning group"); p.declare("link", "", "link to move (default is tip of jmg)"); @@ -184,7 +184,8 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP return success; } -// TODO: move these as default implementation to PropagateEitherWay? +// -1 TODO: move these as default implementation to PropagateEitherWay? +// Essentially, here compute() is a class-specific worker function bool MoveTo::computeForward(const InterfaceState &from){ planning_scene::PlanningScenePtr to; SubTrajectory trajectory; From 90b0b006ebf2d4f578045ff2d4ce74aa8a3862d7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 10 Apr 2018 02:03:40 +0200 Subject: [PATCH 07/10] more todo --- core/src/stages/move_relative.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index efe1d5ce..2c788418 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -122,6 +122,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning } // Cartesian targets require the link name + // TODO: use ik_frame property as in ComputeIK std::string link_name = props.get("link"); const moveit::core::LinkModel* link; if (link_name.empty()) From 433db67cf9b1377674991267aa9de29cf558c579 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 15 Apr 2018 11:04:04 +0200 Subject: [PATCH 08/10] only attempt to merge if there is something to merge --- .../moveit/task_constructor/stages/connect.h | 6 +++--- core/src/stages/connect.cpp | 13 +++++++++---- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 083afcfc..35e4df2f 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -76,9 +76,9 @@ public: protected: SolutionBase* storeSequential(const std::vector& sub_trajectories, const std::vector& intermediate_scenes); - robot_trajectory::RobotTrajectoryPtr merge(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state); + robot_trajectory::RobotTrajectoryConstPtr merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state); protected: GroupPlannerVector planner_; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index beeb19b6..60a86a18 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { // mark solution as failure solution->setCost(std::numeric_limits::infinity()); } else { - robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); + auto t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState()); if (t) { connect(from, to, SubTrajectory(t)); return true; @@ -218,11 +218,16 @@ SolutionBase* Connect::storeSequential(const std::vector& sub_trajectories, - const std::vector& intermediate_scenes, - const moveit::core::RobotState& state) +robot_trajectory::RobotTrajectoryConstPtr Connect::merge(const std::vector& sub_trajectories, + const std::vector& intermediate_scenes, + const moveit::core::RobotState& state) { + // no need to merge if there is only a single sub trajectory + if (sub_trajectories.size() == 1) + return sub_trajectories[0]; + auto jmg = merged_jmg_.get(); + assert(jmg); robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg); // TODO: check merged trajectory for collisions return trajectory; From 5c525f8729e4b372dbbe84294c72ae33093195ec Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 10 Apr 2018 12:34:43 +0200 Subject: [PATCH 09/10] consider v4hn's comment --- core/src/stages/connect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 60a86a18..b27f75b1 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -158,7 +158,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions); robot_trajectory::RobotTrajectoryPtr trajectory; - if (!pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints)) + if (!pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints)) break; sub_trajectories.push_back(trajectory); From b8a4653f317a51cd8d3b89970e29936bed706890 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 15 Apr 2018 11:44:10 +0200 Subject: [PATCH 10/10] relax equality condition for joint values for inactive groups --- core/src/stages/connect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index b27f75b1..a8f2735c 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -126,7 +126,7 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& const unsigned int num = jm->getVariableCount(); Eigen::Map positions_from (from.getJointPositions(jm), num); Eigen::Map positions_to (to.getJointPositions(jm), num); - if (!positions_from.array().isApprox(positions_to.array())) { + if (!positions_from.array().isApprox(positions_to.array(), 1e-4)) { ROS_INFO_STREAM_ONCE_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose() << "] != [" << positions_to.transpose() << "]");