From 1a2eafa2564cad4e23a28511fbdcafc96ec09f56 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 17 Sep 2020 23:00:12 +0200 Subject: [PATCH 01/67] fix stage test mock base class init needs to be called although this didn't pose problems until now. --- core/test/test_stage.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 519678d0..e39d450b 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -28,6 +28,7 @@ public: void init(const moveit::core::RobotModelConstPtr& robot_model) override { ps.reset((new PlanningScene(robot_model))); + Generator::init(robot_model); } bool canCompute() const override { return true; } From 9337743de2654895961033aa57b679bc1409a744 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 19 May 2020 16:25:30 +0200 Subject: [PATCH 02/67] add CostTerm interface basic stages can now be configured with arbitrary cost terms. --- .../moveit/task_constructor/container.h | 3 +++ core/include/moveit/task_constructor/stage.h | 6 ++++++ .../include/moveit/task_constructor/stage_p.h | 13 +++++++++++- core/src/stage.cpp | 21 ++++++++++++++++--- 4 files changed, 39 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 27b2fde0..8aa557a9 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -80,6 +80,9 @@ public: protected: ContainerBase(ContainerBasePrivate* impl); + +private: + using Stage::setCostTerm; }; std::ostream& operator<<(std::ostream& os, const ContainerBase& stage); diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 4b4fcd30..bcb9f1f3 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -159,6 +159,7 @@ public: PARENT = 2, INTERFACE = 4, }; + virtual ~Stage(); /// auto-convert Stage to StagePrivate* when needed @@ -212,6 +213,9 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); + using CostTerm = std::function; + void setCostTerm(const CostTerm&); + const ordered& solutions() const; const std::list& failures() const; size_t numFailures() const; @@ -227,8 +231,10 @@ public: void setProperty(const std::string& name, const boost::any& value); /// overload: const char* values are stored as std::string inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); } + /// analyze source of error and report accordingly [[noreturn]] void reportPropertyError(const Property::error& e); + double getTotalComputeTime() const; protected: diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 150080f5..3fbabfa0 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -154,8 +154,16 @@ public: total_compute_time_ += compute_stop_time - compute_start_time; } + /** compute cost for solution through configured CostTerm + * + * @return true if solution remains feasible (is no failure) + */ + bool addCost(SolutionBase& solution); + protected: - Stage* me_; // associated/owning Stage instance + // associated/owning Stage instance + Stage* me_; + std::string name_; PropertyMap properties_; @@ -163,6 +171,9 @@ protected: InterfacePtr starts_; InterfacePtr ends_; + // user-configurable cost estimator + Stage::CostTerm cost_term_; + // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 38761006..45932615 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -151,8 +151,9 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, solution->setStartState(from); solution->setEndState(*to_it); - if (!solution->isFailure()) + if (!solution->isFailure() && addCost(*solution)) { nextStarts()->add(*to_it); + } newSolution(solution); } @@ -168,7 +169,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, solution->setStartState(*from_it); solution->setEndState(to); - if (!solution->isFailure()) + if (!solution->isFailure() && addCost(*solution)) prevEnds()->add(*from_it); newSolution(solution); @@ -185,7 +186,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution solution->setStartState(*from); solution->setEndState(*to); - if (!solution->isFailure()) { + if (!solution->isFailure() && addCost(*solution)) { prevEnds()->add(*from); nextStarts()->add(*to); } @@ -200,6 +201,9 @@ void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, solution->setStartState(from); solution->setEndState(to); + if (!solution->isFailure()) + addCost(*solution); + newSolution(solution); } @@ -212,6 +216,13 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } +bool StagePrivate::addCost(SolutionBase& solution) { + if (cost_term_) + solution.setCost(cost_term_(solution)); + + return !solution.isFailure(); +} + Stage::Stage(StagePrivate* impl) : pimpl_(impl) { assert(impl); auto& p = properties(); @@ -308,6 +319,10 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } +void Stage::setCostTerm(const CostTerm& cost_term) { + pimpl()->cost_term_ = cost_term; +} + const ordered& Stage::solutions() const { return pimpl()->solutions_; } From dd9ce974ce17fc14d12ad052c524711fdbb667e1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 21 Sep 2020 21:43:34 +0200 Subject: [PATCH 03/67] tf2 compatibility for Noetic (#206) --- .../visualization_tools/CMakeLists.txt | 5 +++++ .../src/marker_visualization.cpp | 18 +++++++++++++----- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index c69429f9..7ee5bd16 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -2,6 +2,11 @@ set(MOVEIT_LIB_NAME moveit_task_visualization_tools) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_tools) +# TODO: Remove when Kinetic support is dropped +if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2? + add_definitions(-DRVIZ_TF1) +endif() + set(HEADERS ${PROJECT_INCLUDE}/display_solution.h ${PROJECT_INCLUDE}/marker_visualization.h diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index f8f00a0c..b3671772 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -7,6 +7,9 @@ #include #include #include +#ifndef RVIZ_TF1 +#include +#endif #include #include #include @@ -69,18 +72,23 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce Ogre::Vector3 pos; try { +#ifdef RVIZ_TF1 tf::TransformListener* tf = context->getFrameManager()->getTFClient(); - std::string error_msg; - if (!tf->canTransform(planning_frame_, fixed_frame, ros::Time(), &error_msg)) { - ROS_WARN_STREAM_NAMED("MarkerVisualization", error_msg); - return false; // frame transform not (yet) available - } tf::StampedTransform tm; tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm); auto q = tm.getRotation(); auto p = tm.getOrigin(); quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z()); pos = Ogre::Vector3(p.x(), p.y(), p.z()); +#else + std::shared_ptr tf = context->getFrameManager()->getTF2BufferPtr(); + geometry_msgs::TransformStamped tm; + tm = tf->lookupTransform(planning_frame_, fixed_frame, ros::Time()); + auto q = tm.transform.rotation; + auto p = tm.transform.translation; + quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z); + pos = Ogre::Vector3(p.x, p.y, p.z); +#endif } catch (const tf2::TransformException& e) { ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what()); return false; From 9eae554d89770938bca91adb8f1012107388f7ac Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 19 May 2020 16:54:31 +0200 Subject: [PATCH 04/67] provide Constant and PathLength cost terms --- .../moveit/task_constructor/cost_terms.h | 27 ++++++++ core/src/CMakeLists.txt | 1 + core/src/cost_terms.cpp | 61 +++++++++++++++++++ 3 files changed, 89 insertions(+) create mode 100644 core/include/moveit/task_constructor/cost_terms.h create mode 100644 core/src/cost_terms.cpp diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h new file mode 100644 index 00000000..586797ea --- /dev/null +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -0,0 +1,27 @@ +#pragma once + +#include + +namespace moveit { +namespace task_constructor { +namespace cost { + +/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() + +/// add a constant cost to each solution +class ConstantCost +{ +public: + ConstantCost(double cost) : cost_(cost) {} + + double operator()(const SolutionBase&) { return cost_; } + +private: + double cost_; +}; + +/// define cost as execution duration of the whole trajectory +double PathLengthCost(const SolutionBase& s); +} +} +} diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index cf8b6ad1..263c5033 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -19,6 +19,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/solvers/pipeline_planner.h container.cpp + cost_terms.cpp introspection.cpp marker_tools.cpp merge.cpp diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp new file mode 100644 index 00000000..cc0f3758 --- /dev/null +++ b/core/src/cost_terms.cpp @@ -0,0 +1,61 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holders nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner */ + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace cost { + +double PathLengthCost(const SolutionBase& s) { + auto* trajectory{ dynamic_cast(&s) }; + + if (trajectory) { + return trajectory->trajectory() ? trajectory->trajectory()->getDuration() : 0.0; + } else { + // there is no other alternative implementation of SolutionBase so static_cast is safe + auto& seq{ *static_cast(&s) }; + + double cost{ 0.0 }; + for (auto& sub : seq.solutions()) + cost += PathLengthCost(*sub); + return cost; + } +} +} +} +} From ffc62c2d1123dd339e8e47d99ad133a30ed7e3dd Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 19 May 2020 16:59:12 +0200 Subject: [PATCH 05/67] add explicit default CostTerms to most basic stages Simplifies code and makes the costs of each stage explicit. --- core/src/stages/connect.cpp | 28 ++++++----------------- core/src/stages/fix_collision_objects.cpp | 5 ++++ core/src/stages/fixed_cartesian_poses.cpp | 4 ++++ core/src/stages/fixed_state.cpp | 6 ++++- core/src/stages/generate_pose.cpp | 3 +++ core/src/stages/modify_planning_scene.cpp | 6 ++++- core/src/stages/move_relative.cpp | 11 ++++----- core/src/stages/move_to.cpp | 11 ++++----- 8 files changed, 37 insertions(+), 37 deletions(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0d07f52d..8d38da55 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -40,12 +40,16 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); + setCostTerm(cost::PathLengthCost); + auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); p.declare("path_constraints", moveit_msgs::Constraints(), @@ -181,15 +185,6 @@ Connect::makeSequential(const std::vectorgetWayPointDurations()) - cost += distance; - } - SolutionSequence::container_type sub_solutions; for (const auto& sub : sub_trajectories) { planning_scene::PlanningSceneConstPtr end_ps = *++scene_it; @@ -212,24 +207,15 @@ Connect::makeSequential(const std::vector(std::move(sub_solutions), cost); + return std::make_shared(std::move(sub_solutions)); } SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, const std::vector& intermediate_scenes, const moveit::core::RobotState& state) { - // calculate cost - double cost = 0; - for (const auto& trajectory : sub_trajectories) { - if (!trajectory) - continue; - for (const double& distance : trajectory->getWayPointDurations()) - cost += distance; - } - // no need to merge if there is only a single sub trajectory if (sub_trajectories.size() == 1) - return std::make_shared(sub_trajectories[0], cost); + return std::make_shared(sub_trajectories[0]); auto jmg = merged_jmg_.get(); assert(jmg); @@ -242,7 +228,7 @@ SubTrajectoryPtr Connect::merge(const std::vector("path_constraints"))) return SubTrajectoryPtr(); - return std::make_shared(trajectory, cost); + return std::make_shared(trajectory); } } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 2e518608..0d0f9cc8 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -40,6 +40,8 @@ #include #include +#include + #include #include #include @@ -53,6 +55,9 @@ namespace task_constructor { namespace stages { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { + // TODO: possibly weight solutions based on the required displacement? + setCostTerm(cost::ConstantCost(0.0)); + auto& p = properties(); p.declare("max_penetration", "maximally corrected penetration depth"); p.declare("direction", "direction vector to use for corrections"); diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index aecd16ee..a78c2414 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -36,7 +36,9 @@ #include #include +#include #include + #include #include @@ -47,6 +49,8 @@ namespace stages { using PosesList = std::vector; FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { + setCostTerm(cost::ConstantCost(0.0)); + auto& p = properties(); p.declare("poses", PosesList(), "target poses to spawn"); } diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index b6a2aa5a..d76d37ac 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -35,13 +35,17 @@ /* Authors: Robert Haschke */ #include +#include + #include namespace moveit { namespace task_constructor { namespace stages { -FixedState::FixedState(const std::string& name) : Generator(name) {} +FixedState::FixedState(const std::string& name) : Generator(name) { + setCostTerm(cost::ConstantCost(0.0)); +} void FixedState::setState(const planning_scene::PlanningScenePtr& scene) { scene_ = scene; diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index ebcee60c..85efbc44 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -45,6 +46,8 @@ namespace task_constructor { namespace stages { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { + setCostTerm(cost::ConstantCost(0.0)); + auto& p = properties(); p.declare("pose", "target pose to pass on in spawned states"); } diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index f57650b8..36a45753 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -38,13 +38,17 @@ #include #include +#include + #include namespace moveit { namespace task_constructor { namespace stages { -ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {} +ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { + setCostTerm(cost::ConstantCost(0.0)); +} void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) { auto it_inserted = attach_objects_.insert(std::make_pair(attach_link, std::make_pair(Names(), attach))); diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index b5544a0e..da398dcd 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -37,6 +37,8 @@ */ #include +#include + #include #include #include @@ -47,6 +49,8 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { + setCostTerm(cost::PathLengthCost); + auto& p = properties(); p.property("timeout").setDefaultValue(1.0); p.declare("group", "name of planning group"); @@ -291,13 +295,6 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning robot_trajectory->reverse(); solution.setTrajectory(robot_trajectory); - // set cost - double cost = 0; - for (const double& distance : robot_trajectory->getWayPointDurations()) { - cost += distance; - } - solution.setCost(cost); - if (!success) solution.markAsFailure(); return true; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index a3c09a36..21db9815 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -37,6 +37,8 @@ */ #include +#include + #include #include #include @@ -48,6 +50,8 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { + setCostTerm(cost::PathLengthCost); + auto& p = properties(); p.property("timeout").setDefaultValue(1.0); p.declare("group", "name of planning group"); @@ -254,13 +258,6 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP robot_trajectory->reverse(); solution.setTrajectory(robot_trajectory); - // set cost - double cost = 0; - for (const double& distance : robot_trajectory->getWayPointDurations()) { - cost += distance; - } - solution.setCost(cost); - if (!success) solution.markAsFailure(); From f79625d1bd54be2fedc9e843155f53c9ca1afeec Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 20 May 2020 14:26:33 +0200 Subject: [PATCH 06/67] fully restrict CostTerms to primitive solutions simplify implementations. --- .../include/moveit/task_constructor/cost_terms.h | 5 ++--- core/include/moveit/task_constructor/stage.h | 3 +-- core/src/cost_terms.cpp | 16 ++-------------- core/src/stage.cpp | 9 +++++---- 4 files changed, 10 insertions(+), 23 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 586797ea..a1ad49ed 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -7,21 +7,20 @@ namespace task_constructor { namespace cost { /// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() - /// add a constant cost to each solution class ConstantCost { public: ConstantCost(double cost) : cost_(cost) {} - double operator()(const SolutionBase&) { return cost_; } + double operator()(const SubTrajectory&) { return cost_; } private: double cost_; }; /// define cost as execution duration of the whole trajectory -double PathLengthCost(const SolutionBase& s); +double PathLengthCost(const SubTrajectory& s); } } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index bcb9f1f3..2336b633 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -213,8 +213,7 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTerm = std::function; - void setCostTerm(const CostTerm&); + using CostTerm = std::function; const ordered& solutions() const; const std::list& failures() const; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index cc0f3758..a508103a 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -41,20 +41,8 @@ namespace moveit { namespace task_constructor { namespace cost { -double PathLengthCost(const SolutionBase& s) { - auto* trajectory{ dynamic_cast(&s) }; - - if (trajectory) { - return trajectory->trajectory() ? trajectory->trajectory()->getDuration() : 0.0; - } else { - // there is no other alternative implementation of SolutionBase so static_cast is safe - auto& seq{ *static_cast(&s) }; - - double cost{ 0.0 }; - for (auto& sub : seq.solutions()) - cost += PathLengthCost(*sub); - return cost; - } +double PathLengthCost(const SubTrajectory& s) { + return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 45932615..0e29e674 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -217,8 +217,9 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { } bool StagePrivate::addCost(SolutionBase& solution) { - if (cost_term_) - solution.setCost(cost_term_(solution)); + auto* trajectory{ dynamic_cast(&solution) }; + if (trajectory && cost_term_) + solution.setCost(cost_term_(*trajectory)); return !solution.isFailure(); } @@ -319,8 +320,8 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } -void Stage::setCostTerm(const CostTerm& cost_term) { - pimpl()->cost_term_ = cost_term; +void Stage::setCostTerm(const CostTerm& term) { + pimpl()->cost_term_ = term; } const ordered& Stage::solutions() const { From f3fec26f5a3281996a037d23a259a916faa260a9 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 20 May 2020 14:27:33 +0200 Subject: [PATCH 07/67] add CostTransform CostTerms only apply to primitive solutions and generalizing them to Containers would make them quite unintuitive (and adds overhead). Instead CostTransform can be used in any container to scale, crop, square the cost of the solutions. I initially thought about adding scaling factors, but then again, other transforms are of interest just as well. --- core/include/moveit/task_constructor/stage.h | 21 +++++++++++++++++++ .../include/moveit/task_constructor/stage_p.h | 1 + core/src/stage.cpp | 21 +++++++++++++++++-- 3 files changed, 41 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 2336b633..397a56a6 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -214,6 +214,27 @@ public: void removeSolutionCallback(SolutionCallbackList::const_iterator which); using CostTerm = std::function; + using CostTransform = std::function; + + /* \brief set method to determine costs for solutions of this stage + * + * This interface supports only primitive solutions, no containers. + * + * In order to support containers, the associated CostTerms would need to be able + * to decide costs for arbitrary partial solutions of the container. + * These need to be determined for scheduling, but would make the terms too complex. + * + * See setCostTransform + */ + void setCostTerm(const CostTerm& term); + + /* \brief set CostTransform to be applied to costs of this stage's solution costs + * + * This interface can be used to modify the cost associated with each solution of this stage/container. + * The most common case would be linear weighting `[=a](double c){ return a*c; }` + * but any non-linear transform is obviously possible as well. + */ + void setCostTransform(const CostTransform& transform); const ordered& solutions() const; const std::list& failures() const; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 3fbabfa0..ad0f1bfa 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -173,6 +173,7 @@ protected: // user-configurable cost estimator Stage::CostTerm cost_term_; + Stage::CostTransform cost_transform_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 0e29e674..37396f8f 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -97,7 +97,12 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) { } StagePrivate::StagePrivate(Stage* me, const std::string& name) - : me_(me), name_(name), total_compute_time_{}, parent_(nullptr), introspection_(nullptr) {} + : me_(me) + , name_(name) + , cost_transform_([](auto x) { return x; }) + , total_compute_time_{} + , parent_(nullptr) + , introspection_(nullptr) {} InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; @@ -217,9 +222,17 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { } bool StagePrivate::addCost(SolutionBase& solution) { + double cost{ 0.0 }; + auto* trajectory{ dynamic_cast(&solution) }; if (trajectory && cost_term_) - solution.setCost(cost_term_(*trajectory)); + // CostTerm only applies to SubTrajectory + cost = cost_term_(*trajectory); + else + // everything else is just transformed + cost = solution.cost(); + + solution.setCost(cost_transform_(cost)); return !solution.isFailure(); } @@ -324,6 +337,10 @@ void Stage::setCostTerm(const CostTerm& term) { pimpl()->cost_term_ = term; } +void Stage::setCostTransform(const CostTransform& transform) { + pimpl()->cost_transform_ = transform; +} + const ordered& Stage::solutions() const { return pimpl()->solutions_; } From 9060404037b9936d059a296d888a64b0e9da92d2 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 19 Jun 2020 23:07:57 +0200 Subject: [PATCH 08/67] improve comments --- core/include/moveit/task_constructor/cost_terms.h | 2 +- core/include/moveit/task_constructor/stage.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index a1ad49ed..7a7f942b 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -19,7 +19,7 @@ private: double cost_; }; -/// define cost as execution duration of the whole trajectory +/// execution duration of the whole trajectory double PathLengthCost(const SubTrajectory& s); } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 397a56a6..5a229f9d 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -231,7 +231,7 @@ public: /* \brief set CostTransform to be applied to costs of this stage's solution costs * * This interface can be used to modify the cost associated with each solution of this stage/container. - * The most common case would be linear weighting `[=a](double c){ return a*c; }` + * The most common case would be linear weighting `[](double cost){ return constant*cost; }` * but any non-linear transform is obviously possible as well. */ void setCostTransform(const CostTransform& transform); From 56268cb6cc0f51dfa6668e29084e4182e511f8c2 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 19 Jun 2020 23:08:12 +0200 Subject: [PATCH 09/67] first attempts on ClearanceCost --- .../moveit/task_constructor/cost_terms.h | 3 ++ core/src/cost_terms.cpp | 36 +++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 7a7f942b..821afd94 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -21,6 +21,9 @@ private: /// execution duration of the whole trajectory double PathLengthCost(const SubTrajectory& s); + +/// distance to self-collision +double ClearanceCost(const SubTrajectory& s); } } } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index a508103a..00f3754f 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -35,7 +35,12 @@ /* Authors: Michael Goerner */ #include +#include + #include +#include + +#include namespace moveit { namespace task_constructor { @@ -44,6 +49,37 @@ namespace cost { double PathLengthCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } + +double ClearanceCost(const SubTrajectory& s) { + collision_detection::DistanceRequest request; + request.type = collision_detection::DistanceRequestType::GLOBAL; + + // TODO: possibly parameterize hardcoded property name? + const std::string group{ "group" }; + auto& state_properties{ s.start()->properties() }; + auto& stage_properties{ s.creator()->properties() }; + request.group_name = state_properties.hasProperty(group) ? state_properties.get(group) : + stage_properties.get(group); + + request.enableGroup(s.start()->scene()->getRobotModel()); + request.acm = &s.start()->scene()->getAllowedCollisionMatrix(); + + collision_detection::DistanceResult result; + + s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState()); + + if (result.minimum_distance.distance <= 0) { + // TODO: this should be a comment in the solution + ROS_ERROR_STREAM_NAMED("ClearanceCost", "allegedly valid solution has an unwanted collide between '" + << result.minimum_distance.link_names[0] << "' and '" + << result.minimum_distance.link_names[1] << "'"); + return std::numeric_limits::infinity(); + } else { + // ROS_INFO_STREAM_NAMED("ClearanceCost", result.minimum_distance.link_names[0] << " has distance " << + // result.minimum_distance.distance << " to " << result.minimum_distance.link_names[1]); + return 1.0 / (result.minimum_distance.distance + 1e-5); + } +} } } } From a15204b40b799aead184bf5f687c4e57d163560b Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 29 Jun 2020 22:24:35 +0200 Subject: [PATCH 10/67] allow wrappers to use CostTerm --- core/include/moveit/task_constructor/container.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 8aa557a9..f1767f98 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -81,7 +81,7 @@ public: protected: ContainerBase(ContainerBasePrivate* impl); -private: + /// most containers should only use setCostTransform and leave the costs to their children using Stage::setCostTerm; }; std::ostream& operator<<(std::ostream& os, const ContainerBase& stage); @@ -227,6 +227,9 @@ public: bool canCompute() const override; void compute() override; + // Wrappers sometimes do the real work (e.g., IK), so they can specify costs + using Stage::setCostTerm; + protected: WrapperBase(WrapperBasePrivate* impl, Stage::pointer&& child = Stage::pointer()); }; From f9ba3027287ca5d503cfe715e08f13b284902d17 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 09:53:58 +0200 Subject: [PATCH 11/67] enable costs to (optionally) return a comment With more complicated costs (such as the ClearanceCost), it's useful to get comments. --- .../moveit/task_constructor/cost_terms.h | 2 +- core/include/moveit/task_constructor/stage.h | 5 +++- core/src/cost_terms.cpp | 18 +++++++++------ core/src/stage.cpp | 23 +++++++++++++++---- 4 files changed, 34 insertions(+), 14 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 821afd94..91c0ab8f 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -23,7 +23,7 @@ private: double PathLengthCost(const SubTrajectory& s); /// distance to self-collision -double ClearanceCost(const SubTrajectory& s); +double ClearanceCost(const SubTrajectory& s, std::string& comment); } } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 5a229f9d..857f2012 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -213,7 +213,9 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTerm = std::function; + using CostTerm = std::function; + using CostTermShort = std::function; + using CostTransform = std::function; /* \brief set method to determine costs for solutions of this stage @@ -227,6 +229,7 @@ public: * See setCostTransform */ void setCostTerm(const CostTerm& term); + void setCostTerm(const CostTermShort& term); /* \brief set CostTransform to be applied to costs of this stage's solution costs * diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 00f3754f..769060c9 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -42,6 +42,8 @@ #include +#include + namespace moveit { namespace task_constructor { namespace cost { @@ -50,7 +52,7 @@ double PathLengthCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double ClearanceCost(const SubTrajectory& s) { +double ClearanceCost(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = collision_detection::DistanceRequestType::GLOBAL; @@ -68,15 +70,17 @@ double ClearanceCost(const SubTrajectory& s) { s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState()); + const auto& links = result.minimum_distance.link_names; + if (result.minimum_distance.distance <= 0) { - // TODO: this should be a comment in the solution - ROS_ERROR_STREAM_NAMED("ClearanceCost", "allegedly valid solution has an unwanted collide between '" - << result.minimum_distance.link_names[0] << "' and '" - << result.minimum_distance.link_names[1] << "'"); + boost::format desc("ClearCost: allegedly valid solution has an unwanted collide between '%1%' and '%2%'"); + desc % links[0] % links[1]; + comment = desc.str(); return std::numeric_limits::infinity(); } else { - // ROS_INFO_STREAM_NAMED("ClearanceCost", result.minimum_distance.link_names[0] << " has distance " << - // result.minimum_distance.distance << " to " << result.minimum_distance.link_names[1]); + boost::format desc("ClearCost: distance %1% between'%2%' and '%3%'"); + desc % result.minimum_distance.distance % links[0] % links[1]; + comment = desc.str(); return 1.0 / (result.minimum_distance.distance + 1e-5); } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 37396f8f..f5ced345 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -225,13 +225,22 @@ bool StagePrivate::addCost(SolutionBase& solution) { double cost{ 0.0 }; auto* trajectory{ dynamic_cast(&solution) }; - if (trajectory && cost_term_) - // CostTerm only applies to SubTrajectory - cost = cost_term_(*trajectory); - else - // everything else is just transformed + if (trajectory && cost_term_) { + // compute specific cost + std::string comment; + cost = cost_term_(*trajectory, comment); + // If a comment was specified, add it to the solution + if (!comment.empty() && !solution.comment().empty()) { + solution.setComment(solution.comment() + " (" + comment + ")"); + } else if (!comment.empty()) { + solution.setComment(comment); + } + } else { + // otherwise forward current cost cost = solution.cost(); + } + // all costs are transformed solution.setCost(cost_transform_(cost)); return !solution.isFailure(); @@ -337,6 +346,10 @@ void Stage::setCostTerm(const CostTerm& term) { pimpl()->cost_term_ = term; } +void Stage::setCostTerm(const CostTermShort& term) { + setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); +} + void Stage::setCostTransform(const CostTransform& transform) { pimpl()->cost_transform_ = transform; } From cf0e4e3b017daafd8a3c8a944438b5e43566f12e Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 11:10:33 +0200 Subject: [PATCH 12/67] establish struct cost pattern for costs with multiple parameters, this option-style is more useful than verbose getter/setter. There is nothing to hide here. --- core/include/moveit/task_constructor/cost_terms.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 91c0ab8f..2c2d7af4 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -8,15 +8,14 @@ namespace cost { /// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() /// add a constant cost to each solution -class ConstantCost +struct ConstantCost { public: - ConstantCost(double cost) : cost_(cost) {} + ConstantCost(double c) : cost(c) {} - double operator()(const SubTrajectory&) { return cost_; } + double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; } -private: - double cost_; + double cost; }; /// execution duration of the whole trajectory From 18ed51d7f3e26dd092e665039f3319eec1bf20b3 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 11:13:58 +0200 Subject: [PATCH 13/67] turn Clearance cost into parameterized struct There is a lot of variations for how to compute clearance. --- .../moveit/task_constructor/cost_terms.h | 20 ++++++++++++++-- core/src/cost_terms.cpp | 24 +++++++++++-------- 2 files changed, 32 insertions(+), 12 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 2c2d7af4..b8ac02c5 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -21,8 +21,24 @@ public: /// execution duration of the whole trajectory double PathLengthCost(const SubTrajectory& s); -/// distance to self-collision -double ClearanceCost(const SubTrajectory& s, std::string& comment); +/** inverse distance to collision + * + * \arg interface compute distances using START or END interface of solution + * \arg with_world check distances to world objects as well or only look at self-collisions + * \arg group_property the name of the property which defines the group to look at + * */ +struct ClearanceCost +{ + ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, + std::string group_property = "group") + : interface(interface), with_world(with_world), group_property(group_property) {} + + double operator()(const SubTrajectory& s, std::string& comment); + + Interface::Direction interface; + bool with_world; + std::string group_property; +}; } } } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 769060c9..2b3e6b92 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -52,23 +52,27 @@ double PathLengthCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double ClearanceCost(const SubTrajectory& s, std::string& comment) { +double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = collision_detection::DistanceRequestType::GLOBAL; - // TODO: possibly parameterize hardcoded property name? - const std::string group{ "group" }; - auto& state_properties{ s.start()->properties() }; - auto& stage_properties{ s.creator()->properties() }; - request.group_name = state_properties.hasProperty(group) ? state_properties.get(group) : - stage_properties.get(group); + const auto& state = (interface == Interface::START) ? s.start() : s.end(); - request.enableGroup(s.start()->scene()->getRobotModel()); - request.acm = &s.start()->scene()->getAllowedCollisionMatrix(); + // prefer interface state property over stage property to find group_name + // TODO: This pattern is general enough to justify its own interface (in the properties?). + auto& state_properties{ state->properties() }; + auto& stage_properties{ s.creator()->properties() }; + request.group_name = state_properties.hasProperty(group_property) ? + state_properties.get(group_property) : + stage_properties.get(group_property); + + // look at all forbidden collisions involving group_name + request.enableGroup(state->scene()->getRobotModel()); + request.acm = &state->scene()->getAllowedCollisionMatrix(); collision_detection::DistanceResult result; - s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState()); + state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState()); const auto& links = result.minimum_distance.link_names; From a92753dafcf9af8e0bcaa0a915edcee31b7ac629 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 11:31:17 +0200 Subject: [PATCH 14/67] add cumulative distance to ClearanceCost --- .../moveit/task_constructor/cost_terms.h | 4 ++- core/src/cost_terms.cpp | 25 +++++++++++++++---- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index b8ac02c5..c32c192c 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -25,11 +25,12 @@ double PathLengthCost(const SubTrajectory& s); * * \arg interface compute distances using START or END interface of solution * \arg with_world check distances to world objects as well or only look at self-collisions + * \arg cumulative if true, compute clearance as aggregated distance of all bodies * \arg group_property the name of the property which defines the group to look at * */ struct ClearanceCost { - ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, + ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, std::string group_property = "group") : interface(interface), with_world(with_world), group_property(group_property) {} @@ -37,6 +38,7 @@ struct ClearanceCost Interface::Direction interface; bool with_world; + bool cumulative; std::string group_property; }; } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 2b3e6b92..e7edd2e2 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -54,7 +54,8 @@ double PathLengthCost(const SubTrajectory& s) { double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; - request.type = collision_detection::DistanceRequestType::GLOBAL; + request.type = + cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; const auto& state = (interface == Interface::START) ? s.start() : s.end(); @@ -74,6 +75,16 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState()); + double distance{ 0.0 }; + if (cumulative) { + for (const auto& distance_of_pair : result.distances) { + assert(distance_of_pair.second.size() == 1); + distance += distance_of_pair.second[0].distance; + } + } else { + distance = result.minimum_distance.distance; + } + const auto& links = result.minimum_distance.link_names; if (result.minimum_distance.distance <= 0) { @@ -82,10 +93,14 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { comment = desc.str(); return std::numeric_limits::infinity(); } else { - boost::format desc("ClearCost: distance %1% between'%2%' and '%3%'"); - desc % result.minimum_distance.distance % links[0] % links[1]; - comment = desc.str(); - return 1.0 / (result.minimum_distance.distance + 1e-5); + if (cumulative) { + comment = "ClearCost: cumulative distance " + std::to_string(distance); + } else { + boost::format desc("ClearCost: distance %1% between '%2%' and '%3%'"); + desc % result.minimum_distance.distance % links[0] % links[1]; + comment = desc.str(); + } + return 1.0 / (distance + 1e-5); } } } From 6dbc742f2071cff78bf1a054f05bf5edd41b5b53 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 17:01:24 +0200 Subject: [PATCH 15/67] implement correct PathLengthCost The previous implementation depends on the dynamics limits of the robot, which might be interesting in some cases, but shouldn't be a default anywhere. --- core/include/moveit/task_constructor/cost_terms.h | 5 ++++- core/src/cost_terms.cpp | 12 ++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index c32c192c..5a6bc8ed 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -18,9 +18,12 @@ public: double cost; }; -/// execution duration of the whole trajectory +/// trajectory length (interpolated between waypoints) double PathLengthCost(const SubTrajectory& s); +/// execution duration of the whole trajectory +double TrajectoryDurationCost(const SubTrajectory& s); + /** inverse distance to collision * * \arg interface compute distances using START or END interface of solution diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index e7edd2e2..49a16d1f 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -49,6 +49,18 @@ namespace task_constructor { namespace cost { double PathLengthCost(const SubTrajectory& s) { + const auto& traj = s.trajectory(); + + if (traj == nullptr) + return 0.0; + + double path_length{ 0.0 }; + for (size_t i = 1; i < traj->getWayPointCount(); ++i) + path_length += traj->getWayPoint(i - 1).distance(traj->getWayPoint(i)); + return path_length; +} + +double TrajectoryDurationCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } From 9a96a7e4f66d7691f4a5af3480302583dae0839e Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 20:39:56 +0200 Subject: [PATCH 16/67] implement LinkMotionCost --- .../moveit/task_constructor/cost_terms.h | 9 ++++++++ core/src/cost_terms.cpp | 23 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 5a6bc8ed..d16b6722 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -24,6 +24,15 @@ double PathLengthCost(const SubTrajectory& s); /// execution duration of the whole trajectory double TrajectoryDurationCost(const SubTrajectory& s); +struct LinkMotionCost +{ + LinkMotionCost(std::string link_name) : link_name(link_name) {} + + double operator()(const SubTrajectory&, std::string&); + + std::string link_name; +}; + /** inverse distance to collision * * \arg interface compute distances using START or END interface of solution diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 49a16d1f..bbdad413 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -64,6 +64,29 @@ double TrajectoryDurationCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } +double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) { + const auto& traj{ s.trajectory() }; + + if (traj == nullptr || traj->getWayPointCount() == 0) + return 0.0; + + if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) { + boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory"); + desc % link_name; + comment = desc.str(); + return std::numeric_limits::infinity(); + } + + double distance{ 0.0 }; + Eigen::Translation3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; + for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) { + Eigen::Translation3d new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; + distance += (new_position.vector() - position.vector()).norm(); + position = new_position; + } + return distance; +} + double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = From 14c885621f9d6e54cf1e4a762442974cae02ec7a Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 21:02:53 +0200 Subject: [PATCH 17/67] remove Cost suffix for elements in mtc::cost:: --- .../moveit/task_constructor/cost_terms.h | 20 +++++++++---------- core/src/cost_terms.cpp | 8 ++++---- core/src/stages/connect.cpp | 2 +- core/src/stages/fix_collision_objects.cpp | 2 +- core/src/stages/fixed_cartesian_poses.cpp | 2 +- core/src/stages/fixed_state.cpp | 2 +- core/src/stages/generate_pose.cpp | 2 +- core/src/stages/modify_planning_scene.cpp | 2 +- core/src/stages/move_relative.cpp | 2 +- core/src/stages/move_to.cpp | 2 +- 10 files changed, 22 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index d16b6722..8f551664 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -4,14 +4,14 @@ namespace moveit { namespace task_constructor { +/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() namespace cost { -/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() /// add a constant cost to each solution -struct ConstantCost +struct Constant { public: - ConstantCost(double c) : cost(c) {} + Constant(double c) : cost(c) {} double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; } @@ -19,14 +19,14 @@ public: }; /// trajectory length (interpolated between waypoints) -double PathLengthCost(const SubTrajectory& s); +double PathLength(const SubTrajectory& s); /// execution duration of the whole trajectory -double TrajectoryDurationCost(const SubTrajectory& s); +double TrajectoryDuration(const SubTrajectory& s); -struct LinkMotionCost +struct LinkMotion { - LinkMotionCost(std::string link_name) : link_name(link_name) {} + LinkMotion(std::string link_name) : link_name(link_name) {} double operator()(const SubTrajectory&, std::string&); @@ -40,10 +40,10 @@ struct LinkMotionCost * \arg cumulative if true, compute clearance as aggregated distance of all bodies * \arg group_property the name of the property which defines the group to look at * */ -struct ClearanceCost +struct Clearance { - ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, - std::string group_property = "group") + Clearance(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, + std::string group_property = "group") : interface(interface), with_world(with_world), group_property(group_property) {} double operator()(const SubTrajectory& s, std::string& comment); diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index bbdad413..5dc139d3 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -48,7 +48,7 @@ namespace moveit { namespace task_constructor { namespace cost { -double PathLengthCost(const SubTrajectory& s) { +double PathLength(const SubTrajectory& s) { const auto& traj = s.trajectory(); if (traj == nullptr) @@ -60,11 +60,11 @@ double PathLengthCost(const SubTrajectory& s) { return path_length; } -double TrajectoryDurationCost(const SubTrajectory& s) { +double TrajectoryDuration(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) { +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -87,7 +87,7 @@ double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) return distance; } -double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { +double Clearance::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 8d38da55..cb66a78d 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -48,7 +48,7 @@ namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 0d0f9cc8..05b820e8 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -56,7 +56,7 @@ namespace stages { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { // TODO: possibly weight solutions based on the required displacement? - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); auto& p = properties(); p.declare("max_penetration", "maximally corrected penetration depth"); diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index a78c2414..d6f0d96a 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -49,7 +49,7 @@ namespace stages { using PosesList = std::vector; FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); auto& p = properties(); p.declare("poses", PosesList(), "target poses to spawn"); diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index d76d37ac..971b6da6 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -44,7 +44,7 @@ namespace task_constructor { namespace stages { FixedState::FixedState(const std::string& name) : Generator(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); } void FixedState::setState(const planning_scene::PlanningScenePtr& scene) { diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index 85efbc44..ebdb73ba 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -46,7 +46,7 @@ namespace task_constructor { namespace stages { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); auto& p = properties(); p.declare("pose", "target pose to pass on in spawned states"); diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 36a45753..8343151e 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -47,7 +47,7 @@ namespace task_constructor { namespace stages { ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); } void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index da398dcd..218cd721 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -49,7 +49,7 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 21db9815..c151f580 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -50,7 +50,7 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); From 7a00ac646e9ce8bda640e96f611b3241c18d88ce Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 16 Jul 2020 15:39:57 +0200 Subject: [PATCH 18/67] add missing license --- .../moveit/task_constructor/cost_terms.h | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 8f551664..2b46813d 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -1,3 +1,41 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hamburg University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael 'v4hn' Goerner + Desc: Define implementations for general CostTerm's to use with Stage::setCostTerm() +*/ + #pragma once #include From d2bd0a00377e58ff55cea338eb193b0b70dd8893 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 16 Jul 2020 16:07:43 +0200 Subject: [PATCH 19/67] generalize Clearance cost - can now be used to estimate cost for either interface state or the trajectory - Introduced Interface::Direction NONE as a way of pointing to the trajectory in contrast to START or END --- .../moveit/task_constructor/cost_terms.h | 13 +-- .../include/moveit/task_constructor/storage.h | 1 + core/src/cost_terms.cpp | 85 +++++++++++++------ 3 files changed, 68 insertions(+), 31 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 2b46813d..26140060 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -73,23 +73,24 @@ struct LinkMotion /** inverse distance to collision * - * \arg interface compute distances using START or END interface of solution - * \arg with_world check distances to world objects as well or only look at self-collisions + * \arg with_world check distances to world objects or look at self-collisions * \arg cumulative if true, compute clearance as aggregated distance of all bodies * \arg group_property the name of the property which defines the group to look at + * \arg interface compute distances using START or END interface of solution *only*, instead of averaging over + * trajectory * */ struct Clearance { - Clearance(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, - std::string group_property = "group") - : interface(interface), with_world(with_world), group_property(group_property) {} + Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group", + Interface::Direction interface = Interface::NONE) + : with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {} double operator()(const SubTrajectory& s, std::string& comment); - Interface::Direction interface; bool with_world; bool cumulative; std::string group_property; + Interface::Direction interface; }; } } diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 3bf0e08f..581602ca 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -163,6 +163,7 @@ public: enum Direction { + NONE = 0, FORWARD, BACKWARD, START = FORWARD, diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 5dc139d3..12cf4d55 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -88,11 +88,13 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { } double Clearance::operator()(const SubTrajectory& s, std::string& comment) { + const std::string PREFIX{ "Clearance: " }; + collision_detection::DistanceRequest request; request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; - const auto& state = (interface == Interface::START) ? s.start() : s.end(); + const auto& state{ (interface == Interface::END) ? s.end() : s.start() }; // prefer interface state property over stage property to find group_name // TODO: This pattern is general enough to justify its own interface (in the properties?). @@ -106,37 +108,70 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) { request.enableGroup(state->scene()->getRobotModel()); request.acm = &state->scene()->getAllowedCollisionMatrix(); - collision_detection::DistanceResult result; + // compute relevant distance data for state & robot + auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { + collision_detection::DistanceResult result; + if (with_world) + state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); + else + state->scene()->getCollisionEnv()->distanceSelf(request, result, robot); - state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState()); + if (result.minimum_distance.distance <= 0) { + return result.minimum_distance; + } + + if (cumulative) { + double distance{ 0.0 }; + for (const auto& distance_of_pair : result.distances) { + assert(distance_of_pair.second.size() == 1); + distance += distance_of_pair.second[0].distance; + } + result.minimum_distance.distance = distance; + } + + return result.minimum_distance; + } }; + + auto collision_comment{ [=](const auto& distance) { + boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" }; + desc % distance.link_names[0] % distance.link_names[1]; + return desc.str(); + } }; double distance{ 0.0 }; - if (cumulative) { - for (const auto& distance_of_pair : result.distances) { - assert(distance_of_pair.second.size() == 1); - distance += distance_of_pair.second[0].distance; + + if (interface == Interface::START || interface == Interface::END || + (interface == Interface::NONE && s.trajectory() == nullptr)) { + auto distance_data{ check_distance(state, state->scene()->getCurrentState()) }; + if (distance_data.distance < 0) { + comment = collision_comment(distance_data); + return std::numeric_limits::infinity(); } - } else { - distance = result.minimum_distance.distance; - } - - const auto& links = result.minimum_distance.link_names; - - if (result.minimum_distance.distance <= 0) { - boost::format desc("ClearCost: allegedly valid solution has an unwanted collide between '%1%' and '%2%'"); - desc % links[0] % links[1]; - comment = desc.str(); - return std::numeric_limits::infinity(); - } else { - if (cumulative) { - comment = "ClearCost: cumulative distance " + std::to_string(distance); - } else { - boost::format desc("ClearCost: distance %1% between '%2%' and '%3%'"); - desc % result.minimum_distance.distance % links[0] % links[1]; + distance = distance_data.distance; + if (!cumulative) { + boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" }; + desc % distance % distance_data.link_names[0] % distance_data.link_names[1]; comment = desc.str(); + } else { + comment = PREFIX + "cumulative distance " + std::to_string(distance); } - return 1.0 / (distance + 1e-5); + } else { // check trajectory + for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) { + auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i)); + if (distance_data.distance < 0) { + comment = collision_comment(distance_data); + return std::numeric_limits::infinity(); + } + distance += distance_data.distance; + } + distance /= s.trajectory()->getWayPointCount(); + + boost::format desc(PREFIX + "average%1% distance: %2%"); + desc % (cumulative ? " cumulative" : "") % distance; + comment = desc.str(); } + + return 1.0 / (distance + 1e-5); } } } From 1f8feaaf0a551b95aaef57b041e8d5d5d158e325 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 16 Jul 2020 16:14:51 +0200 Subject: [PATCH 20/67] refactor addCost->computeCost --- core/include/moveit/task_constructor/stage_p.h | 2 +- core/src/stage.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index ad0f1bfa..208639d0 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -158,7 +158,7 @@ public: * * @return true if solution remains feasible (is no failure) */ - bool addCost(SolutionBase& solution); + bool computeCost(SolutionBase& solution); protected: // associated/owning Stage instance diff --git a/core/src/stage.cpp b/core/src/stage.cpp index f5ced345..8c9265e6 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -156,7 +156,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, solution->setStartState(from); solution->setEndState(*to_it); - if (!solution->isFailure() && addCost(*solution)) { + if (!solution->isFailure() && computeCost(*solution)) { nextStarts()->add(*to_it); } @@ -174,7 +174,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, solution->setStartState(*from_it); solution->setEndState(to); - if (!solution->isFailure() && addCost(*solution)) + if (!solution->isFailure() && computeCost(*solution)) prevEnds()->add(*from_it); newSolution(solution); @@ -191,7 +191,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution solution->setStartState(*from); solution->setEndState(*to); - if (!solution->isFailure() && addCost(*solution)) { + if (!solution->isFailure() && computeCost(*solution)) { prevEnds()->add(*from); nextStarts()->add(*to); } @@ -207,7 +207,7 @@ void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, solution->setEndState(to); if (!solution->isFailure()) - addCost(*solution); + computeCost(*solution); newSolution(solution); } @@ -221,7 +221,7 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } -bool StagePrivate::addCost(SolutionBase& solution) { +bool StagePrivate::computeCost(SolutionBase& solution) { double cost{ 0.0 }; auto* trajectory{ dynamic_cast(&solution) }; From bbf7d415f6627e747b2a0ab7cba88e34284304bc Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 16 Jul 2020 17:18:14 +0200 Subject: [PATCH 21/67] make cost operators const --- core/include/moveit/task_constructor/cost_terms.h | 4 ++-- core/src/cost_terms.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 26140060..d0ad411e 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -66,7 +66,7 @@ struct LinkMotion { LinkMotion(std::string link_name) : link_name(link_name) {} - double operator()(const SubTrajectory&, std::string&); + double operator()(const SubTrajectory&, std::string&) const; std::string link_name; }; @@ -85,7 +85,7 @@ struct Clearance Interface::Direction interface = Interface::NONE) : with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {} - double operator()(const SubTrajectory& s, std::string& comment); + double operator()(const SubTrajectory& s, std::string& comment) const; bool with_world; bool cumulative; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 12cf4d55..69ce13f2 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -64,7 +64,7 @@ double TrajectoryDuration(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -87,7 +87,7 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { return distance; } -double Clearance::operator()(const SubTrajectory& s, std::string& comment) { +double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; collision_detection::DistanceRequest request; From 53c0964618c9920faea0f931fe771fa80bd02b8f Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 28 Jul 2020 18:27:02 +0200 Subject: [PATCH 22/67] cost computation: provide valid interface states for Solution The CostTerm's should get only a single solution that is well-setup with its InterfaceStates. That's impossible though because these states are stored in different places depending on the cost. To avoid this, set stub states for cost computation and change them to the real states later on. This is motivated by the Clearance cost which can act on an InterfaceState only. --- .../include/moveit/task_constructor/storage.h | 29 ++++++-- core/src/stage.cpp | 74 +++++++++++++++---- core/src/storage.cpp | 10 +++ 3 files changed, 91 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 581602ca..e706258a 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -197,6 +197,7 @@ class StagePrivate; /// abstract base class for solutions (primitive and sequences) class SolutionBase { + friend StagePrivate; // for set[Start|End]StateUnsafe public: virtual ~SolutionBase() = default; @@ -207,18 +208,22 @@ public: template inline const InterfaceState::Solutions& trajectories() const; + /** set the solution's start_state_ + * + * Must not be used with different states because it registers the solution with the state as well. + */ inline void setStartState(const InterfaceState& state) { - // only allow setting once (by Stage) assert(start_ == nullptr || start_ == &state); - start_ = &state; - const_cast(state).addOutgoing(this); + setStartStateUnsafe(state); } + /** set the solution's end_state_ + * + * Must not be used with different states because it registers the solution with the state as well. + */ inline void setEndState(const InterfaceState& state) { - // only allow setting once (by Stage) assert(end_ == nullptr || end_ == &state); - end_ = &state; - const_cast(state).addIncoming(this); + setEndStateUnsafe(state); } inline const Stage* creator() const { return creator_; } @@ -247,6 +252,18 @@ protected: SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "") : creator_(creator), cost_(cost), comment_(std::move(comment)) {} + /** unsafe setter for start_state_ + * + * must only be used if the previously set state removes its link to this solution + */ + void setStartStateUnsafe(const InterfaceState& state); + + /** unsafe setter for end_state_ + * + * must only be used if the previously set state removes its link to this solution + */ + void setEndStateUnsafe(const InterfaceState& state); + private: // back-pointer to creating stage, allows to access sub-solutions Stage* creator_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 8c9265e6..89e0241b 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -130,7 +130,6 @@ void StagePrivate::validateConnectivity() const { } bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { - solution->setCreator(me()); if (introspection_) introspection_->registerSolution(*solution); @@ -147,34 +146,59 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) { assert(nextStarts()); + + solution->setCreator(me()); + + if (!solution->isFailure()) { + // chicken-and-egg problem: we don't know whether/where we will store the solution, + // but need to store it properly to compute the cost with a clean interface: + // set state copies for solution before computing cost: + InterfaceState tmp_from{ from }, tmp_to{ to }; + solution->setStartState(tmp_from); + solution->setEndState(tmp_to); + + computeCost(*solution); + } + if (!storeSolution(solution)) return; // solution dropped me()->forwardProperties(from, to); auto to_it = states_.insert(states_.end(), std::move(to)); - solution->setStartState(from); - solution->setEndState(*to_it); + // register stored interfaces with solution + solution->setStartStateUnsafe(from); + solution->setEndStateUnsafe(*to_it); - if (!solution->isFailure() && computeCost(*solution)) { + if (!solution->isFailure()) nextStarts()->add(*to_it); - } newSolution(solution); } void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution) { assert(prevEnds()); + + solution->setCreator(me()); + + if (!solution->isFailure()) { + InterfaceState tmp_from{ from }, tmp_to{ to }; + solution->setStartState(tmp_from); + solution->setEndState(tmp_to); + + computeCost(*solution); + } + if (!storeSolution(solution)) return; // solution dropped me()->forwardProperties(to, from); auto from_it = states_.insert(states_.end(), std::move(from)); - solution->setStartState(*from_it); - solution->setEndState(to); + solution->setStartStateUnsafe(*from_it); + solution->setEndStateUnsafe(to); - if (!solution->isFailure() && computeCost(*solution)) + if (!solution->isFailure()) prevEnds()->add(*from_it); newSolution(solution); @@ -182,16 +206,27 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) { assert(prevEnds() && nextStarts()); + + solution->setCreator(me()); + + if (!solution->isFailure()) { + InterfaceState tmp_from{ state }, tmp_to{ state }; + solution->setStartState(tmp_from); + solution->setEndState(tmp_to); + + computeCost(*solution); + } + if (!storeSolution(solution)) return; // solution dropped auto from = states_.insert(states_.end(), InterfaceState(state)); // copy auto to = states_.insert(states_.end(), std::move(state)); - solution->setStartState(*from); - solution->setEndState(*to); + solution->setStartStateUnsafe(*from); + solution->setEndStateUnsafe(*to); - if (!solution->isFailure() && computeCost(*solution)) { + if (!solution->isFailure()) { prevEnds()->add(*from); nextStarts()->add(*to); } @@ -200,14 +235,21 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution } void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { + solution->setCreator(me()); + + if (!solution->isFailure()) { + InterfaceState tmp_from{ from }, tmp_to{ to }; + solution->setStartState(tmp_from); + solution->setEndState(tmp_to); + + computeCost(*solution); + } + if (!storeSolution(solution)) return; // solution dropped - solution->setStartState(from); - solution->setEndState(to); - - if (!solution->isFailure()) - computeCost(*solution); + solution->setStartStateUnsafe(from); + solution->setEndStateUnsafe(to); newSolution(solution); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ed53b4ec..330231eb 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -141,6 +141,16 @@ void SolutionBase::setCreator(Stage* creator) { creator_ = creator; } +void SolutionBase::setStartStateUnsafe(const InterfaceState& state) { + start_ = &state; + const_cast(state).addOutgoing(this); +} + +void SolutionBase::setEndStateUnsafe(const InterfaceState& state) { + end_ = &state; + const_cast(state).addIncoming(this); +} + void SolutionBase::setCost(double cost) { cost_ = cost; } From e53d943b758547714cae3ceb36ccb1d8df29a832 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 17:23:08 +0200 Subject: [PATCH 23/67] remove CostTransform If required, this can hopefully be implemented through Wrapper stages soon. --- core/include/moveit/task_constructor/stage.h | 11 ----------- core/include/moveit/task_constructor/stage_p.h | 1 - core/src/stage.cpp | 8 +------- 3 files changed, 1 insertion(+), 19 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 857f2012..9c0d7461 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -215,9 +215,6 @@ public: using CostTerm = std::function; using CostTermShort = std::function; - - using CostTransform = std::function; - /* \brief set method to determine costs for solutions of this stage * * This interface supports only primitive solutions, no containers. @@ -231,14 +228,6 @@ public: void setCostTerm(const CostTerm& term); void setCostTerm(const CostTermShort& term); - /* \brief set CostTransform to be applied to costs of this stage's solution costs - * - * This interface can be used to modify the cost associated with each solution of this stage/container. - * The most common case would be linear weighting `[](double cost){ return constant*cost; }` - * but any non-linear transform is obviously possible as well. - */ - void setCostTransform(const CostTransform& transform); - const ordered& solutions() const; const std::list& failures() const; size_t numFailures() const; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 208639d0..221034e9 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -173,7 +173,6 @@ protected: // user-configurable cost estimator Stage::CostTerm cost_term_; - Stage::CostTransform cost_transform_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 89e0241b..4b5247bc 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -99,7 +99,6 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) { StagePrivate::StagePrivate(Stage* me, const std::string& name) : me_(me) , name_(name) - , cost_transform_([](auto x) { return x; }) , total_compute_time_{} , parent_(nullptr) , introspection_(nullptr) {} @@ -282,8 +281,7 @@ bool StagePrivate::computeCost(SolutionBase& solution) { cost = solution.cost(); } - // all costs are transformed - solution.setCost(cost_transform_(cost)); + solution.setCost(cost); return !solution.isFailure(); } @@ -392,10 +390,6 @@ void Stage::setCostTerm(const CostTermShort& term) { setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); } -void Stage::setCostTransform(const CostTransform& transform) { - pimpl()->cost_transform_ = transform; -} - const ordered& Stage::solutions() const { return pimpl()->solutions_; } From 039b0a2c27ec24931bee73d5b92435d79a9522f8 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 17:25:10 +0200 Subject: [PATCH 24/67] move more overhead to computeCost --- .../include/moveit/task_constructor/stage_p.h | 7 +-- core/src/stage.cpp | 61 ++++++------------- 2 files changed, 21 insertions(+), 47 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 221034e9..5ada0542 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -154,11 +154,8 @@ public: total_compute_time_ += compute_stop_time - compute_start_time; } - /** compute cost for solution through configured CostTerm - * - * @return true if solution remains feasible (is no failure) - */ - bool computeCost(SolutionBase& solution); + /** compute cost for solution through configured CostTerm */ + void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); protected: // associated/owning Stage instance diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 4b5247bc..767c4518 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -148,19 +148,11 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, solution->setCreator(me()); - if (!solution->isFailure()) { - // chicken-and-egg problem: we don't know whether/where we will store the solution, - // but need to store it properly to compute the cost with a clean interface: - // set state copies for solution before computing cost: - InterfaceState tmp_from{ from }, tmp_to{ to }; - solution->setStartState(tmp_from); - solution->setEndState(tmp_to); - - computeCost(*solution); - } + computeCost(from, to, *solution); if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(from, to); auto to_it = states_.insert(states_.end(), std::move(to)); @@ -180,16 +172,11 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, solution->setCreator(me()); - if (!solution->isFailure()) { - InterfaceState tmp_from{ from }, tmp_to{ to }; - solution->setStartState(tmp_from); - solution->setEndState(tmp_to); - - computeCost(*solution); - } + computeCost(from, to, *solution); if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(to, from); auto from_it = states_.insert(states_.end(), std::move(from)); @@ -208,13 +195,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution solution->setCreator(me()); - if (!solution->isFailure()) { - InterfaceState tmp_from{ state }, tmp_to{ state }; - solution->setStartState(tmp_from); - solution->setEndState(tmp_to); - - computeCost(*solution); - } + computeCost(state, state, *solution); if (!storeSolution(solution)) return; // solution dropped @@ -236,13 +217,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { solution->setCreator(me()); - if (!solution->isFailure()) { - InterfaceState tmp_from{ from }, tmp_to{ to }; - solution->setStartState(tmp_from); - solution->setEndState(tmp_to); - - computeCost(*solution); - } + computeCost(from, to, *solution); if (!storeSolution(solution)) return; // solution dropped @@ -262,28 +237,30 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } -bool StagePrivate::computeCost(SolutionBase& solution) { - double cost{ 0.0 }; +void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { + // no reason to compute costs for a failed solution + if (solution.isFailure() || !cost_term_) + return; + + // chicken-and-egg problem: we don't know whether/where we will store the solution yet, + // but need to store it properly to compute the cost with a clean interface: + // set state copies for solution before computing cost: + InterfaceState tmp_from{ from }, tmp_to{ to }; + solution.setStartState(tmp_from); + solution.setEndState(tmp_to); auto* trajectory{ dynamic_cast(&solution) }; - if (trajectory && cost_term_) { + if (trajectory) { // compute specific cost std::string comment; - cost = cost_term_(*trajectory, comment); + solution.setCost(cost_term_(*trajectory, comment)); // If a comment was specified, add it to the solution if (!comment.empty() && !solution.comment().empty()) { solution.setComment(solution.comment() + " (" + comment + ")"); } else if (!comment.empty()) { solution.setComment(comment); } - } else { - // otherwise forward current cost - cost = solution.cost(); } - - solution.setCost(cost); - - return !solution.isFailure(); } Stage::Stage(StagePrivate* impl) : pimpl_(impl) { From d68ab383b63817fcb8240ae880b7e203a798e5ae Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 17:38:03 +0200 Subject: [PATCH 25/67] allow setCostTerm for containers implement visitor pattern for cost computation on solutions - compute cost terms for solution subtrees instead of only for SubTrajectory - allows users to set cost terms for containers --- .../moveit/task_constructor/container.h | 6 ----- core/include/moveit/task_constructor/stage.h | 9 +------ .../include/moveit/task_constructor/stage_p.h | 2 +- .../include/moveit/task_constructor/storage.h | 17 +++++++++++++ core/src/stage.cpp | 24 ++++++------------ core/src/storage.cpp | 25 +++++++++++++++++++ 6 files changed, 52 insertions(+), 31 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index f1767f98..27b2fde0 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -80,9 +80,6 @@ public: protected: ContainerBase(ContainerBasePrivate* impl); - - /// most containers should only use setCostTransform and leave the costs to their children - using Stage::setCostTerm; }; std::ostream& operator<<(std::ostream& os, const ContainerBase& stage); @@ -227,9 +224,6 @@ public: bool canCompute() const override; void compute() override; - // Wrappers sometimes do the real work (e.g., IK), so they can specify costs - using Stage::setCostTerm; - protected: WrapperBase(WrapperBasePrivate* impl, Stage::pointer&& child = Stage::pointer()); }; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 9c0d7461..9a40cf69 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -213,17 +213,10 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTerm = std::function; using CostTermShort = std::function; /* \brief set method to determine costs for solutions of this stage * - * This interface supports only primitive solutions, no containers. - * - * In order to support containers, the associated CostTerms would need to be able - * to decide costs for arbitrary partial solutions of the container. - * These need to be determined for scheduling, but would make the terms too complex. - * - * See setCostTransform + * For composite stages, costs are computed for each SubTrajectory and aggregated */ void setCostTerm(const CostTerm& term); void setCostTerm(const CostTermShort& term); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 5ada0542..0d094108 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -169,7 +169,7 @@ protected: InterfacePtr ends_; // user-configurable cost estimator - Stage::CostTerm cost_term_; + CostTerm cost_term_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index e706258a..f8e5e669 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -193,6 +193,13 @@ private: using base_type::remove_if; }; +class SubTrajectory; +/** Interface for cost terms to be configured via Stage::setCostTerm() + * + * Look at moveit/task_constructor/cost_terms.h for various implementations + */ +using CostTerm = std::function; + class StagePrivate; /// abstract base class for solutions (primitive and sequences) class SolutionBase @@ -245,6 +252,9 @@ public: Introspection* introspection = nullptr) const = 0; void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; + /// compute cost of this solution according to the specified CostTerm + virtual double computeCost(const CostTerm& cost, std::string& comment) const = 0; + /// order solutions by their cost bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; } @@ -294,6 +304,8 @@ public: void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override; + double computeCost(const CostTerm& cost, std::string& comment) const override; + private: // actual trajectory, might be empty robot_trajectory::RobotTrajectoryConstPtr trajectory_; @@ -319,6 +331,9 @@ public: /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + /// aggregate costs along the sequence + double computeCost(const CostTerm& cost, std::string& comment) const override; + const container_type& solutions() const { return subsolutions_; } inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); } @@ -348,6 +363,8 @@ public: : WrappedSolution(creator, wrapped, wrapped->cost()) {} void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const override; + /// compute cost of wrapped child + double computeCost(const CostTerm& cost, std::string& comment) const override; const SolutionBase* wrapped() const { return wrapped_; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 767c4518..b4e6e051 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -97,11 +97,7 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) { } StagePrivate::StagePrivate(Stage* me, const std::string& name) - : me_(me) - , name_(name) - , total_compute_time_{} - , parent_(nullptr) - , introspection_(nullptr) {} + : me_(me), name_(name), total_compute_time_{}, parent_(nullptr), introspection_(nullptr) {} InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; @@ -249,17 +245,13 @@ void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& solution.setStartState(tmp_from); solution.setEndState(tmp_to); - auto* trajectory{ dynamic_cast(&solution) }; - if (trajectory) { - // compute specific cost - std::string comment; - solution.setCost(cost_term_(*trajectory, comment)); - // If a comment was specified, add it to the solution - if (!comment.empty() && !solution.comment().empty()) { - solution.setComment(solution.comment() + " (" + comment + ")"); - } else if (!comment.empty()) { - solution.setComment(comment); - } + std::string comment; + solution.setCost(solution.computeCost(cost_term_, comment)); + // If a comment was specified, add it to the solution + if (!comment.empty() && !solution.comment().empty()) { + solution.setComment(solution.comment() + " (" + comment + ")"); + } else if (!comment.empty()) { + solution.setComment(comment); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 330231eb..ff096a12 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -178,6 +178,10 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Int this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff); } +double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const { + return f(*this, comment); +} + void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } @@ -217,6 +221,22 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, } } +double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) const { + double cost{ 0.0 }; + std::string subcomment; + for (auto& solution : subsolutions_) { + cost += solution->computeCost(f, subcomment); + if (!subcomment.empty()) { + if (!comment.empty()) + comment.append(", "); + comment.append(subcomment); + subcomment.clear(); + } + } + + return cost; +} + void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection) const { wrapped_->fillMessage(solution, introspection); @@ -227,5 +247,10 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0); solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg)); } + +double WrappedSolution::computeCost(const CostTerm& f, std::string& comment) const { + return wrapped_->computeCost(f, comment); +} + } // namespace task_constructor } // namespace moveit From a0975e3afc4c7c805cb390bc2adfb4d16137dd3e Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 21:13:51 +0200 Subject: [PATCH 26/67] avoid ambiguity of setCostTerm(nullptr) --- core/include/moveit/task_constructor/stage.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 9a40cf69..510d6070 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -220,6 +220,8 @@ public: */ void setCostTerm(const CostTerm& term); void setCostTerm(const CostTermShort& term); + // avoid overloading ambiguity for resetting the cost term + void setCostTerm(const std::nullptr_t) { setCostTerm(static_cast(nullptr)); } const ordered& solutions() const; const std::list& failures() const; From 939f0b7256e9d62779dc223629c0a5f6824580ea Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 21:14:29 +0200 Subject: [PATCH 27/67] setCostTermShort(nullptr) must not set a valid function --- core/src/stage.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b4e6e051..a0c75b69 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -356,7 +356,10 @@ void Stage::setCostTerm(const CostTerm& term) { } void Stage::setCostTerm(const CostTermShort& term) { - setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); + if (term) + setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); + else + pimpl()->cost_term_ = nullptr; } const ordered& Stage::solutions() const { From 0e38730fd49fc993171876f9f5de943da49a135f Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 19 Aug 2020 21:15:15 +0200 Subject: [PATCH 28/67] compute cost in liftSolution as well --- core/include/moveit/task_constructor/storage.h | 3 +++ core/src/container.cpp | 8 ++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index f8e5e669..58dcc0cb 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -201,10 +201,13 @@ class SubTrajectory; using CostTerm = std::function; class StagePrivate; +class ContainerBasePrivate; /// abstract base class for solutions (primitive and sequences) class SolutionBase { friend StagePrivate; // for set[Start|End]StateUnsafe + friend ContainerBasePrivate; + public: virtual ~SolutionBase() = default; diff --git a/core/src/container.cpp b/core/src/container.cpp index 2a089456..18892591 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -120,6 +120,10 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to) { + solution->setCreator(me()); + + computeCost(*internal_from, *internal_to, *solution); + if (!storeSolution(solution)) return; @@ -139,8 +143,8 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I InterfaceState* external_to = find_or_create_external(internal_to, created_to); // connect solution to start/end state - solution->setStartState(*external_from); - solution->setEndState(*external_to); + solution->setStartStateUnsafe(*external_from); + solution->setEndStateUnsafe(*external_to); // spawn created states in external interfaces if (created_from) From 1689f6cd95888823e397c4c28d5c8893a0e36580 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 19 Aug 2020 22:41:09 +0200 Subject: [PATCH 29/67] introduce new trivial stage Forward This can be used together with a custom CostTerm to modify costs of a solution. --- .../moveit/task_constructor/stages/forward.h | 61 +++++++++++++++++ core/src/stages/CMakeLists.txt | 2 + core/src/stages/forward.cpp | 65 +++++++++++++++++++ 3 files changed, 128 insertions(+) create mode 100644 core/include/moveit/task_constructor/stages/forward.h create mode 100644 core/src/stages/forward.cpp diff --git a/core/include/moveit/task_constructor/stages/forward.h b/core/include/moveit/task_constructor/stages/forward.h new file mode 100644 index 00000000..6f54c35b --- /dev/null +++ b/core/include/moveit/task_constructor/stages/forward.h @@ -0,0 +1,61 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holders nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* Authors: Michael 'v4hn' Goerner */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +/** Generic Wrapper that passes on a solution + * + * Useful to set a custom CostTransform via Stage::setCostTerm + * to change a solution's cost without loosing the original value + */ +class Forward : public WrapperBase +{ +public: + Forward(const std::string& name = "Forward", Stage::pointer&& child = Stage::pointer()); + + void onNewSolution(const SolutionBase& s) override; +}; +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index c254762f..8becf93e 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/fixed_state.h ${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h + ${PROJECT_INCLUDE}/stages/forward.h ${PROJECT_INCLUDE}/stages/generate_pose.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/generate_place_pose.h @@ -24,6 +25,7 @@ add_library(${PROJECT_NAME}_stages current_state.cpp fixed_state.cpp fixed_cartesian_poses.cpp + forward.cpp generate_pose.cpp generate_grasp_pose.cpp generate_place_pose.cpp diff --git a/core/src/stages/forward.cpp b/core/src/stages/forward.cpp new file mode 100644 index 00000000..2a650c71 --- /dev/null +++ b/core/src/stages/forward.cpp @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holders nor the names of their + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* Author: Michael 'v4hn' Goerner */ + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +Forward::Forward(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} + +void Forward::onNewSolution(const SolutionBase& s) { + this->liftSolution(s); +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit From dcee7c408f33add8bb5fb9883ac0e410d4c739f8 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 19 Aug 2020 22:45:56 +0200 Subject: [PATCH 30/67] add basic tests for using cost terms --- core/test/CMakeLists.txt | 2 + core/test/test_cost_terms.cpp | 253 ++++++++++++++++++++++++++++++++++ 2 files changed, 255 insertions(+) create mode 100644 core/test/test_cost_terms.cpp diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index f36fde93..24ec06a4 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -24,6 +24,8 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp) target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main) + catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp) + target_link_libraries(${PROJECT_NAME}-test-cost_terms ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) # building these integration tests works without moveit config packages add_executable(pick_ur5 pick_ur5.cpp) diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp new file mode 100644 index 00000000..0598ca31 --- /dev/null +++ b/core/test/test_cost_terms.cpp @@ -0,0 +1,253 @@ +#include "models.h" + +#include +#include +#include +#include + +#include + +#include + +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; + +const double STAGE_COST{ 7.0 }; + +class GeneratorMockup : public Generator +{ + PlanningScenePtr ps; + InterfacePtr prev; + InterfacePtr next; + +public: + GeneratorMockup() : Generator("generator") { + prev.reset(new Interface); + next.reset(new Interface); + pimpl()->setPrevEnds(prev); + pimpl()->setNextStarts(next); + } + + void init(const moveit::core::RobotModelConstPtr& robot_model) override { ps.reset(new PlanningScene(robot_model)); } + + bool canCompute() const override { return true; } + + void compute() override { + InterfaceState state(ps); + spawn(std::move(state), STAGE_COST); + } +}; +MOVEIT_CLASS_FORWARD(GeneratorMockup); + +class ConnectMockup : public Connecting +{ + using Connecting::Connecting; + + void compute(const InterfaceState& from, const InterfaceState& to) override { + auto solution{ std::make_shared() }; + solution->setCost(STAGE_COST); + connect(from, to, solution); + } +}; +MOVEIT_CLASS_FORWARD(GeneratorMockup); + +class ForwardMockup : public PropagatingForward +{ + using PropagatingForward::PropagatingForward; + + void computeForward(const InterfaceState& from) override { + SubTrajectory solution; + solution.setCost(STAGE_COST); + InterfaceState to(from); + + sendForward(from, std::move(to), std::move(solution)); + }; +}; +MOVEIT_CLASS_FORWARD(GeneratorMockup); + +class BackwardMockup : public PropagatingBackward +{ + using PropagatingBackward::PropagatingBackward; + + void computeBackward(const InterfaceState& to) override { + SubTrajectory solution; + solution.setCost(STAGE_COST); + InterfaceState from(to); + + sendBackward(std::move(from), to, std::move(solution)); + }; +}; +MOVEIT_CLASS_FORWARD(GeneratorMockup); + +class SerialContainerStandalone : public SerialContainer +{ + moveit::core::RobotModelConstPtr robot; + InterfacePtr dummy; + planning_scene::PlanningSceneConstPtr ps; + InterfaceStatePtr state_start, state_end; + +public: + SerialContainerStandalone(const moveit::core::RobotModelConstPtr& robot) + : SerialContainer() + , robot(robot) + , dummy(std::make_shared()) + , ps(new planning_scene::PlanningScene(robot)) {} + + // reset and prepare for a compute step + void prepare() { + reset(); + + state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); + state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); + + // infer and setup interface from children + Stage* s{ this }; + ContainerBase* b; + + while ((b = dynamic_cast(s))) { + s = &(*b->pimpl()->children().front()); + } + InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK }; + + s = this; + while ((b = dynamic_cast(s))) { + s = &(*b->pimpl()->children().back()); + } + InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK }; + + InterfaceFlags flags{ start_flags | end_flags }; + + pimpl()->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); + pimpl()->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); + SerialContainer::init(robot); + pimpl()->resolveInterface(flags); + + // feed interfaces as required for one computation + if (flags & READS_START) { + pimpl()->starts()->add(*state_start); + } + if (flags & READS_END) { + pimpl()->ends()->add(*state_end); + } + } + + void runCompute() { pimpl()->runCompute(); } + + void computeWithStage(StageUniquePtr&& stage) { + clear(); + add(std::move(stage)); + + prepare(); + runCompute(); + } + + void computeWithStageCost(StageUniquePtr&& stage, const CostTerm& cost_term) { + stage->setCostTerm(cost_term); + computeWithStage(std::move(stage)); + } +}; + +TEST(CostTerm, CostOverwrite) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + const moveit::core::RobotModelConstPtr robot{ getModel() }; + + SerialContainerStandalone container(robot); + + // by default return the cost the stage declared + container.computeWithStage(std::make_unique()); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + + // nullptr is the explicit default + container.computeWithStageCost(std::make_unique(), nullptr); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + + // custom cost overwrites the stage cost + cost::Constant constant_cost{ 1.0 }; + container.computeWithStageCost(std::make_unique(), constant_cost); + EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost); +} + +// test all primitive stage types +TEST(CostTerm, StageTypes) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + + SerialContainerStandalone container(robot); + + const cost::Constant constant{ 1.0 }; + + // already tested above + // cont.computeWithStageCost(std::make_unique(), constant); + // EXPECT_EQ(cont.solutions().front()->cost(), constant.cost); + + container.computeWithStageCost(std::make_unique(), constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + + container.computeWithStageCost(std::make_unique(), constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + + container.computeWithStageCost(std::make_unique(), constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost); +} + +// Forward uses cost from solution +TEST(CostTerm, ForwardUsesCost) { + moveit::core::RobotModelPtr robot{ getModel() }; + SerialContainerStandalone container(robot); + + auto stage{ std::make_unique() }; + cost::Constant constant_stage{ 84.0 }; + stage->setCostTerm(constant_stage); + auto forward{ std::make_unique("forward", std::move(stage)) }; + auto* forward_ptr{ forward.get() }; + + container.computeWithStage(std::move(forward)); + + ASSERT_NE(nullptr, dynamic_cast(&(*forward_ptr->solutions().front()))); + + auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_stage.cost); + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); +} + +// Forward can overwrite cost from solution +TEST(CostTerm, ForwardOverwritesCost) { + moveit::core::RobotModelPtr robot{ getModel() }; + SerialContainerStandalone container(robot); + + auto stage{ std::make_unique() }; + cost::Constant constant_stage{ 84.0 }; + stage->setCostTerm(constant_stage); + auto forward{ std::make_unique("forward", std::move(stage)) }; + auto* forward_ptr{ forward.get() }; + + cost::Constant constant_forward{ 72.0 }; + forward->setCostTerm(constant_forward); + + container.computeWithStage(std::move(forward)); + auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_forward.cost); + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); + + // Forward can modify cost from solution +} + +// Forward can modify cost from solution +TEST(CostTerm, ForwardCanModifyCost) { + moveit::core::RobotModelPtr robot{ getModel() }; + SerialContainerStandalone container(robot); + + auto stage{ std::make_unique() }; + cost::Constant constant{ 8.0 }; + stage->setCostTerm(constant); + auto forward{ std::make_unique("forward", std::move(stage)) }; + auto* forward_ptr{ forward.get() }; + forward->setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); + + container.computeWithStage(std::move(forward)); + auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost); + EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost); +} From 28b98e989c0e52c88a093fdb959f92efc88725cd Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 21 Aug 2020 20:36:21 +0200 Subject: [PATCH 31/67] improve cost term tests - add tests for nested containers - add descriptions to checks - refactor helper to support different containers --- core/test/test_cost_terms.cpp | 227 ++++++++++++++++++++++------------ 1 file changed, 150 insertions(+), 77 deletions(-) diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 0598ca31..0a3d24af 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -16,6 +16,8 @@ using namespace planning_scene; const double STAGE_COST{ 7.0 }; +const double TRAJECTORY_DURATION{ 9.0 }; + class GeneratorMockup : public Generator { PlanningScenePtr ps; @@ -30,7 +32,10 @@ public: pimpl()->setNextStarts(next); } - void init(const moveit::core::RobotModelConstPtr& robot_model) override { ps.reset(new PlanningScene(robot_model)); } + void init(const moveit::core::RobotModelConstPtr& robot_model) override { + ps.reset(new PlanningScene(robot_model)); + Generator::init(robot_model); + } bool canCompute() const override { return true; } @@ -39,7 +44,6 @@ public: spawn(std::move(state), STAGE_COST); } }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); class ConnectMockup : public Connecting { @@ -51,21 +55,32 @@ class ConnectMockup : public Connecting connect(from, to, solution); } }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); class ForwardMockup : public PropagatingForward { + PlanningScenePtr ps; + +public: using PropagatingForward::PropagatingForward; + void init(const moveit::core::RobotModelConstPtr& robot_model) override { + ps.reset(new PlanningScene(robot_model)); + PropagatingForward::init(robot_model); + } + void computeForward(const InterfaceState& from) override { SubTrajectory solution; + auto traj{ std::make_shared(ps->getRobotModel(), + ps->getRobotModel()->getJointModelGroups()[0]) }; + traj->addSuffixWayPoint(ps->getCurrentState(), 0.0); + traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION); + solution.setTrajectory(traj); solution.setCost(STAGE_COST); InterfaceState to(from); sendForward(from, std::move(to), std::move(solution)); }; }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); class BackwardMockup : public PropagatingBackward { @@ -79,9 +94,9 @@ class BackwardMockup : public PropagatingBackward sendBackward(std::move(from), to, std::move(solution)); }; }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); -class SerialContainerStandalone : public SerialContainer +template +class Standalone : public T { moveit::core::RobotModelConstPtr robot; InterfacePtr dummy; @@ -89,15 +104,13 @@ class SerialContainerStandalone : public SerialContainer InterfaceStatePtr state_start, state_end; public: - SerialContainerStandalone(const moveit::core::RobotModelConstPtr& robot) - : SerialContainer() - , robot(robot) - , dummy(std::make_shared()) - , ps(new planning_scene::PlanningScene(robot)) {} + Standalone(const moveit::core::RobotModelConstPtr& robot) + : T(), robot(robot), dummy(std::make_shared()), ps(new planning_scene::PlanningScene(robot)) {} // reset and prepare for a compute step void prepare() { - reset(); + auto impl{ this->pimpl() }; + this->reset(); state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); @@ -106,46 +119,55 @@ public: Stage* s{ this }; ContainerBase* b; - while ((b = dynamic_cast(s))) { + while ((b = dynamic_cast(s))) s = &(*b->pimpl()->children().front()); - } InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK }; s = this; - while ((b = dynamic_cast(s))) { + while ((b = dynamic_cast(s))) s = &(*b->pimpl()->children().back()); - } InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK }; InterfaceFlags flags{ start_flags | end_flags }; - pimpl()->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); - pimpl()->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); - SerialContainer::init(robot); - pimpl()->resolveInterface(flags); + impl->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); + impl->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); + T::init(robot); + impl->resolveInterface(flags); // feed interfaces as required for one computation if (flags & READS_START) { - pimpl()->starts()->add(*state_start); + impl->starts()->add(*state_start); } if (flags & READS_END) { - pimpl()->ends()->add(*state_end); + impl->ends()->add(*state_end); } } - void runCompute() { pimpl()->runCompute(); } + void runCompute() { this->pimpl()->runCompute(); } - void computeWithStage(StageUniquePtr&& stage) { - clear(); - add(std::move(stage)); + void computeWithStages(std::initializer_list stages) { + this->clear(); + + // initializer_list offers only const access (a known shortcoming in C++) + for (const auto& stage : stages) { + this->add(const_cast(stage)); + } prepare(); runCompute(); } - void computeWithStageCost(StageUniquePtr&& stage, const CostTerm& cost_term) { - stage->setCostTerm(cost_term); - computeWithStage(std::move(stage)); + void computeWithStageCost(std::initializer_list stages, const CostTerm& cost_term) { + this->setCostTerm(nullptr); + for (const auto& stage : stages) + const_cast(stage)->setCostTerm(cost_term); + computeWithStages(stages); + } + + void computeWithContainerCost(std::initializer_list stages, const CostTerm& cost_term) { + this->setCostTerm(cost_term); + computeWithStages(stages); } }; @@ -153,28 +175,24 @@ TEST(CostTerm, CostOverwrite) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); const moveit::core::RobotModelConstPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); - // by default return the cost the stage declared - container.computeWithStage(std::make_unique()); - EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + container.computeWithStages({ std::make_unique() }); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default"; - // nullptr is the explicit default - container.computeWithStageCost(std::make_unique(), nullptr); - EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + container.computeWithStageCost({ std::make_unique() }, nullptr); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; - // custom cost overwrites the stage cost cost::Constant constant_cost{ 1.0 }; - container.computeWithStageCost(std::make_unique(), constant_cost); - EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost); + container.computeWithStageCost({ std::make_unique() }, constant_cost); + EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost) << "custom cost overwrites stage cost"; } -// test all primitive stage types TEST(CostTerm, StageTypes) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); const cost::Constant constant{ 1.0 }; @@ -182,72 +200,127 @@ TEST(CostTerm, StageTypes) { // cont.computeWithStageCost(std::make_unique(), constant); // EXPECT_EQ(cont.solutions().front()->cost(), constant.cost); - container.computeWithStageCost(std::make_unique(), constant); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + container.computeWithStageCost({ std::make_unique() }, constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for connect"; - container.computeWithStageCost(std::make_unique(), constant); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + container.computeWithStageCost({ std::make_unique() }, constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for forward propagator"; - container.computeWithStageCost(std::make_unique(), constant); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + container.computeWithStageCost({ std::make_unique() }, constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator"; } -// Forward uses cost from solution TEST(CostTerm, ForwardUsesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; stage->setCostTerm(constant_stage); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; - container.computeWithStage(std::move(forward)); + container.computeWithStages({ std::move(stage) }); - ASSERT_NE(nullptr, dynamic_cast(&(*forward_ptr->solutions().front()))); - - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_stage.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward cost"; } -// Forward can overwrite cost from solution TEST(CostTerm, ForwardOverwritesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; stage->setCostTerm(constant_stage); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; cost::Constant constant_forward{ 72.0 }; - forward->setCostTerm(constant_forward); + container.setCostTerm(constant_forward); - container.computeWithStage(std::move(forward)); - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_forward.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); - - // Forward can modify cost from solution + container.computeWithStages({ std::move(stage) }); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_forward.cost) << "Forward can apply custom cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected"; } -// Forward can modify cost from solution TEST(CostTerm, ForwardCanModifyCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; stage->setCostTerm(constant); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; - forward->setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); + container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); - container.computeWithStage(std::move(forward)); - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost); + container.computeWithStages({ std::move(stage) }); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "Forward can compute cost based on child"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost) << "child's cost is not affected"; +} + +TEST(CostTerm, CompositeSolutions) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + + container.computeWithStages({ std::move(s1), std::move(s2) }); + EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added"; + } + + { + auto s1{ std::make_unique() }; + cost::Constant constant{ 1.0 }; + s1->setCostTerm(constant); + auto s2{ std::make_unique() }; + + container.computeWithStages({ std::move(s1), std::move(s2) }); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant.cost) + << "mix of explicit and implicit cost terms works"; + } + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + auto c1{ std::make_unique() }; + cost::Constant constant1{ 1.0 }; + s1->setCostTerm(constant1); + s2->setCostTerm(constant1); + c1->add(std::move(s1)); + c1->add(std::move(s2)); + + auto s3{ std::make_unique() }; + cost::Constant constant2{ 9.0 }; + s3->setCostTerm(constant2); + + container.computeWithStages({ std::move(c1), std::move(s3) }); + EXPECT_EQ(container.solutions().front()->cost(), 2 * constant1.cost + constant2.cost) + << "cost aggregation works across multiple levels"; + } +} + +TEST(CostTerm, CompositeSolutionsContainerCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + auto s1{ std::make_unique() }; + auto s1_ptr{ s1.get() }; + auto s2{ std::make_unique() }; + + auto c1{ std::make_unique() }; + c1->add(std::move(s1)); + c1->add(std::move(s2)); + + auto s3{ std::make_unique() }; + + container.setCostTerm(cost::TrajectoryDuration); + container.computeWithStages({ std::move(c1), std::move(s3) }); + EXPECT_EQ(container.solutions().front()->cost(), 3 * TRAJECTORY_DURATION) + << "container cost term overwrites stage costs"; + EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; +} } From 8ae1060b4df13ec4958daa4587669788219eb93b Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 21 Aug 2020 20:39:48 +0200 Subject: [PATCH 32/67] add CostAggregator for SerialContainer It can be useful to change the default addition to other operators. The simplest example is applying a Constant cost term to a container. As the tests show, the visitor-based cost computation ends up adding cost::Constant *for each subtrajectory*. --- .../moveit/task_constructor/container.h | 6 +++ .../moveit/task_constructor/container_p.h | 2 + .../include/moveit/task_constructor/storage.h | 10 +++-- core/src/container.cpp | 22 ++++++--- core/src/storage.cpp | 12 ++++- core/test/test_cost_terms.cpp | 45 +++++++++++++++++++ 6 files changed, 87 insertions(+), 10 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 27b2fde0..8ca032e7 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -94,6 +94,12 @@ public: bool canCompute() const override; void compute() override; + /** Specify how to aggregate costs of childrens' solutions + * + * By default costs are added up + */ + void setCostAggregator(const SolutionSequence::CostAggregator& agg); + protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 35a903ad..cad54fcd 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -184,6 +184,8 @@ protected: // validate that child's interface matches mine (considering start or end only as determined by mask) template void validateInterface(const StagePrivate& child, InterfaceFlags required) const; + + SolutionSequence::CostAggregator aggregator_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 58dcc0cb..20d646ec 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -325,15 +325,18 @@ class SolutionSequence : public SolutionBase public: using container_type = std::vector; - explicit SolutionSequence() : SolutionBase() {} - SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr) - : SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {} + explicit SolutionSequence(); + + SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr); void push_back(const SolutionBase& solution); /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + using CostAggregator = std::function; + void setCostAggregator(const CostAggregator&); + /// aggregate costs along the sequence double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -343,6 +346,7 @@ public: inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } private: + CostAggregator aggregator_; /// series of sub solutions container_type subsolutions_; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 18892591..1c1b0993 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -360,8 +360,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { - InterfaceState::Priority prio(static_cast(in.first.size() + 1 + out.first.size()), - in.second + current.cost() + out.second); + InterfaceState::Priority prio( + static_cast(in.first.size() + 1 + out.first.size()), + pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second)); // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { if (std::isinf(prio.cost())) @@ -374,7 +375,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - sorted.insert(std::make_shared(std::move(solution), prio.cost(), this)); + auto sequence{ std::make_shared(std::move(solution), prio.cost(), this) }; + sequence->setCostAggregator(pimpl()->aggregator_); + sorted.insert(std::move(sequence)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -393,7 +396,7 @@ SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(i SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name) - : ContainerBasePrivate(me, name) {} + : ContainerBasePrivate(me, name), aggregator_(std::plus{}) {} void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) { InterfaceFlags flags1 = stage1.requiredInterface(); @@ -518,6 +521,10 @@ void SerialContainerPrivate::validateConnectivity() const { } } +void SerialContainer::setCostAggregator(const SolutionSequence::CostAggregator& agg) { + pimpl()->aggregator_ = agg; +} + bool SerialContainer::canCompute() const { for (const auto& stage : pimpl()->children()) { if (stage->pimpl()->canCompute()) @@ -549,11 +556,14 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso else for (SolutionBase* successor : solutions) { trace.push_back(successor); - trace_cost += successor->cost(); + + double previous_trace_cost { trace_cost }; + + trace_cost = pimpl()->aggregator_(trace_cost, successor->cost()); traverse(*successor, cb, trace, trace_cost); - trace_cost -= successor->cost(); + trace_cost = previous_trace_cost; trace.pop_back(); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ff096a12..1133257e 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -182,10 +182,19 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const return f(*this, comment); } +SolutionSequence::SolutionSequence() : SolutionBase(), aggregator_(std::plus{}) {} + +SolutionSequence::SolutionSequence(container_type&& subsolutions, double cost, Stage* creator) + : SolutionBase(creator, cost), aggregator_(std::plus{}), subsolutions_(std::move(subsolutions)) {} + void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } +void SolutionSequence::setCostAggregator(const CostAggregator& agg) { + aggregator_ = agg; +} + void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -225,7 +234,8 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co double cost{ 0.0 }; std::string subcomment; for (auto& solution : subsolutions_) { - cost += solution->computeCost(f, subcomment); + auto subcost{ solution->computeCost(f, subcomment) }; + cost = aggregator_(cost, subcost); if (!subcomment.empty()) { if (!comment.empty()) comment.append(", "); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 0a3d24af..ca6f382f 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -11,6 +11,8 @@ #include +#include + using namespace moveit::task_constructor; using namespace planning_scene; @@ -323,4 +325,47 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { << "container cost term overwrites stage costs"; EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; } + +TEST(CostTerm, CompositeSolutionsContainerAggregator) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + cost::Constant constant{ 17.0 }; + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + auto s3{ std::make_unique() }; + + container.computeWithStageCost({ std::move(s1), std::move(s2), std::move(s3) }, constant); + EXPECT_EQ(container.solutions().front()->cost(), 3 * constant.cost) << "default cost aggregates through stages"; + } + + { + container.setCostAggregator([](double a, double b) { return std::max(a, b); }); + container.computeWithContainerCost( + { + std::make_unique(), + std::make_unique(), + std::make_unique(), + }, + constant); + + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) + << "custom aggregator works with explicit cost term in container"; + } + + { + container.setCostAggregator([](auto a, auto b) { return std::max(a, b); }); + container.computeWithStageCost( + { + std::make_unique(), + std::make_unique(), + std::make_unique(), + }, + constant); + + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) + << "custom aggregator works without container cost term"; + } } From 034a1553154a6b059e8f19e47bc335605b67c2f7 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 28 Aug 2020 16:16:54 +0200 Subject: [PATCH 33/67] provide CostTerm class for all CostTerms This pattern allows cost::Constant to override the hierarchical cost computation for the SerialContainer and avoid traversing the graph. I implemented the CostTerm::supports() pattern over a full double visitor pattern with overloads for each SolutionBase specialization, because the SerialContainer needs to know whether cost aggregation of the subsolutions should take place or whether the SolutionSequence should be forwarded to the CostTerm. This would not be possible with a `virtual double operator()(const SolutionSequence&)` callback in CostTerm. Alternatively, implementing the hierarchical aggregation in the default implementation of this operator would be possible as well, but breaks intuition: - the corresponding methods to handle `SubTrajectory` and `WrappedSolution` *have to* default to not touching the solution's cost at all so it is inappropriate to have the default implementation for the Sequence do something else - The SerialContainer also aggregates costs outside the `computeCost()` interface (in multiple places in `onNewSolution()` to aggregate costs along partial paths) and thus moving the hierarchical aggregation to the CostTerm methods requires the aggregator to be shared between the Container and the CostTerm, The only shortcoming of the implemented approach, by contrast, is that user implementations that want to handle WrappedSolution or SolutionSequence differently have to ensure the supports_ flags are set correctly. Notice that most custom CostTerms will only ever access SubTrajectories and this case is simplified with the provided CostTerm constructors. --- .../moveit/task_constructor/cost_terms.h | 85 ++++++++++++++----- core/include/moveit/task_constructor/stage.h | 5 +- .../include/moveit/task_constructor/stage_p.h | 1 + .../include/moveit/task_constructor/storage.h | 8 +- core/src/cost_terms.cpp | 64 ++++++++++++-- core/src/stage.cpp | 7 -- core/src/stages/connect.cpp | 2 +- core/src/stages/move_relative.cpp | 2 +- core/src/stages/move_to.cpp | 2 +- core/test/test_cost_terms.cpp | 5 +- 10 files changed, 129 insertions(+), 52 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index d0ad411e..82c07920 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -39,36 +39,80 @@ #pragma once #include +#include namespace moveit { namespace task_constructor { -/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() + +class CostTerm +{ +public: + CostTerm(std::function); + CostTerm(std::function); + CostTerm(std::nullptr_t); + CostTerm(); + + // is this a valid CostTerm? + operator bool() const; + + double operator()(const SolutionBase& s, std::string& comment) const; + + /** describes the supported types of solutions that should be forwarded to this CostTerm + * + * CostTerms that support more than `SubTrajectory`s should inherit and overwrite the internal supports_ flag + */ + enum class SolutionType + { + NONE = 0, + TRAJECTORY = 1 << 0, + SEQUENCE = 1 << 1, + WRAPPER = 1 << 2, + ALL = TRAJECTORY | SEQUENCE | WRAPPER + }; + Flags supports() const; + +protected: + CostTerm(std::function, Flags); + + std::function term_; + Flags supports_; +}; + namespace cost { /// add a constant cost to each solution -struct Constant +class Constant : public CostTerm { public: - Constant(double c) : cost(c) {} - - double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; } + Constant(double c); double cost; }; /// trajectory length (interpolated between waypoints) -double PathLength(const SubTrajectory& s); +class PathLength : public CostTerm +{ +public: + PathLength(); + // TODO(v4hn): allow to consider specific joints only +}; /// execution duration of the whole trajectory -double TrajectoryDuration(const SubTrajectory& s); - -struct LinkMotion +class TrajectoryDuration : public CostTerm { - LinkMotion(std::string link_name) : link_name(link_name) {} +public: + TrajectoryDuration(); +}; - double operator()(const SubTrajectory&, std::string&) const; +class LinkMotion : public CostTerm +{ +public: + LinkMotion(std::string link_name); std::string link_name; + +protected: + double compute(const SubTrajectory&, std::string&) const; }; /** inverse distance to collision @@ -79,19 +123,20 @@ struct LinkMotion * \arg interface compute distances using START or END interface of solution *only*, instead of averaging over * trajectory * */ -struct Clearance +class Clearance : public CostTerm { +public: Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group", - Interface::Direction interface = Interface::NONE) - : with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {} - - double operator()(const SubTrajectory& s, std::string& comment) const; - + Interface::Direction interface = Interface::NONE); bool with_world; bool cumulative; std::string group_property; Interface::Direction interface; + +protected: + double compute(const SubTrajectory& s, std::string& comment) const; }; -} -} -} + +} // namespace cost +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 510d6070..57eeb4de 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -138,6 +138,7 @@ private: }; std::ostream& operator<<(std::ostream& os, const InitStageException& e); +class CostTerm; class ContainerBase; class StagePrivate; class Stage @@ -213,15 +214,11 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTermShort = std::function; /* \brief set method to determine costs for solutions of this stage * * For composite stages, costs are computed for each SubTrajectory and aggregated */ void setCostTerm(const CostTerm& term); - void setCostTerm(const CostTermShort& term); - // avoid overloading ambiguity for resetting the cost term - void setCostTerm(const std::nullptr_t) { setCostTerm(static_cast(nullptr)); } const ordered& solutions() const; const std::list& failures() const; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 0d094108..b9536b0c 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -40,6 +40,7 @@ #include #include +#include #include #include diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 20d646ec..f68226cc 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -193,13 +193,7 @@ private: using base_type::remove_if; }; -class SubTrajectory; -/** Interface for cost terms to be configured via Stage::setCostTerm() - * - * Look at moveit/task_constructor/cost_terms.h for various implementations - */ -using CostTerm = std::function; - +class CostTerm; class StagePrivate; class ContainerBasePrivate; /// abstract base class for solutions (primitive and sequences) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 69ce13f2..15a43bbe 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -42,13 +42,45 @@ #include +#include + #include namespace moveit { namespace task_constructor { + +CostTerm::CostTerm() : term_{ nullptr }, supports_{ SolutionType::NONE } {} + +CostTerm::CostTerm(std::nullptr_t) : CostTerm{} {} + +CostTerm::CostTerm(std::function t) + : term_{ [t](auto&& s, auto&& c) { return t(static_cast(s), c); } } + , supports_{ SolutionType::TRAJECTORY } {} + +CostTerm::CostTerm(std::function t) + : term_{ [t](auto&& s, auto&&) { return t(static_cast(s)); } } + , supports_{ SolutionType::TRAJECTORY } {} + +CostTerm::CostTerm(std::function term, Flags flags) + : term_{ term }, supports_{ flags } {} + +CostTerm::operator bool() const { + return supports_ != Flags{ SolutionType::NONE }; +} + +double CostTerm::operator()(const SolutionBase& s, std::string& comment) const { + assert(bool{ term_ }); + return term_(s, comment); +} + namespace cost { -double PathLength(const SubTrajectory& s) { +Constant::Constant(double c) + : cost{ c }, CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL } {} + +namespace { + +double pathLength(const SubTrajectory& s) { const auto& traj = s.trajectory(); if (traj == nullptr) @@ -60,11 +92,18 @@ double PathLength(const SubTrajectory& s) { return path_length; } -double TrajectoryDuration(const SubTrajectory& s) { - return s.trajectory() ? s.trajectory()->getDuration() : 0.0; -} +} // namespace -double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { +PathLength::PathLength() : CostTerm{ pathLength } {} + +TrajectoryDuration::TrajectoryDuration() + : CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {} + +LinkMotion::LinkMotion(std::string link) + : CostTerm{ [this](const SubTrajectory& s, std::string& c) { return this->compute(s, c); } } + , link_name{ std::move(link) } {} + +double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -87,7 +126,14 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) cons return distance; } -double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { +Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Interface::Direction interface) + : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } + , with_world(with_world) + , cumulative(cumulative) + , group_property(group_property) + , interface(interface) {} + +double Clearance::compute(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; collision_detection::DistanceRequest request; @@ -173,6 +219,6 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const return 1.0 / (distance + 1e-5); } -} -} -} +} // namespace cost +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stage.cpp b/core/src/stage.cpp index a0c75b69..d62eb2bf 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -355,13 +355,6 @@ void Stage::setCostTerm(const CostTerm& term) { pimpl()->cost_term_ = term; } -void Stage::setCostTerm(const CostTermShort& term) { - if (term) - setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); - else - pimpl()->cost_term_ = nullptr; -} - const ordered& Stage::solutions() const { return pimpl()->solutions_; } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index cb66a78d..afa23fe3 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -48,7 +48,7 @@ namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); - setCostTerm(cost::PathLength); + setCostTerm(cost::PathLength{}); auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 218cd721..0c4db5d5 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -49,7 +49,7 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLength); + setCostTerm(cost::PathLength{}); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index c151f580..0c8385c9 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -50,7 +50,7 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLength); + setCostTerm(cost::PathLength{}); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index ca6f382f..9d09a868 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -254,7 +254,8 @@ TEST(CostTerm, ForwardCanModifyCost) { auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; stage->setCostTerm(constant); - container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); + container.setCostTerm(static_cast>( + [](const SubTrajectory& s) { return s.cost() * s.cost(); })); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; @@ -319,7 +320,7 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { auto s3{ std::make_unique() }; - container.setCostTerm(cost::TrajectoryDuration); + container.setCostTerm(cost::TrajectoryDuration{}); container.computeWithStages({ std::move(c1), std::move(s3) }); EXPECT_EQ(container.solutions().front()->cost(), 3 * TRAJECTORY_DURATION) << "container cost term overwrites stage costs"; From 05932ad5f16a40b9e6f2a3cf1968d15b7a58deb0 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 1 Sep 2020 14:43:25 +0200 Subject: [PATCH 34/67] enable implicit construction of CostTerm from lambdas without the need for explicit casts to the correct signature. --- .../moveit/task_constructor/cost_terms.h | 19 ++++++++++-- core/src/cost_terms.cpp | 21 +++++++------- core/test/test_cost_terms.cpp | 29 +++++++++++++++++-- 3 files changed, 53 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 82c07920..f1dd680c 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -47,8 +47,15 @@ namespace task_constructor { class CostTerm { public: - CostTerm(std::function); - CostTerm(std::function); + using SubTrajectorySig = std::function; + using SubTrajectoryShortSig = std::function; + + // accept lambdas according to either signature above + template + CostTerm(const Term& t) : CostTerm{ decltype(sigMatcher(t)){ t } } {} + + CostTerm(const SubTrajectorySig&); + CostTerm(const SubTrajectoryShortSig&); CostTerm(std::nullptr_t); CostTerm(); @@ -72,10 +79,16 @@ public: Flags supports() const; protected: - CostTerm(std::function, Flags); + CostTerm(const std::function&, Flags); std::function term_; Flags supports_; + +private: + template + auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSig{}); + template + auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySig{}); }; namespace cost { diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 15a43bbe..612ff7b3 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -1,4 +1,4 @@ -/********************************************************************* +/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2020, Hamburg University @@ -53,15 +53,15 @@ CostTerm::CostTerm() : term_{ nullptr }, supports_{ SolutionType::NONE } {} CostTerm::CostTerm(std::nullptr_t) : CostTerm{} {} -CostTerm::CostTerm(std::function t) - : term_{ [t](auto&& s, auto&& c) { return t(static_cast(s), c); } } - , supports_{ SolutionType::TRAJECTORY } {} +CostTerm::CostTerm(const SubTrajectorySig& term) + : CostTerm{ [term](const SolutionBase& s, std::string& c) { return term(static_cast(s), c); }, + SolutionType::TRAJECTORY } {} -CostTerm::CostTerm(std::function t) - : term_{ [t](auto&& s, auto&&) { return t(static_cast(s)); } } - , supports_{ SolutionType::TRAJECTORY } {} +CostTerm::CostTerm(const SubTrajectoryShortSig& term) + : CostTerm{ [term](const SolutionBase& s, std::string&) { return term(static_cast(s)); }, + SolutionType::TRAJECTORY } {} -CostTerm::CostTerm(std::function term, Flags flags) +CostTerm::CostTerm(const std::function& term, Flags flags) : term_{ term }, supports_{ flags } {} CostTerm::operator bool() const { @@ -76,7 +76,7 @@ double CostTerm::operator()(const SolutionBase& s, std::string& comment) const { namespace cost { Constant::Constant(double c) - : cost{ c }, CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL } {} + : CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL }, cost{ c } {} namespace { @@ -100,8 +100,7 @@ TrajectoryDuration::TrajectoryDuration() : CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {} LinkMotion::LinkMotion(std::string link) - : CostTerm{ [this](const SubTrajectory& s, std::string& c) { return this->compute(s, c); } } - , link_name{ std::move(link) } {} + : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } }, link_name{ std::move(link) } {} double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 9d09a868..80488f30 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -173,6 +173,32 @@ public: } }; +TEST(CostTerm, SetLambdaCostTerm) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + const moveit::core::RobotModelConstPtr robot{ getModel() }; + + Standalone container(robot); + auto stage{ std::make_unique() }; + stage->setCostTerm([](auto&&) { return 1.0; }); + container.computeWithStages({ std::move(stage) }); + EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use simple lambda signature"; + + stage = std::make_unique(); + stage->setCostTerm([](auto&&, auto&&) { return 1.0; }); + container.computeWithStages({ std::move(stage) }); + EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature"; + + stage = std::make_unique(); + stage->setCostTerm([](auto&&, auto&& comment) { + comment = "I want the user to see this"; + return 1.0; + }); + container.computeWithStages({ std::move(stage) }); + auto sol = std::dynamic_pointer_cast(container.solutions().front()); + EXPECT_EQ(sol->cost(), 1.0); + EXPECT_EQ(sol->solutions()[0]->comment(), "I want the user to see this") << "can write to comment"; +} + TEST(CostTerm, CostOverwrite) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); const moveit::core::RobotModelConstPtr robot{ getModel() }; @@ -254,8 +280,7 @@ TEST(CostTerm, ForwardCanModifyCost) { auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; stage->setCostTerm(constant); - container.setCostTerm(static_cast>( - [](const SubTrajectory& s) { return s.cost() * s.cost(); })); + container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; From 64e2440c53d45379b07aae33aa1ee44fb7ed6635 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 15:42:25 +0200 Subject: [PATCH 35/67] simplify LinkMotion computation review feedback --- core/src/cost_terms.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 612ff7b3..d22ec5d4 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -116,10 +116,10 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { } double distance{ 0.0 }; - Eigen::Translation3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; + Eigen::Vector3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) { - Eigen::Translation3d new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; - distance += (new_position.vector() - position.vector()).norm(); + const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; + distance += (new_position - position).norm(); position = new_position; } return distance; From cdc7cb04fc8dbfc004219b3431e4024907b1e87b Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 16:07:09 +0200 Subject: [PATCH 36/67] refactor Forward -> PassThrough based on review feedback --- .../stages/{forward.h => passthrough.h} | 4 +-- core/src/stages/CMakeLists.txt | 4 +-- .../stages/{forward.cpp => passthrough.cpp} | 6 ++--- core/test/test_cost_terms.cpp | 26 +++++++++---------- 4 files changed, 20 insertions(+), 20 deletions(-) rename core/include/moveit/task_constructor/stages/{forward.h => passthrough.h} (94%) rename core/src/stages/{forward.cpp => passthrough.cpp} (91%) diff --git a/core/include/moveit/task_constructor/stages/forward.h b/core/include/moveit/task_constructor/stages/passthrough.h similarity index 94% rename from core/include/moveit/task_constructor/stages/forward.h rename to core/include/moveit/task_constructor/stages/passthrough.h index 6f54c35b..61f1c40d 100644 --- a/core/include/moveit/task_constructor/stages/forward.h +++ b/core/include/moveit/task_constructor/stages/passthrough.h @@ -49,10 +49,10 @@ namespace stages { * Useful to set a custom CostTransform via Stage::setCostTerm * to change a solution's cost without loosing the original value */ -class Forward : public WrapperBase +class PassThrough : public WrapperBase { public: - Forward(const std::string& name = "Forward", Stage::pointer&& child = Stage::pointer()); + PassThrough(const std::string& name = "PassThrough", Stage::pointer&& child = Stage::pointer()); void onNewSolution(const SolutionBase& s) override; }; diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 8becf93e..857a4977 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -5,11 +5,11 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/fixed_state.h ${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h - ${PROJECT_INCLUDE}/stages/forward.h ${PROJECT_INCLUDE}/stages/generate_pose.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/generate_place_pose.h ${PROJECT_INCLUDE}/stages/compute_ik.h + ${PROJECT_INCLUDE}/stages/passthrough.h ${PROJECT_INCLUDE}/stages/predicate_filter.h ${PROJECT_INCLUDE}/stages/connect.h @@ -25,11 +25,11 @@ add_library(${PROJECT_NAME}_stages current_state.cpp fixed_state.cpp fixed_cartesian_poses.cpp - forward.cpp generate_pose.cpp generate_grasp_pose.cpp generate_place_pose.cpp compute_ik.cpp + passthrough.cpp predicate_filter.cpp connect.cpp diff --git a/core/src/stages/forward.cpp b/core/src/stages/passthrough.cpp similarity index 91% rename from core/src/stages/forward.cpp rename to core/src/stages/passthrough.cpp index 2a650c71..786221e1 100644 --- a/core/src/stages/forward.cpp +++ b/core/src/stages/passthrough.cpp @@ -33,7 +33,7 @@ *********************************************************************/ /* Author: Michael 'v4hn' Goerner */ -#include +#include #include #include @@ -54,9 +54,9 @@ namespace moveit { namespace task_constructor { namespace stages { -Forward::Forward(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} +PassThrough::PassThrough(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} -void Forward::onNewSolution(const SolutionBase& s) { +void PassThrough::onNewSolution(const SolutionBase& s) { this->liftSolution(s); } diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 80488f30..c819c343 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include @@ -238,10 +238,10 @@ TEST(CostTerm, StageTypes) { EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator"; } -TEST(CostTerm, ForwardUsesCost) { +TEST(CostTerm, PassThroughUsesCost) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - Standalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; @@ -250,32 +250,32 @@ TEST(CostTerm, ForwardUsesCost) { container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward cost"; + EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "PassThrough forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals PassThrough cost"; } -TEST(CostTerm, ForwardOverwritesCost) { +TEST(CostTerm, PassThroughOverwritesCost) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - Standalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; stage->setCostTerm(constant_stage); - cost::Constant constant_forward{ 72.0 }; - container.setCostTerm(constant_forward); + cost::Constant constant_passthrough{ 72.0 }; + container.setCostTerm(constant_passthrough); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_forward.cost) << "Forward can apply custom cost"; + EXPECT_EQ(wrapped.cost(), constant_passthrough.cost) << "PassThrough can apply custom cost"; EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected"; } -TEST(CostTerm, ForwardCanModifyCost) { +TEST(CostTerm, PassThroughCanModifyCost) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - Standalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; @@ -284,7 +284,7 @@ TEST(CostTerm, ForwardCanModifyCost) { container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "Forward can compute cost based on child"; + EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "PassThrough can compute cost based on child"; EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost) << "child's cost is not affected"; } From f63d5b6892dc65a6dbb967e67503524689eeff16 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 17:41:58 +0200 Subject: [PATCH 37/67] Make transform in cost::Clearance configurable In theory this can be done with a PassThrough with a modifying transform, but this specific mapping is intrinsic to the definition of Clearance as a cost. --- core/include/moveit/task_constructor/cost_terms.h | 2 ++ core/src/cost_terms.cpp | 11 ++++++----- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index f1dd680c..f25a27c5 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -146,6 +146,8 @@ public: std::string group_property; Interface::Direction interface; + std::function distance_to_cost; + protected: double compute(const SubTrajectory& s, std::string& comment) const; }; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index d22ec5d4..72c99750 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -127,10 +127,11 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Interface::Direction interface) : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } - , with_world(with_world) - , cumulative(cumulative) - , group_property(group_property) - , interface(interface) {} + , with_world{ with_world } + , cumulative{ cumulative } + , group_property{ group_property } + , interface{ interface } + , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} double Clearance::compute(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; @@ -216,7 +217,7 @@ double Clearance::compute(const SubTrajectory& s, std::string& comment) const { comment = desc.str(); } - return 1.0 / (distance + 1e-5); + return distance_to_cost(distance); } } // namespace cost } // namespace task_constructor From cce7380f9def62c53956bce494a38e3c2a3b681c Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 20:23:24 +0200 Subject: [PATCH 38/67] Clearance: provide better interface for different modes Interface::Direction should always designate a valid direction. --- core/include/moveit/task_constructor/cost_terms.h | 13 +++++++++++-- core/include/moveit/task_constructor/storage.h | 1 - core/src/cost_terms.cpp | 10 +++++----- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index f25a27c5..6872a024 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -139,12 +139,21 @@ protected: class Clearance : public CostTerm { public: + enum class Mode + { + AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */, + START_INTERFACE, + END_INTERFACE, + TRAJECTORY + }; + Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group", - Interface::Direction interface = Interface::NONE); + Mode mode = Mode::AUTO); bool with_world; bool cumulative; std::string group_property; - Interface::Direction interface; + + Mode mode; std::function distance_to_cost; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index f68226cc..52138ae2 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -163,7 +163,6 @@ public: enum Direction { - NONE = 0, FORWARD, BACKWARD, START = FORWARD, diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 72c99750..0298a122 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -125,12 +125,12 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { return distance; } -Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Interface::Direction interface) +Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode) : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } , with_world{ with_world } , cumulative{ cumulative } , group_property{ group_property } - , interface{ interface } + , mode{ mode } , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} double Clearance::compute(const SubTrajectory& s, std::string& comment) const { @@ -140,7 +140,7 @@ double Clearance::compute(const SubTrajectory& s, std::string& comment) const { request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; - const auto& state{ (interface == Interface::END) ? s.end() : s.start() }; + const auto& state{ (mode == Mode::END_INTERFACE) ? s.end() : s.start() }; // prefer interface state property over stage property to find group_name // TODO: This pattern is general enough to justify its own interface (in the properties?). @@ -186,8 +186,8 @@ double Clearance::compute(const SubTrajectory& s, std::string& comment) const { double distance{ 0.0 }; - if (interface == Interface::START || interface == Interface::END || - (interface == Interface::NONE && s.trajectory() == nullptr)) { + if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE || + (mode == Mode::AUTO && s.trajectory() == nullptr)) { auto distance_data{ check_distance(state, state->scene()->getCurrentState()) }; if (distance_data.distance < 0) { comment = collision_comment(distance_data); From 45e6cdb47db40025dc9de3ff887f6079289e921c Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 14 Sep 2020 19:24:01 +0200 Subject: [PATCH 39/67] Revert "add CostAggregator for SerialContainer" This reverts commit dc7ce9bdfac97eb468f5a850adcb27cc118b5fd7. It turns out multiple places in SerialContainer's cost inference expect 0.0 as the neutral element (which is why std::min and multiply did not work). While these additional issues can be fixed, it would make the interface much less elegant. We should consider adding it back if an actual use-case is there to discuss. --- .../moveit/task_constructor/container.h | 6 --- .../moveit/task_constructor/container_p.h | 2 - .../include/moveit/task_constructor/storage.h | 10 ++--- core/src/container.cpp | 22 +++------ core/src/storage.cpp | 12 +---- core/test/test_cost_terms.cpp | 45 ------------------- 6 files changed, 10 insertions(+), 87 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 8ca032e7..27b2fde0 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -94,12 +94,6 @@ public: bool canCompute() const override; void compute() override; - /** Specify how to aggregate costs of childrens' solutions - * - * By default costs are added up - */ - void setCostAggregator(const SolutionSequence::CostAggregator& agg); - protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index cad54fcd..35a903ad 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -184,8 +184,6 @@ protected: // validate that child's interface matches mine (considering start or end only as determined by mask) template void validateInterface(const StagePrivate& child, InterfaceFlags required) const; - - SolutionSequence::CostAggregator aggregator_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 52138ae2..5bf81a1f 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -318,18 +318,15 @@ class SolutionSequence : public SolutionBase public: using container_type = std::vector; - explicit SolutionSequence(); - - SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr); + explicit SolutionSequence() : SolutionBase() {} + SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr) + : SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {} void push_back(const SolutionBase& solution); /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; - using CostAggregator = std::function; - void setCostAggregator(const CostAggregator&); - /// aggregate costs along the sequence double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -339,7 +336,6 @@ public: inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } private: - CostAggregator aggregator_; /// series of sub solutions container_type subsolutions_; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 1c1b0993..18892591 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -360,9 +360,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { - InterfaceState::Priority prio( - static_cast(in.first.size() + 1 + out.first.size()), - pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second)); + InterfaceState::Priority prio(static_cast(in.first.size() + 1 + out.first.size()), + in.second + current.cost() + out.second); // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { if (std::isinf(prio.cost())) @@ -375,9 +374,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - auto sequence{ std::make_shared(std::move(solution), prio.cost(), this) }; - sequence->setCostAggregator(pimpl()->aggregator_); - sorted.insert(std::move(sequence)); + sorted.insert(std::make_shared(std::move(solution), prio.cost(), this)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -396,7 +393,7 @@ SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(i SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name) - : ContainerBasePrivate(me, name), aggregator_(std::plus{}) {} + : ContainerBasePrivate(me, name) {} void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) { InterfaceFlags flags1 = stage1.requiredInterface(); @@ -521,10 +518,6 @@ void SerialContainerPrivate::validateConnectivity() const { } } -void SerialContainer::setCostAggregator(const SolutionSequence::CostAggregator& agg) { - pimpl()->aggregator_ = agg; -} - bool SerialContainer::canCompute() const { for (const auto& stage : pimpl()->children()) { if (stage->pimpl()->canCompute()) @@ -556,14 +549,11 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso else for (SolutionBase* successor : solutions) { trace.push_back(successor); - - double previous_trace_cost { trace_cost }; - - trace_cost = pimpl()->aggregator_(trace_cost, successor->cost()); + trace_cost += successor->cost(); traverse(*successor, cb, trace, trace_cost); - trace_cost = previous_trace_cost; + trace_cost -= successor->cost(); trace.pop_back(); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 1133257e..ff096a12 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -182,19 +182,10 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const return f(*this, comment); } -SolutionSequence::SolutionSequence() : SolutionBase(), aggregator_(std::plus{}) {} - -SolutionSequence::SolutionSequence(container_type&& subsolutions, double cost, Stage* creator) - : SolutionBase(creator, cost), aggregator_(std::plus{}), subsolutions_(std::move(subsolutions)) {} - void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } -void SolutionSequence::setCostAggregator(const CostAggregator& agg) { - aggregator_ = agg; -} - void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -234,8 +225,7 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co double cost{ 0.0 }; std::string subcomment; for (auto& solution : subsolutions_) { - auto subcost{ solution->computeCost(f, subcomment) }; - cost = aggregator_(cost, subcost); + cost += solution->computeCost(f, subcomment); if (!subcomment.empty()) { if (!comment.empty()) comment.append(", "); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index c819c343..7809a81b 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -11,8 +11,6 @@ #include -#include - using namespace moveit::task_constructor; using namespace planning_scene; @@ -351,47 +349,4 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { << "container cost term overwrites stage costs"; EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; } - -TEST(CostTerm, CompositeSolutionsContainerAggregator) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); - Standalone container{ getModel() }; - - cost::Constant constant{ 17.0 }; - - { - auto s1{ std::make_unique() }; - auto s2{ std::make_unique() }; - auto s3{ std::make_unique() }; - - container.computeWithStageCost({ std::move(s1), std::move(s2), std::move(s3) }, constant); - EXPECT_EQ(container.solutions().front()->cost(), 3 * constant.cost) << "default cost aggregates through stages"; - } - - { - container.setCostAggregator([](double a, double b) { return std::max(a, b); }); - container.computeWithContainerCost( - { - std::make_unique(), - std::make_unique(), - std::make_unique(), - }, - constant); - - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) - << "custom aggregator works with explicit cost term in container"; - } - - { - container.setCostAggregator([](auto a, auto b) { return std::max(a, b); }); - container.computeWithStageCost( - { - std::make_unique(), - std::make_unique(), - std::make_unique(), - }, - constant); - - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) - << "custom aggregator works without container cost term"; - } } From 62586c2688a884db41076c08feb44abc771b4e52 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 16 Sep 2020 19:08:53 +0200 Subject: [PATCH 40/67] rework CostTerm's from support flags to vtable requested in review. Without support for custom aggregators, which we dropped again after finding more flaws with it, I agree that this is the nicer solution. On the downside, it converts the interfaces from copyable objects to another round of shared_ptrs. I added shortcuts for lambda costs to keep support for `stage->setCostTerm([](auto&& s){ return 42; })` without the additional `stage->setCostTerm(LambdaCostTerm{ [](auto&& s){ return 42; } } )` --- .../moveit/task_constructor/cost_terms.h | 96 ++++++++++--------- core/include/moveit/task_constructor/stage.h | 16 ++-- .../include/moveit/task_constructor/stage_p.h | 2 +- core/src/cost_terms.cpp | 85 +++++++++------- core/src/stage.cpp | 20 +++- core/src/stages/connect.cpp | 2 +- core/src/stages/fix_collision_objects.cpp | 2 +- core/src/stages/fixed_cartesian_poses.cpp | 2 +- core/src/stages/fixed_state.cpp | 2 +- core/src/stages/generate_pose.cpp | 2 +- core/src/stages/modify_planning_scene.cpp | 2 +- core/src/stages/move_relative.cpp | 2 +- core/src/stages/move_to.cpp | 2 +- core/src/storage.cpp | 16 +--- core/test/test_cost_terms.cpp | 49 +++++----- 15 files changed, 163 insertions(+), 137 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 6872a024..d61dce15 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -44,51 +44,56 @@ namespace moveit { namespace task_constructor { +/** basic interface to compute costs for solutions + * + * If your cost term will only work on SubTrajectory solution objects, + * inherit from TrajectoryCostTerm instead. + */ + +MOVEIT_CLASS_FORWARD(CostTerm); class CostTerm { public: - using SubTrajectorySig = std::function; - using SubTrajectoryShortSig = std::function; + CostTerm() = default; + CostTerm(std::nullptr_t) : CostTerm{} {} + + virtual double operator()(const SubTrajectory& s, std::string& comment) const; + virtual double operator()(const SolutionSequence& s, std::string& comment) const; + virtual double operator()(const WrappedSolution& s, std::string& comment) const; +}; + +/** base class for cost terms that only work on SubTrajectory solutions + * + */ +class TrajectoryCostTerm : public CostTerm +{ +public: + double operator()(const WrappedSolution& s, std::string& comment) const override; +}; + +class LambdaCostTerm : public TrajectoryCostTerm +{ +public: + using SubTrajectorySignature = std::function; + using SubTrajectoryShortSignature = std::function; // accept lambdas according to either signature above - template - CostTerm(const Term& t) : CostTerm{ decltype(sigMatcher(t)){ t } } {} + template ()))> + LambdaCostTerm(const Term& t) : LambdaCostTerm{ Signature{ t } } {} - CostTerm(const SubTrajectorySig&); - CostTerm(const SubTrajectoryShortSig&); - CostTerm(std::nullptr_t); - CostTerm(); + LambdaCostTerm(const SubTrajectorySignature&); + LambdaCostTerm(const SubTrajectoryShortSignature&); - // is this a valid CostTerm? - operator bool() const; - - double operator()(const SolutionBase& s, std::string& comment) const; - - /** describes the supported types of solutions that should be forwarded to this CostTerm - * - * CostTerms that support more than `SubTrajectory`s should inherit and overwrite the internal supports_ flag - */ - enum class SolutionType - { - NONE = 0, - TRAJECTORY = 1 << 0, - SEQUENCE = 1 << 1, - WRAPPER = 1 << 2, - ALL = TRAJECTORY | SEQUENCE | WRAPPER - }; - Flags supports() const; + double operator()(const SubTrajectory& s, std::string& comment) const override; protected: - CostTerm(const std::function&, Flags); - - std::function term_; - Flags supports_; + SubTrajectorySignature term_; private: template - auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSig{}); + static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSignature{}); template - auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySig{}); + static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySignature{}); }; namespace cost { @@ -97,35 +102,39 @@ namespace cost { class Constant : public CostTerm { public: - Constant(double c); + Constant(double c) : cost{ c } {}; + + double operator()(const SubTrajectory&, std::string&) const override; + double operator()(const SolutionSequence& s, std::string& comment) const override; + double operator()(const WrappedSolution& s, std::string& comment) const override; double cost; }; /// trajectory length (interpolated between waypoints) -class PathLength : public CostTerm +class PathLength : public TrajectoryCostTerm { -public: - PathLength(); // TODO(v4hn): allow to consider specific joints only +public: + double operator()(const SubTrajectory&, std::string&) const override; }; /// execution duration of the whole trajectory -class TrajectoryDuration : public CostTerm +class TrajectoryDuration : public TrajectoryCostTerm { public: - TrajectoryDuration(); + double operator()(const SubTrajectory&, std::string&) const override; }; -class LinkMotion : public CostTerm +/** length of Cartesian trajection of a link */ +class LinkMotion : public TrajectoryCostTerm { public: LinkMotion(std::string link_name); std::string link_name; -protected: - double compute(const SubTrajectory&, std::string&) const; + double operator()(const SubTrajectory&, std::string&) const override; }; /** inverse distance to collision @@ -136,7 +145,7 @@ protected: * \arg interface compute distances using START or END interface of solution *only*, instead of averaging over * trajectory * */ -class Clearance : public CostTerm +class Clearance : public TrajectoryCostTerm { public: enum class Mode @@ -157,8 +166,7 @@ public: std::function distance_to_cost; -protected: - double compute(const SubTrajectory& s, std::string& comment) const; + double operator()(const SubTrajectory& s, std::string& comment) const override; }; } // namespace cost diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 57eeb4de..c6ac9b57 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -138,7 +138,8 @@ private: }; std::ostream& operator<<(std::ostream& os, const InitStageException& e); -class CostTerm; +MOVEIT_CLASS_FORWARD(CostTerm); +class LambdaCostTerm; class ContainerBase; class StagePrivate; class Stage @@ -214,11 +215,14 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - /* \brief set method to determine costs for solutions of this stage - * - * For composite stages, costs are computed for each SubTrajectory and aggregated - */ - void setCostTerm(const CostTerm& term); + /** set method to determine costs for solutions of this stage */ + + void setCostTerm(const CostTermConstPtr& term); + // overload to accept appropriate lambda expressions + template ::value>> + void setCostTerm(T term) { + setCostTerm(std::make_shared(term)); + } const ordered& solutions() const; const std::list& failures() const; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index b9536b0c..3982f7c6 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -170,7 +170,7 @@ protected: InterfacePtr ends_; // user-configurable cost estimator - CostTerm cost_term_; + CostTermConstPtr cost_term_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 0298a122..09439fdb 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -49,38 +49,60 @@ namespace moveit { namespace task_constructor { -CostTerm::CostTerm() : term_{ nullptr }, supports_{ SolutionType::NONE } {} - -CostTerm::CostTerm(std::nullptr_t) : CostTerm{} {} - -CostTerm::CostTerm(const SubTrajectorySig& term) - : CostTerm{ [term](const SolutionBase& s, std::string& c) { return term(static_cast(s), c); }, - SolutionType::TRAJECTORY } {} - -CostTerm::CostTerm(const SubTrajectoryShortSig& term) - : CostTerm{ [term](const SolutionBase& s, std::string&) { return term(static_cast(s)); }, - SolutionType::TRAJECTORY } {} - -CostTerm::CostTerm(const std::function& term, Flags flags) - : term_{ term }, supports_{ flags } {} - -CostTerm::operator bool() const { - return supports_ != Flags{ SolutionType::NONE }; +double CostTerm::operator()(const SubTrajectory& s, std::string&) const { + return s.cost(); } -double CostTerm::operator()(const SolutionBase& s, std::string& comment) const { +double CostTerm::operator()(const SolutionSequence& s, std::string& comment) const { + double cost{ 0.0 }; + std::string subcomment; + for (auto& solution : s.solutions()) { + cost += solution->computeCost((*this), subcomment); + if (!subcomment.empty()) { + if (!comment.empty()) + comment.append(", "); + comment.append(subcomment); + subcomment.clear(); + } + } + + return cost; +} + +double CostTerm::operator()(const WrappedSolution& s, std::string& /*comment*/) const { + return s.cost(); +} + +double TrajectoryCostTerm::operator()(const WrappedSolution& s, std::string& comment) const { + return s.wrapped()->computeCost(*this, comment); +} + +LambdaCostTerm::LambdaCostTerm(const SubTrajectorySignature& term) + : term_{ [term](const SolutionBase& s, std::string& c) { return term(static_cast(s), c); } } {} + +LambdaCostTerm::LambdaCostTerm(const SubTrajectoryShortSignature& term) + : term_{ [term](const SolutionBase& s, std::string&) { return term(static_cast(s)); } } {} + +double LambdaCostTerm::operator()(const SubTrajectory& s, std::string& comment) const { assert(bool{ term_ }); return term_(s, comment); } namespace cost { -Constant::Constant(double c) - : CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL }, cost{ c } {} +double Constant::operator()(const SubTrajectory&, std::string&) const { + return cost; +} -namespace { +double Constant::operator()(const SolutionSequence&, std::string&) const { + return cost; +} -double pathLength(const SubTrajectory& s) { +double Constant::operator()(const WrappedSolution&, std::string&) const { + return cost; +} + +double PathLength::operator()(const SubTrajectory& s, std::string&) const { const auto& traj = s.trajectory(); if (traj == nullptr) @@ -92,17 +114,13 @@ double pathLength(const SubTrajectory& s) { return path_length; } -} // namespace +double TrajectoryDuration::operator()(const SubTrajectory& s, std::string&) const { + return s.trajectory() ? s.trajectory()->getDuration() : 0.0; +} -PathLength::PathLength() : CostTerm{ pathLength } {} +LinkMotion::LinkMotion(std::string link) : link_name{ std::move(link) } {} -TrajectoryDuration::TrajectoryDuration() - : CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {} - -LinkMotion::LinkMotion(std::string link) - : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } }, link_name{ std::move(link) } {} - -double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -126,14 +144,13 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { } Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode) - : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } - , with_world{ with_world } + : with_world{ with_world } , cumulative{ cumulative } , group_property{ group_property } , mode{ mode } , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} -double Clearance::compute(const SubTrajectory& s, std::string& comment) const { +double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; collision_detection::DistanceRequest request; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index d62eb2bf..3d9c6f0e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -235,7 +235,7 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { // no reason to compute costs for a failed solution - if (solution.isFailure() || !cost_term_) + if (solution.isFailure()) return; // chicken-and-egg problem: we don't know whether/where we will store the solution yet, @@ -246,7 +246,9 @@ void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& solution.setEndState(tmp_to); std::string comment; - solution.setCost(solution.computeCost(cost_term_, comment)); + assert(cost_term_); + solution.setCost(solution.computeCost(*cost_term_, comment)); + // If a comment was specified, add it to the solution if (!comment.empty() && !solution.comment().empty()) { solution.setComment(solution.comment() + " (" + comment + ")"); @@ -297,8 +299,13 @@ void Stage::reset() { } void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { - // init properties once from parent auto impl = pimpl(); + + // add a default cost term if none was given by the user + if (!impl->cost_term_) + impl->cost_term_ = std::make_unique(); + + // init properties once from parent impl->properties_.reset(); if (impl->parent()) { try { @@ -351,8 +358,11 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } -void Stage::setCostTerm(const CostTerm& term) { - pimpl()->cost_term_ = term; +void Stage::setCostTerm(const CostTermConstPtr& term) { + if (!term) + pimpl()->cost_term_ = std::make_unique(); + else + pimpl()->cost_term_ = std::move(term); } const ordered& Stage::solutions() const { diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index afa23fe3..c9f4cd06 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -48,7 +48,7 @@ namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); - setCostTerm(cost::PathLength{}); + setCostTerm(std::make_unique()); auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 05b820e8..4546454e 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -56,7 +56,7 @@ namespace stages { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { // TODO: possibly weight solutions based on the required displacement? - setCostTerm(cost::Constant(0.0)); + setCostTerm(std::make_unique(0.0)); auto& p = properties(); p.declare("max_penetration", "maximally corrected penetration depth"); diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index d6f0d96a..068f2759 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -49,7 +49,7 @@ namespace stages { using PosesList = std::vector; FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::Constant(0.0)); + setCostTerm(std::make_unique(0.0)); auto& p = properties(); p.declare("poses", PosesList(), "target poses to spawn"); diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index 971b6da6..fddc9e4d 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -44,7 +44,7 @@ namespace task_constructor { namespace stages { FixedState::FixedState(const std::string& name) : Generator(name) { - setCostTerm(cost::Constant(0.0)); + setCostTerm(std::make_unique(0.0)); } void FixedState::setState(const planning_scene::PlanningScenePtr& scene) { diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index ebdb73ba..8315bc95 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -46,7 +46,7 @@ namespace task_constructor { namespace stages { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::Constant(0.0)); + setCostTerm(std::make_unique(0.0)); auto& p = properties(); p.declare("pose", "target pose to pass on in spawned states"); diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 8343151e..c14605a6 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -47,7 +47,7 @@ namespace task_constructor { namespace stages { ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { - setCostTerm(cost::Constant(0.0)); + setCostTerm(std::make_unique(0.0)); } void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 0c4db5d5..e5266825 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -49,7 +49,7 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLength{}); + setCostTerm(std::make_unique()); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 0c8385c9..f9f915c3 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -50,7 +50,7 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLength{}); + setCostTerm(std::make_unique()); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ff096a12..be1690ab 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -222,19 +222,7 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, } double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) const { - double cost{ 0.0 }; - std::string subcomment; - for (auto& solution : subsolutions_) { - cost += solution->computeCost(f, subcomment); - if (!subcomment.empty()) { - if (!comment.empty()) - comment.append(", "); - comment.append(subcomment); - subcomment.clear(); - } - } - - return cost; + return f(*this, comment); } void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, @@ -249,7 +237,7 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti } double WrappedSolution::computeCost(const CostTerm& f, std::string& comment) const { - return wrapped_->computeCost(f, comment); + return f(*this, comment); } } // namespace task_constructor diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 7809a81b..95fbcfd3 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -158,14 +158,14 @@ public: runCompute(); } - void computeWithStageCost(std::initializer_list stages, const CostTerm& cost_term) { + void computeWithStageCost(std::initializer_list stages, const CostTermConstPtr& cost_term) { this->setCostTerm(nullptr); for (const auto& stage : stages) const_cast(stage)->setCostTerm(cost_term); computeWithStages(stages); } - void computeWithContainerCost(std::initializer_list stages, const CostTerm& cost_term) { + void computeWithContainerCost(std::initializer_list stages, const CostTermPtr& cost_term) { this->setCostTerm(cost_term); computeWithStages(stages); } @@ -209,9 +209,9 @@ TEST(CostTerm, CostOverwrite) { container.computeWithStageCost({ std::make_unique() }, nullptr); EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; - cost::Constant constant_cost{ 1.0 }; + auto constant_cost{ std::make_shared(1.0) }; container.computeWithStageCost({ std::make_unique() }, constant_cost); - EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost) << "custom cost overwrites stage cost"; + EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost"; } TEST(CostTerm, StageTypes) { @@ -220,20 +220,20 @@ TEST(CostTerm, StageTypes) { Standalone container(robot); - const cost::Constant constant{ 1.0 }; + auto constant{ std::make_shared(1.0) }; // already tested above // cont.computeWithStageCost(std::make_unique(), constant); // EXPECT_EQ(cont.solutions().front()->cost(), constant.cost); container.computeWithStageCost({ std::make_unique() }, constant); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for connect"; + EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect"; container.computeWithStageCost({ std::make_unique() }, constant); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for forward propagator"; + EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator"; container.computeWithStageCost({ std::make_unique() }, constant); - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator"; + EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator"; } TEST(CostTerm, PassThroughUsesCost) { @@ -242,14 +242,14 @@ TEST(CostTerm, PassThroughUsesCost) { Standalone container(robot); auto stage{ std::make_unique() }; - cost::Constant constant_stage{ 84.0 }; + auto constant_stage{ std::make_shared(84.0) }; stage->setCostTerm(constant_stage); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "PassThrough forwards children's cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals PassThrough cost"; + EXPECT_EQ(wrapped.cost(), constant_stage->cost) << "PassThrough forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage->cost) << "Child cost equals PassThrough cost"; } TEST(CostTerm, PassThroughOverwritesCost) { @@ -258,16 +258,16 @@ TEST(CostTerm, PassThroughOverwritesCost) { Standalone container(robot); auto stage{ std::make_unique() }; - cost::Constant constant_stage{ 84.0 }; + auto constant_stage{ std::make_shared(84.0) }; stage->setCostTerm(constant_stage); - cost::Constant constant_passthrough{ 72.0 }; + auto constant_passthrough{ std::make_shared(72.0) }; container.setCostTerm(constant_passthrough); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_passthrough.cost) << "PassThrough can apply custom cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected"; + EXPECT_EQ(wrapped.cost(), constant_passthrough->cost) << "PassThrough can apply custom cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage->cost) << "child's cost is not affected"; } TEST(CostTerm, PassThroughCanModifyCost) { @@ -276,14 +276,14 @@ TEST(CostTerm, PassThroughCanModifyCost) { Standalone container(robot); auto stage{ std::make_unique() }; - cost::Constant constant{ 8.0 }; + auto constant{ std::make_shared(8.0) }; stage->setCostTerm(constant); container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "PassThrough can compute cost based on child"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost) << "child's cost is not affected"; + EXPECT_EQ(wrapped.cost(), constant->cost * constant->cost) << "PassThrough can compute cost based on child"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "child's cost is not affected"; } TEST(CostTerm, CompositeSolutions) { @@ -300,12 +300,12 @@ TEST(CostTerm, CompositeSolutions) { { auto s1{ std::make_unique() }; - cost::Constant constant{ 1.0 }; + auto constant{ std::make_shared(1.0) }; s1->setCostTerm(constant); auto s2{ std::make_unique() }; container.computeWithStages({ std::move(s1), std::move(s2) }); - EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant.cost) + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost) << "mix of explicit and implicit cost terms works"; } @@ -313,18 +313,18 @@ TEST(CostTerm, CompositeSolutions) { auto s1{ std::make_unique() }; auto s2{ std::make_unique() }; auto c1{ std::make_unique() }; - cost::Constant constant1{ 1.0 }; + auto constant1{ std::make_shared(1.0) }; s1->setCostTerm(constant1); s2->setCostTerm(constant1); c1->add(std::move(s1)); c1->add(std::move(s2)); auto s3{ std::make_unique() }; - cost::Constant constant2{ 9.0 }; + auto constant2{ std::make_shared(9.0) }; s3->setCostTerm(constant2); container.computeWithStages({ std::move(c1), std::move(s3) }); - EXPECT_EQ(container.solutions().front()->cost(), 2 * constant1.cost + constant2.cost) + EXPECT_EQ(container.solutions().front()->cost(), 2 * constant1->cost + constant2->cost) << "cost aggregation works across multiple levels"; } } @@ -343,10 +343,9 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { auto s3{ std::make_unique() }; - container.setCostTerm(cost::TrajectoryDuration{}); + container.setCostTerm(std::make_unique()); container.computeWithStages({ std::move(c1), std::move(s3) }); EXPECT_EQ(container.solutions().front()->cost(), 3 * TRAJECTORY_DURATION) << "container cost term overwrites stage costs"; EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; } -} From 687fcb6a6dc313bc9e08defecd5c92836375c340 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 17 Sep 2020 12:18:30 +0200 Subject: [PATCH 41/67] Update comments to solution's computeCost() methods --- core/include/moveit/task_constructor/storage.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 5bf81a1f..25aa8c51 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -248,7 +248,7 @@ public: Introspection* introspection = nullptr) const = 0; void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; - /// compute cost of this solution according to the specified CostTerm + /// required to dispatch to type-specific CostTerm methods via vtable virtual double computeCost(const CostTerm& cost, std::string& comment) const = 0; /// order solutions by their cost @@ -327,7 +327,6 @@ public: /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; - /// aggregate costs along the sequence double computeCost(const CostTerm& cost, std::string& comment) const override; const container_type& solutions() const { return subsolutions_; } @@ -359,7 +358,7 @@ public: : WrappedSolution(creator, wrapped, wrapped->cost()) {} void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const override; - /// compute cost of wrapped child + double computeCost(const CostTerm& cost, std::string& comment) const override; const SolutionBase* wrapped() const { return wrapped_; } From 5d0fd577549a12ceb8cc65d7827b94c998330d18 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 17 Sep 2020 22:56:19 +0200 Subject: [PATCH 42/67] initialize default CostTerm in constructor It seems to be too fragile to rely on `init` being called. On the other hand we should really enforce this anyway. -.- --- core/src/stage.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3d9c6f0e..58abd555 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -97,7 +97,12 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) { } StagePrivate::StagePrivate(Stage* me, const std::string& name) - : me_(me), name_(name), total_compute_time_{}, parent_(nullptr), introspection_(nullptr) {} + : me_{ me } + , name_{ name } + , cost_term_{ std::make_unique() } + , total_compute_time_{} + , parent_{ nullptr } + , introspection_{ nullptr } {} InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; @@ -301,10 +306,6 @@ void Stage::reset() { void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { auto impl = pimpl(); - // add a default cost term if none was given by the user - if (!impl->cost_term_) - impl->cost_term_ = std::make_unique(); - // init properties once from parent impl->properties_.reset(); if (impl->parent()) { From 04a8afb16c9675113cff69f7665e6355551c8e01 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 21 Sep 2020 14:00:49 +0200 Subject: [PATCH 43/67] melodic compatibility --- core/src/cost_terms.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 09439fdb..a1384c0a 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -175,9 +175,18 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { collision_detection::DistanceResult result; if (with_world) +#if MOVEIT_MASTER state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); +#else + state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(), + robot); +#endif else +#if MOVEIT_MASTER state->scene()->getCollisionEnv()->distanceSelf(request, result, robot); +#else + state->scene()->getCollisionRobot()->distanceSelf(request, result, robot); +#endif if (result.minimum_distance.distance <= 0) { return result.minimum_distance; From 198f8f53898c2ebee770f45ecb8f0b429c406639 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 17 Sep 2020 15:22:59 +0200 Subject: [PATCH 44/67] clearer naming of cost variables --- core/test/test_cost_terms.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 95fbcfd3..ee62f0c8 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -242,14 +242,14 @@ TEST(CostTerm, PassThroughUsesCost) { Standalone container(robot); auto stage{ std::make_unique() }; - auto constant_stage{ std::make_shared(84.0) }; - stage->setCostTerm(constant_stage); + auto constant{ std::make_shared(84.0) }; + stage->setCostTerm(constant); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_stage->cost) << "PassThrough forwards children's cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage->cost) << "Child cost equals PassThrough cost"; + EXPECT_EQ(wrapped.cost(), constant->cost) << "PassThrough forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "Child cost equals PassThrough cost"; } TEST(CostTerm, PassThroughOverwritesCost) { @@ -258,16 +258,16 @@ TEST(CostTerm, PassThroughOverwritesCost) { Standalone container(robot); auto stage{ std::make_unique() }; - auto constant_stage{ std::make_shared(84.0) }; - stage->setCostTerm(constant_stage); + auto constant_inner{ std::make_shared(84.0) }; + stage->setCostTerm(constant_inner); - auto constant_passthrough{ std::make_shared(72.0) }; - container.setCostTerm(constant_passthrough); + auto constant_outer{ std::make_shared(72.0) }; + container.setCostTerm(constant_outer); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_passthrough->cost) << "PassThrough can apply custom cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage->cost) << "child's cost is not affected"; + EXPECT_EQ(wrapped.cost(), constant_outer->cost) << "PassThrough can apply custom cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_inner->cost) << "child's cost is not affected"; } TEST(CostTerm, PassThroughCanModifyCost) { From 4089d5eefada7b1c654b76407dc9d72e0b6417ab Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 13 Jul 2020 16:49:27 +0200 Subject: [PATCH 45/67] Container::remove(): return removed Stage --- core/include/moveit/task_constructor/container.h | 4 ++-- core/include/moveit/task_constructor/container_p.h | 2 +- core/src/container.cpp | 13 +++++++------ 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 27b2fde0..02c5a347 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -65,8 +65,8 @@ public: void add(Stage::pointer&& stage); virtual bool insert(Stage::pointer&& stage, int before = -1); - virtual bool remove(int pos); - virtual bool remove(Stage* child); + virtual Stage::pointer remove(int pos); + virtual Stage::pointer remove(Stage* child); virtual void clear(); void reset() override; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 35a903ad..d762c34d 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -89,7 +89,7 @@ public: const_iterator childByIndex(int index, bool for_insert = false) const; /// remove child at given iterator position, returns fals if pos is invalid - bool remove(ContainerBasePrivate::const_iterator pos); + Stage::pointer remove(ContainerBasePrivate::const_iterator pos); /// traversing all stages up to max_depth bool traverseStages(const ContainerBase::StageCallback& processor, unsigned int cur_depth, diff --git a/core/src/container.cpp b/core/src/container.cpp index 18892591..68a47f88 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -204,20 +204,21 @@ bool ContainerBase::insert(Stage::pointer&& stage, int before) { return true; } -bool ContainerBasePrivate::remove(ContainerBasePrivate::const_iterator pos) { +Stage::pointer ContainerBasePrivate::remove(ContainerBasePrivate::const_iterator pos) { if (pos == children_.end()) - return false; + return Stage::pointer(); (*pos)->pimpl()->unparent(); - children_.erase(pos); - return true; + Stage::pointer result = std::move(*children_.erase(pos, pos)); // stage from non-const iterator to pos + children_.erase(pos); // actually erase stage + return result; } -bool ContainerBase::remove(int pos) { +Stage::pointer ContainerBase::remove(int pos) { return pimpl()->remove(pimpl()->childByIndex(pos, false)); } -bool ContainerBase::remove(Stage* child) { +Stage::pointer ContainerBase::remove(Stage* child) { auto it = pimpl()->children_.begin(), end = pimpl()->children_.end(); for (; it != end && it->get() != child; ++it) ; From d0dc384bdf8e08cda0211dba90fb7e56b0f1c8c9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 15 Jul 2020 15:08:23 +0200 Subject: [PATCH 46/67] Update TaskModel for all columns, including time --- visualization/motion_planning_tasks/src/remote_task_model.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 565037c8..3fb547fa 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -353,7 +353,7 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs: // emit notify about model changes when node was already visited if (n->node_flags_ & WAS_VISITED) { QModelIndex idx = index(n); - dataChanged(idx.sibling(idx.row(), 1), idx.sibling(idx.row(), 2)); + dataChanged(idx.sibling(idx.row(), 1), idx.sibling(idx.row(), 3)); } } } From 8152994e52f588afc82383367242d38d51486816 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 11 Jul 2020 16:54:14 +0200 Subject: [PATCH 47/67] Fix interface resolution --- core/src/container.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 18892591..b3c3d656 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -756,8 +756,8 @@ void Fallbacks::onNewSolution(const SolutionBase& s) { MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} void MergerPrivate::resolveInterface(InterfaceFlags expected) { - ContainerBasePrivate::resolveInterface(expected); - switch (interfaceFlags()) { + ParallelContainerBasePrivate::resolveInterface(expected); + switch (requiredInterface()) { case PROPAGATE_FORWARDS: case PROPAGATE_BACKWARDS: case UNKNOWN: From a0d54b6e695ea167f4855f6a1f95c22c9e93bab6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 23 Sep 2020 07:49:31 +0200 Subject: [PATCH 48/67] Adapt codecov.yaml unifying it with MoveIt's config file --- codecov.yaml | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/codecov.yaml b/codecov.yaml index 132f7ec6..4b9d534f 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -1,14 +1,10 @@ coverage: + precision: 2 + round: up + range: "45...70" status: project: default: target: auto - # allow coverage to drop by this amount and still post success threshold: 0.5% - base: auto - branches: - - master - if_not_found: success - if_ci_failed: error - only_pulls: false - informational: true + patch: off From 14fc3d25864f8cba1e2a028051a38c9308540228 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 11 Jul 2020 16:54:40 +0200 Subject: [PATCH 49/67] Reject failures early --- core/src/container.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index b3c3d656..a43419f8 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -805,6 +805,9 @@ void Merger::compute() { } void Merger::onNewSolution(const SolutionBase& s) { + if (s.isFailure()) // ignore failure solutions + return; + auto impl = pimpl(); switch (impl->interfaceFlags()) { case PROPAGATE_FORWARDS: @@ -821,8 +824,8 @@ void Merger::onNewSolution(const SolutionBase& s) { void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) { const SubTrajectory* trajectory = dynamic_cast(&s); - if (!trajectory) { - ROS_ERROR_NAMED("Merger", "Only simple trajectories are supported"); + if (!trajectory || !trajectory->trajectory()) { + ROS_ERROR_NAMED("Merger", "Only simple, valid trajectories are supported"); return; } @@ -872,7 +875,7 @@ void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) { StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared(std::move(t))); } -void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s) { +void MergerPrivate::onNewGeneratorSolution(const SolutionBase& /* s */) { // TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states } @@ -918,13 +921,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, // transform vector of SubTrajectories into vector of RobotTrajectories std::vector sub_trajectories; sub_trajectories.reserve(sub_solutions.size()); - for (const auto& sub : sub_solutions) { - // TODO: directly skip failures in mergeAnyCombination() or even earlier - if (sub->isFailure()) - return; - if (sub->trajectory()) - sub_trajectories.push_back(sub->trajectory()); - } + for (const auto& sub : sub_solutions) + sub_trajectories.push_back(sub->trajectory()); moveit::core::JointModelGroup* jmg = jmg_merged_.get(); robot_trajectory::RobotTrajectoryPtr merged; From 7499f7b297882794d037639e6785a256d8146f44 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 11 Jul 2020 16:55:18 +0200 Subject: [PATCH 50/67] Report merge failures --- core/src/container.cpp | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index a43419f8..299c5ff3 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -937,18 +937,28 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, if (!merged) return; - // check merged trajectory for collisions - if (!start_scene->isPathValid(*merged)) - return; - SubTrajectory t(merged); - // accumulate costs and markers - double costs = 0.0; - for (const auto& sub : sub_solutions) { - costs += sub->cost(); - t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end()); + + // check merged trajectory for collisions + std::vector invalid_index; + if (!start_scene->isPathValid(*merged, "", true, &invalid_index)) { + t.markAsFailure(); + std::ostringstream oss; + oss << "Invalid waypoint(s): "; + if (invalid_index.size() == merged->getWayPointCount()) + oss << "all"; + else for (size_t i : invalid_index) + oss << i << ", "; + t.setComment(oss.str()); + } else { + // accumulate costs and markers + double costs = 0.0; + for (const auto& sub : sub_solutions) { + costs += sub->cost(); + t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end()); + } + t.setCost(costs); } - t.setCost(costs); spawner(std::move(t)); } } // namespace task_constructor From 88c217e9a6fae04eab35312a5a6e443bf41642a5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 22 Sep 2020 10:54:51 +0200 Subject: [PATCH 51/67] Report exceptions (e.g. config issues) --- core/src/container.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 299c5ff3..d05d10b1 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -864,14 +864,16 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) { void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) { // generate target state planning_scene::PlanningScenePtr to = from->scene()->diff(); - to->setCurrentState(t.trajectory()->getLastWayPoint()); + if (t.trajectory() && !t.trajectory()->empty()) + to->setCurrentState(t.trajectory()->getLastWayPoint()); StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared(std::move(t))); } void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) { // generate target state planning_scene::PlanningScenePtr from = to->scene()->diff(); - from->setCurrentState(t.trajectory()->getFirstWayPoint()); + if (t.trajectory() && !t.trajectory()->empty()) + from->setCurrentState(t.trajectory()->getFirstWayPoint()); StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared(std::move(t))); } @@ -929,14 +931,16 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions, try { merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg); } catch (const std::runtime_error& e) { - ROS_INFO_STREAM_NAMED("Merger", this->name() << "Merging failed: " << e.what()); + SubTrajectory t; + t.markAsFailure(); + t.setComment(e.what()); + spawner(std::move(t)); return; } if (jmg_merged_.get() != jmg) jmg_merged_.reset(jmg); - if (!merged) - return; + assert(merged); SubTrajectory t(merged); // check merged trajectory for collisions From be270cb57466d456ede2131d6cfaa8257b8428d2 Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Sat, 24 Oct 2020 04:22:05 +0300 Subject: [PATCH 52/67] Task: Add ability to set timeout (#213) Task::setTimeout will allow setting an overall timeout. --- core/include/moveit/task_constructor/task.h | 3 +++ core/src/task.cpp | 7 ++++++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index ce6e6c04..51c4066d 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -112,6 +112,9 @@ public: using WrapperBase::removeSolutionCallback; using WrapperBase::SolutionCallback; + using WrapperBase::setTimeout; + using WrapperBase::timeout; + /// reset all stages void reset() final; /// initialize all stages with given scene diff --git a/core/src/task.cpp b/core/src/task.cpp index 83f096c4..56baab8f 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -109,6 +109,8 @@ Task::Task(const std::string& id, bool introspection, ContainerBase::pointer&& c if (!id.empty()) stages()->setName(id); + setTimeout(std::numeric_limits::max()); + // monitor state on commandline // addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout))); // enable introspection by default, but only if ros::init() was called @@ -296,8 +298,11 @@ bool Task::plan(size_t max_solutions) { init(); impl->preempt_requested_ = false; + const double available_time = timeout(); + const auto start_time = std::chrono::steady_clock::now(); while (ros::ok() && !impl->preempt_requested_ && canCompute() && - (max_solutions == 0 || numSolutions() < max_solutions)) { + (max_solutions == 0 || numSolutions() < max_solutions) && + std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() < available_time) { compute(); for (const auto& cb : impl->task_cbs_) cb(*this); From cb0f20cad739fcaea38162aa4fd8e215bac26814 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 24 Oct 2020 03:04:35 +0200 Subject: [PATCH 53/67] Improve task unittests --- core/src/task.cpp | 3 +- core/test/test_container.cpp | 81 +++++++++++++++++++++++++++++++----- 2 files changed, 72 insertions(+), 12 deletions(-) diff --git a/core/src/task.cpp b/core/src/task.cpp index 56baab8f..f27ca630 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -300,8 +300,7 @@ bool Task::plan(size_t max_solutions) { impl->preempt_requested_ = false; const double available_time = timeout(); const auto start_time = std::chrono::steady_clock::now(); - while (ros::ok() && !impl->preempt_requested_ && canCompute() && - (max_solutions == 0 || numSolutions() < max_solutions) && + while (!impl->preempt_requested_ && canCompute() && (max_solutions == 0 || numSolutions() < max_solutions) && std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() < available_time) { compute(); for (const auto& cb : impl->task_cbs_) diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index 4aacb106..8ad4007d 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -8,6 +8,8 @@ #include "gtest_value_printers.h" #include #include +#include +#include using namespace moveit::task_constructor; @@ -15,14 +17,18 @@ static unsigned int MOCK_ID = 0; class GeneratorMockup : public Generator { + moveit::core::RobotModelConstPtr robot; int runs = 0; public: GeneratorMockup(int runs = 0) : Generator("generator " + std::to_string(++MOCK_ID)), runs(runs) {} + void init(const moveit::core::RobotModelConstPtr& robot_model) override { robot = robot_model; } bool canCompute() const override { return runs > 0; } void compute() override { - if (runs > 0) + if (runs > 0) { --runs; + spawn(InterfaceState(std::make_shared(robot)), SubTrajectory()); + } } }; @@ -33,7 +39,9 @@ public: : MonitoringGenerator("monitoring generator " + std::to_string(++MOCK_ID), monitored) {} bool canCompute() const override { return false; } void compute() override {} - void onNewSolution(const SolutionBase& /*solution*/) override {} + void onNewSolution(const SolutionBase& s) override { + spawn(InterfaceState(s.end()->scene()->diff()), SubTrajectory()); + } }; class PropagatorMockup : public PropagatingEitherWay @@ -44,13 +52,17 @@ class PropagatorMockup : public PropagatingEitherWay public: PropagatorMockup(int fw = 0, int bw = 0) : PropagatingEitherWay("propagate " + std::to_string(++MOCK_ID)), fw_runs(fw), bw_runs(bw) {} - void computeForward(const InterfaceState& /* from */) override { - if (fw_runs > 0) + void computeForward(const InterfaceState& from) override { + if (fw_runs > 0) { --fw_runs; + sendForward(from, InterfaceState(from.scene()->diff()), SubTrajectory()); + } } - void computeBackward(const InterfaceState& /* from */) override { - if (bw_runs > 0) + void computeBackward(const InterfaceState& to) override { + if (bw_runs > 0) { --bw_runs; + sendBackward(InterfaceState(to.scene()->diff()), to, SubTrajectory()); + } } }; class ForwardMockup : public PropagatorMockup @@ -70,15 +82,29 @@ public: } }; +// ForwardMockup that takes a while for its computation +class TimedForwardMockup : public ForwardMockup +{ + std::chrono::milliseconds duration_; + +public: + TimedForwardMockup(std::chrono::milliseconds duration) : ForwardMockup(1000), duration_(duration) {} + void computeForward(const InterfaceState& from) override { + std::this_thread::sleep_for(duration_); + ForwardMockup::computeForward(from); + } +}; + class ConnectMockup : public Connecting { int runs = 0; public: ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(++MOCK_ID)), runs(runs) {} - void compute(const InterfaceState& /* from */, const InterfaceState& /* to */) override { + void compute(const InterfaceState& from, const InterfaceState& to) override { if (runs > 0) --runs; + connect(from, to, std::make_shared()); } }; @@ -673,13 +699,13 @@ TEST(Task, reuse) { ref->setState(scene); t.add(Stage::pointer(ref)); - t.add(std::make_unique()); + t.add(std::make_unique(2)); t.add(std::make_unique(ref)); }; try { configure(t); - t.plan(1); + EXPECT_TRUE(t.plan(1)); t = Task("second"); t.setRobotModel(robot_model); @@ -688,8 +714,43 @@ TEST(Task, reuse) { EXPECT_EQ(static_cast(t.stages()->pimpl()->parent()), static_cast(&t)); configure(t); - t.plan(1); + EXPECT_TRUE(t.plan(1)); } catch (const InitStageException& e) { ADD_FAILURE() << "InitStageException:" << std::endl << e << t; } } + +TEST(Task, timeout) { + // create dummy robot model + moveit::core::RobotModelBuilder builder("robot", "base"); + builder.addChain("base->a->b->c", "continuous"); + builder.addGroupChain("base", "c", "group"); + + Task t; + t.setRobotModel(builder.build()); + + auto timeout = std::chrono::milliseconds(10); + t.add(std::make_unique(100)); // allow up to 100 solutions spawned + t.add(std::make_unique(timeout)); + + // no timeout, but limited number of solutions + EXPECT_TRUE(t.plan(3)); + EXPECT_EQ(t.solutions().size(), 3u); + + // zero timeout fails + t.reset(); + t.setTimeout(0.0); + EXPECT_FALSE(t.plan()); + + // time for 1 solution + t.reset(); + t.setTimeout(std::chrono::duration(timeout).count()); + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 1u); + + // time for 2 solutions + t.reset(); + t.setTimeout(std::chrono::duration(2 * timeout).count()); + EXPECT_TRUE(t.plan()); + EXPECT_EQ(t.solutions().size(), 2u); +} From 8b8666c64ea90f34d042d415aa345292c745f014 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Oct 2020 17:33:40 +0200 Subject: [PATCH 54/67] TaskPanel: Fix assertion When loading an .rviz config with MTC displays disabled, the mainloop_job to create a TaskPanel will never be executed (because the display is disabled). Removing the display will then hit the assertion that DISPLAY_COUNT > 0. Fixed, by not relying on Display::update, but just scheduling a Qt GUI job via QTimer::singleShot(). --- visualization/motion_planning_tasks/src/task_display.cpp | 4 ++-- visualization/motion_planning_tasks/src/task_panel.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 149e3e9f..227e2643 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -98,8 +98,8 @@ void TaskDisplay::onInitialize() { trajectory_visual_->onInitialize(scene_node_, context_); task_list_model_->setDisplayContext(context_); // create a new TaskPanel by default - // by post-poning this to main loop, we can ensure that rviz has loaded everything before - mainloop_jobs_.addJob([this]() { TaskPanel::incDisplayCount(context_->getWindowManager()); }); + // by post-poning this to Qt's GUI loop, we can ensure that rviz has loaded everything before + QTimer::singleShot(0, [this]() { TaskPanel::incDisplayCount(context_->getWindowManager()); }); } void TaskDisplay::loadRobotModel() { diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 08cf33bd..ab20b78a 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -135,7 +135,7 @@ void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager) { rviz::VisualizationFrame* vis_frame = dynamic_cast(window_manager); if (SINGLETON || !vis_frame) - return; // already define, nothing to do + return; // already defined, nothing to do QDockWidget* dock = vis_frame->addPanelByName("Motion Planning Tasks", "moveit_task_constructor/Motion Planning Tasks", From 43828506b523b63dd1c8cfa2aa78ec7b2ebf764d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 23 Oct 2020 03:12:39 +0200 Subject: [PATCH 55/67] Fix TaskView segfault We shouldn't "change" solution/property models if they didn't actually changed. --- .../motion_planning_tasks/src/task_panel.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index ab20b78a..9428356d 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -407,12 +407,12 @@ void TaskView::onCurrentStageChanged(const QModelIndex& current, const QModelInd QItemSelectionModel* sm = view->selectionModel(); QAbstractItemModel* m = task ? task->getSolutionModel(task_index) : nullptr; - view->setModel(m); - view->sortByColumn(sort_column, sort_order); - delete sm; // we don't store the selection model - sm = view->selectionModel(); + if (view->model() != m) { + view->setModel(m); + view->sortByColumn(sort_column, sort_order); + delete sm; // we don't store the selection model - if (sm) { + sm = view->selectionModel(); connect(sm, SIGNAL(currentChanged(QModelIndex, QModelIndex)), this, SLOT(onCurrentSolutionChanged(QModelIndex, QModelIndex))); connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)), this, @@ -420,11 +420,13 @@ void TaskView::onCurrentStageChanged(const QModelIndex& current, const QModelInd } // update the PropertyModel - QTreeView* pview = d_ptr->property_view; - sm = pview->selectionModel(); + view = d_ptr->property_view; + sm = view->selectionModel(); m = task ? task->getPropertyModel(task_index) : nullptr; - pview->setModel(m); - delete sm; // we don't store the selection model + if (view->model() != m) { + view->setModel(m); + delete sm; // we don't store the selection model + } } void TaskView::onCurrentSolutionChanged(const QModelIndex& current, const QModelIndex& previous) { From d78048719dcb919ec2ddb1aa21b7f2cb8e11e61c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 23 Oct 2020 10:22:33 +0200 Subject: [PATCH 56/67] Fix memory leakage --- .../properties/property_from_yaml.cpp | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp index 8931f31b..3ef34146 100644 --- a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp +++ b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp @@ -49,6 +49,17 @@ namespace mtc = ::moveit::task_constructor; namespace { +class ScopedYamlEvent +{ +public: + ~ScopedYamlEvent() { yaml_event_delete(&event_); } + operator yaml_event_t const &() const { return event_; } + operator yaml_event_t&() { return event_; } + +private: + yaml_event_t event_; +}; + // Event-based YAML parser, creating an rviz::Property tree // https://www.wpsoftware.net/andrew/pages/libyaml.html class Parser @@ -60,10 +71,11 @@ public: ~Parser(); rviz::Property* process(const QString& name, const QString& description, rviz::Property* old) const; + +private: static rviz::Property* createScalar(const QString& name, const QString& description, const QByteArray& value, rviz::Property* old); -private: // return true if there was no error so far bool noError() const { return parser_.error == YAML_NO_ERROR; } // parse a single event and return it's type, YAML_ERROR_EVENT on parsing error @@ -98,7 +110,6 @@ Parser::~Parser() { int Parser::parse(yaml_event_t& event) const { if (!yaml_parser_parse(&parser_, &event)) { - yaml_event_delete(&event); return YAML_ERROR_EVENT; } return event.type; @@ -108,7 +119,7 @@ int Parser::parse(yaml_event_t& event) const { rviz::Property* Parser::process(const QString& name, const QString& description, rviz::Property* old) const { bool stop = false; while (!stop) { - yaml_event_t event; + ScopedYamlEvent event; switch (parse(event)) { case YAML_ERROR_EVENT: return Parser::createScalar(name, description, "YAML error", old); @@ -205,7 +216,7 @@ rviz::Property* Parser::processMapping(const QString& name, const QString& descr int index = 0; // current child index in root bool stop = false; while (!stop && noError()) { // parse all map items - yaml_event_t event; + ScopedYamlEvent event; switch (parse(event)) { // parse key case YAML_MAPPING_END_EVENT: // all fine, reached end of mapping stop = true; @@ -263,7 +274,7 @@ rviz::Property* Parser::processSequence(const QString& name, const QString& desc int index = 0; // current child index in root bool stop = false; while (!stop && noError()) { // parse all map items - yaml_event_t event; + ScopedYamlEvent event; switch (parse(event)) { case YAML_SEQUENCE_END_EVENT: // all fine, reached end of sequence stop = true; From c7151dcc48037c37ee1217fbe92c458d99b63b56 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 23 Oct 2020 14:43:18 +0200 Subject: [PATCH 57/67] Remove mainloop jobs All topics are handled by the Display's update_nh_ within the main GUI thread anyway. --- .../src/task_display.cpp | 42 ++++++++----------- .../motion_planning_tasks/src/task_display.h | 3 -- 2 files changed, 17 insertions(+), 28 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 227e2643..8afecaa9 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -172,7 +172,6 @@ void TaskDisplay::calculateOffsetPosition() { void TaskDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); - mainloop_jobs_.executeJobs(); trajectory_visual_->update(wall_dt, ros_dt); } @@ -199,38 +198,31 @@ inline std::string getUniqueId(const std::string& process_id, const std::string& void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) { const std::string id = getUniqueId(msg->process_id, msg->id); - mainloop_jobs_.addJob([this, id, msg]() { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); - task_list_model_->processTaskDescriptionMessage(id, *msg); + setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); + task_list_model_->processTaskDescriptionMessage(id, *msg); - // Start listening to other topics if this is the first description - // Waiting for the description ensures we do not receive data that cannot be interpreted yet - if (!received_task_description_ && !msg->stages.empty()) { - received_task_description_ = true; - task_statistics_sub = - update_nh_.subscribe(base_ns_ + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this); - task_solution_sub = update_nh_.subscribe(base_ns_ + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this); - } - }); + // Start listening to other topics if this is the first description + // Waiting for the description ensures we do not receive data that cannot be interpreted yet + if (!received_task_description_ && !msg->stages.empty()) { + received_task_description_ = true; + task_statistics_sub = update_nh_.subscribe(base_ns_ + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this); + task_solution_sub = update_nh_.subscribe(base_ns_ + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this); + } } void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg) { const std::string id = getUniqueId(msg->process_id, msg->id); - mainloop_jobs_.addJob([this, id, msg]() { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); - task_list_model_->processTaskStatisticsMessage(id, *msg); - }); + setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); + task_list_model_->processTaskStatisticsMessage(id, *msg); } void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg) { const std::string id = getUniqueId(msg->process_id, msg->task_id); - mainloop_jobs_.addJob([this, id, msg]() { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); - const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg); - if (s) - trajectory_visual_->showTrajectory(s, false); - return; - }); + setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); + const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg); + if (s) + trajectory_visual_->showTrajectory(s, false); + return; } void TaskDisplay::changedTaskSolutionTopic() { @@ -256,7 +248,7 @@ void TaskDisplay::changedTaskSolutionTopic() { base_ns_ = solution_topic.toStdString().substr(0, solution_topic.length() - strlen(SOLUTION_TOPIC)); // listen to task descriptions updates - task_description_sub = update_nh_.subscribe(base_ns_ + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this); + task_description_sub = update_nh_.subscribe(base_ns_ + DESCRIPTION_TOPIC, 10, &TaskDisplay::taskDescriptionCB, this); // service to request solutions get_solution_client = diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index a220079a..12877733 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -121,9 +121,6 @@ protected: ros::Subscriber task_statistics_sub; ros::ServiceClient get_solution_client; - // handle processing of task+solution messages in Qt mainloop - moveit::tools::JobQueue mainloop_jobs_; - // The trajectory playback component std::unique_ptr trajectory_visual_; // The TaskListModel storing actual task and solution data From 3a7a4eb953619fc101ebae69664a6397cfecac1c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 23 Oct 2020 15:21:43 +0200 Subject: [PATCH 58/67] Connect: Fix segfault when accessing solutions from multiple plan() calls Connect::init() was resetting its dynamically created JMG merged_jmg_, thus invalidating all previous solutions. Only reset the JMG in reset(). --- core/src/stages/connect.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index c9f4cd06..f2552618 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -86,9 +86,8 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) { } } - if (!errors && groups.size() >= 2) { // enable merging? + if (!errors && groups.size() >= 2 && !merged_jmg_) { // enable merging? try { - merged_jmg_.reset(); merged_jmg_.reset(task_constructor::merge(groups)); } catch (const std::runtime_error& e) { ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging."); From b071a059f988c05e386b9e8120ae998e785d082b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Oct 2020 18:14:27 +0200 Subject: [PATCH 59/67] Rename TaskPrivate::id -> TaskPrivate::ns TaskPrivate's id_ actually served as a namespace parameter. --- core/include/moveit/task_constructor/task.h | 4 +--- core/include/moveit/task_constructor/task_p.h | 6 ++--- core/src/introspection.cpp | 6 +---- core/src/task.cpp | 15 ++++-------- msgs/msg/Solution.msg | 5 +--- msgs/msg/TaskDescription.msg | 5 +--- msgs/msg/TaskStatistics.msg | 5 +--- .../src/task_display.cpp | 24 +++++-------------- .../src/task_list_model.cpp | 21 +++++++--------- .../src/task_list_model.h | 6 ++--- .../test/test_task_model.cpp | 15 ++++++------ 11 files changed, 38 insertions(+), 74 deletions(-) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 51c4066d..f1ac3dfa 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -78,14 +78,12 @@ public: createPlanner(const moveit::core::RobotModelConstPtr& model, const std::string& ns = "move_group", const std::string& planning_plugin_param_name = "planning_plugin", const std::string& adapter_plugins_param_name = "request_adapters"); - Task(const std::string& id = "", bool introspection = true, + Task(const std::string& ns = "", bool introspection = true, ContainerBase::pointer&& container = std::make_unique("task pipeline")); Task(Task&& other); // NOLINT(performance-noexcept-move-constructor) Task& operator=(Task&& other); // NOLINT(performance-noexcept-move-constructor) ~Task() override; - const std::string& id() const; - const moveit::core::RobotModelConstPtr& getRobotModel() const; /// setting the robot model also resets the task void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model); diff --git a/core/include/moveit/task_constructor/task_p.h b/core/include/moveit/task_constructor/task_p.h index 87c10975..68cda9a3 100644 --- a/core/include/moveit/task_constructor/task_p.h +++ b/core/include/moveit/task_constructor/task_p.h @@ -53,15 +53,15 @@ class TaskPrivate : public WrapperBasePrivate friend class Task; public: - TaskPrivate(Task* me, const std::string& id); - const std::string& id() const { return id_; } + TaskPrivate(Task* me, const std::string& ns); + const std::string& ns() const { return ns_; } const ContainerBase* stages() const; protected: static void swap(StagePrivate*& lhs, StagePrivate*& rhs); private: - std::string id_; + std::string ns_; robot_model_loader::RobotModelLoaderPtr robot_model_loader_; moveit::core::RobotModelConstPtr robot_model_; bool preempt_requested_; diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 8873c68a..3d1820d7 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -63,7 +63,7 @@ class IntrospectionPrivate { public: IntrospectionPrivate(const TaskPrivate* task) - : nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace + : nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace , task_(task) , process_id_(getProcessId()) { task_description_publisher_ = @@ -82,7 +82,6 @@ public: // send empty task description message to indicate reset ::moveit_task_constructor_msgs::TaskDescription msg; msg.process_id = process_id_; - msg.id = task_->id(); task_description_publisher_.publish(msg); } @@ -144,7 +143,6 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, co s.start()->scene()->getPlanningSceneMsg(msg.start_scene); msg.process_id = impl->process_id_; - msg.task_id = impl->task_->id(); } void Introspection::publishSolution(const SolutionBase& s) { @@ -242,7 +240,6 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription msg.stages.clear(); impl->task_->stages()->traverseRecursively(stage_processor); - msg.id = impl->task_->id(); msg.process_id = impl->process_id_; return msg; } @@ -263,7 +260,6 @@ Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg.stages.clear(); impl->task_->stages()->traverseRecursively(stage_processor); - msg.id = impl->task_->id(); msg.process_id = impl->process_id_; return msg; } diff --git a/core/src/task.cpp b/core/src/task.cpp index 56baab8f..9d61dc7f 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -71,8 +71,8 @@ std::string rosNormalizeName(const std::string& name) { namespace moveit { namespace task_constructor { -TaskPrivate::TaskPrivate(Task* me, const std::string& id) - : WrapperBasePrivate(me, std::string()), id_(rosNormalizeName(id)), preempt_requested_(false) {} +TaskPrivate::TaskPrivate(Task* me, const std::string& ns) + : WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {} void swap(StagePrivate*& lhs, StagePrivate*& rhs) { // It only makes sense to swap pimpl instances of a Task! @@ -104,11 +104,8 @@ const ContainerBase* TaskPrivate::stages() const { return children().empty() ? nullptr : static_cast(children().front().get()); } -Task::Task(const std::string& id, bool introspection, ContainerBase::pointer&& container) - : WrapperBase(new TaskPrivate(this, id), std::move(container)) { - if (!id.empty()) - stages()->setName(id); - +Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& container) + : WrapperBase(new TaskPrivate(this, ns), std::move(container)) { setTimeout(std::numeric_limits::max()); // monitor state on commandline @@ -360,10 +357,6 @@ void Task::setProperty(const std::string& name, const boost::any& value) { wrapped()->setProperty(name, value); } -const std::string& Task::id() const { - return pimpl()->id(); -} - const core::RobotModelConstPtr& Task::getRobotModel() const { auto impl = pimpl(); return impl->robot_model_; diff --git a/msgs/msg/Solution.msg b/msgs/msg/Solution.msg index d4f330e2..3d5ca768 100644 --- a/msgs/msg/Solution.msg +++ b/msgs/msg/Solution.msg @@ -1,9 +1,6 @@ -# id of generating process +# id of generating task string process_id -# id of associated task -string task_id - # planning scene of start state moveit_msgs/PlanningScene start_scene diff --git a/msgs/msg/TaskDescription.msg b/msgs/msg/TaskDescription.msg index f9386744..5098432b 100644 --- a/msgs/msg/TaskDescription.msg +++ b/msgs/msg/TaskDescription.msg @@ -1,8 +1,5 @@ -# id of generating process +# unique id of this task string process_id -# unique ID of this task -string id - # list of all stages, including the task stage itself StageDescription[] stages diff --git a/msgs/msg/TaskStatistics.msg b/msgs/msg/TaskStatistics.msg index 3fc62ce3..c1fc9bdd 100644 --- a/msgs/msg/TaskStatistics.msg +++ b/msgs/msg/TaskStatistics.msg @@ -1,8 +1,5 @@ -# id of generating process +# unique id of generating task string process_id -# unique of this task -string id - # list of all stages, including the task stage itself StageStatistics[] stages diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 149e3e9f..cf713482 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -188,20 +188,10 @@ void TaskDisplay::changedRobotDescription() { loadRobotModel(); } -inline std::string getUniqueId(const std::string& process_id, const std::string& task_id) { - std::string id{ process_id }; - if (!task_id.empty()) { - id += "/"; - id += task_id; - } - return id; -} - void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) { - const std::string id = getUniqueId(msg->process_id, msg->id); - mainloop_jobs_.addJob([this, id, msg]() { + mainloop_jobs_.addJob([this, msg]() { setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); - task_list_model_->processTaskDescriptionMessage(id, *msg); + task_list_model_->processTaskDescriptionMessage(*msg); // Start listening to other topics if this is the first description // Waiting for the description ensures we do not receive data that cannot be interpreted yet @@ -215,18 +205,16 @@ void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDesc } void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg) { - const std::string id = getUniqueId(msg->process_id, msg->id); - mainloop_jobs_.addJob([this, id, msg]() { + mainloop_jobs_.addJob([this, msg]() { setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); - task_list_model_->processTaskStatisticsMessage(id, *msg); + task_list_model_->processTaskStatisticsMessage(*msg); }); } void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg) { - const std::string id = getUniqueId(msg->process_id, msg->task_id); - mainloop_jobs_.addJob([this, id, msg]() { + mainloop_jobs_.addJob([this, msg]() { setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); - const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg); + const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg); if (s) trajectory_visual_->showTrajectory(s, false); return; diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 7336af2b..831165cf 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -240,10 +240,9 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const { // process a task description message: // update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one -void TaskListModel::processTaskDescriptionMessage(const std::string& id, - const moveit_task_constructor_msgs::TaskDescription& msg) { - // retrieve existing or insert new remote task for given id - auto it_inserted = remote_tasks_.insert(std::make_pair(id, nullptr)); +void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg) { + // retrieve existing or insert new remote task for given task id + auto it_inserted = remote_tasks_.insert(std::make_pair(msg.process_id, nullptr)); bool created = it_inserted.second; const auto& task_it = it_inserted.first; RemoteTaskModel*& remote_task = task_it->second; @@ -275,17 +274,16 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id, // insert newly created model into this' model instance if (created) { - ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.id.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.process_id.c_str()); insertModel(remote_task, -1); } } // process a task statistics message -void TaskListModel::processTaskStatisticsMessage(const std::string& id, - const moveit_task_constructor_msgs::TaskStatistics& msg) { - auto it = remote_tasks_.find(id); +void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg) { + auto it = remote_tasks_.find(msg.process_id); if (it == remote_tasks_.cend()) { - ROS_WARN("unknown task: %s", id.c_str()); + ROS_WARN("unknown task: %s", msg.process_id.c_str()); return; } @@ -296,9 +294,8 @@ void TaskListModel::processTaskStatisticsMessage(const std::string& id, remote_task->processStageStatistics(msg.stages); } -DisplaySolutionPtr TaskListModel::processSolutionMessage(const std::string& id, - const moveit_task_constructor_msgs::Solution& msg) { - auto it = remote_tasks_.find(id); +DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) { + auto it = remote_tasks_.find(msg.process_id); if (it == remote_tasks_.cend()) return DisplaySolutionPtr(); // unkown task diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index f18c3817..7e9883a5 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -157,11 +157,11 @@ public: QVariant data(const QModelIndex& index, int role) const override; /// process an incoming task description message - only call in Qt's main loop - void processTaskDescriptionMessage(const std::string& id, const moveit_task_constructor_msgs::TaskDescription& msg); + void processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg); /// process an incoming task description message - only call in Qt's main loop - void processTaskStatisticsMessage(const std::string& id, const moveit_task_constructor_msgs::TaskStatistics& msg); + void processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg); /// process an incoming solution message - only call in Qt's main loop - DisplaySolutionPtr processSolutionMessage(const std::string& id, const moveit_task_constructor_msgs::Solution& msg); + DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg); /// insert a TaskModel, pos is relative to modelCount() bool insertModel(BaseTaskModel* model, int pos = -1); diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 461f0bba..fbb779c7 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -55,10 +55,11 @@ protected: int num_inserts = 0; int num_updates = 0; - moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name) { + moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name, + const std::string& process_id = std::string()) { moveit_task_constructor_msgs::TaskDescription t; - t.id = name; uint id = 0, root_id; + t.process_id = process_id.empty() ? name : process_id; moveit_task_constructor_msgs::StageDescription desc; desc.parent_id = id; @@ -127,7 +128,7 @@ protected: SCOPED_TRACE("first i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskDescriptionMessage("1", genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first")); if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task @@ -141,7 +142,7 @@ protected: SCOPED_TRACE("second i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskDescriptionMessage("2", genMsg("second")); // 1 notify for inserted task + model.processTaskDescriptionMessage(genMsg("second")); // 1 notify for inserted task if (i == 0) EXPECT_EQ(num_inserts, 1); @@ -200,13 +201,13 @@ TEST_F(TaskListModelTest, threeChildren) { TEST_F(TaskListModelTest, visitedPopulate) { // first population without children children = 0; - model.processTaskDescriptionMessage("1", genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first")); validate(model, { "first" }); // validation visits root node EXPECT_EQ(num_inserts, 1); children = 3; num_inserts = 0; - model.processTaskDescriptionMessage("1", genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first")); validate(model, { "first" }); // second population with children should emit insert notifies for them EXPECT_EQ(num_inserts, 3); @@ -215,7 +216,7 @@ TEST_F(TaskListModelTest, visitedPopulate) { TEST_F(TaskListModelTest, deletion) { children = 3; - model.processTaskDescriptionMessage("1", genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first")); auto m = model.getModel(model.index(0, 0)).first; int num_deletes = 0; QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; }); From c471879b08a5a2471ad1deea84160a86f16adef3 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Oct 2020 18:29:12 +0200 Subject: [PATCH 60/67] Rename process_id -> task_id --- core/src/introspection.cpp | 19 +++++++++++-------- msgs/msg/Solution.msg | 2 +- msgs/msg/TaskDescription.msg | 2 +- msgs/msg/TaskStatistics.msg | 2 +- .../src/task_list_model.cpp | 10 +++++----- .../test/test_task_model.cpp | 4 ++-- 6 files changed, 21 insertions(+), 18 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 3d1820d7..9e41d409 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -46,16 +46,19 @@ #include #include +#include #include namespace moveit { namespace task_constructor { namespace { -std::string getProcessId() { +std::string getTaskId(const TaskPrivate* task) { + std::ostringstream oss; char our_hostname[256] = { 0 }; gethostname(our_hostname, sizeof(our_hostname) - 1); - return std::to_string(getpid()) + "@" + our_hostname; + oss << our_hostname << "_" << getpid() << "_" << reinterpret_cast(task); + return oss.str(); } } // namespace @@ -65,7 +68,7 @@ public: IntrospectionPrivate(const TaskPrivate* task) : nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace , task_(task) - , process_id_(getProcessId()) { + , task_id_(getTaskId(task)) { task_description_publisher_ = nh_.advertise(DESCRIPTION_TOPIC, 2, true); // send reset message as early as possible to give subscribers time to see it @@ -81,7 +84,7 @@ public: void indicateReset() { // send empty task description message to indicate reset ::moveit_task_constructor_msgs::TaskDescription msg; - msg.process_id = process_id_; + msg.task_id = task_id_; task_description_publisher_.publish(msg); } @@ -96,7 +99,7 @@ public: ros::NodeHandle nh_; /// associated task const TaskPrivate* task_; - const std::string process_id_; + const std::string task_id_; /// publish task detailed description and current state ros::Publisher task_description_publisher_; @@ -142,7 +145,7 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, co s.fillMessage(msg, this); s.start()->scene()->getPlanningSceneMsg(msg.start_scene); - msg.process_id = impl->process_id_; + msg.task_id = impl->task_id_; } void Introspection::publishSolution(const SolutionBase& s) { @@ -240,7 +243,7 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription msg.stages.clear(); impl->task_->stages()->traverseRecursively(stage_processor); - msg.process_id = impl->process_id_; + msg.task_id = impl->task_id_; return msg; } @@ -260,7 +263,7 @@ Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg.stages.clear(); impl->task_->stages()->traverseRecursively(stage_processor); - msg.process_id = impl->process_id_; + msg.task_id = impl->task_id_; return msg; } } // namespace task_constructor diff --git a/msgs/msg/Solution.msg b/msgs/msg/Solution.msg index 3d5ca768..ebdffff9 100644 --- a/msgs/msg/Solution.msg +++ b/msgs/msg/Solution.msg @@ -1,5 +1,5 @@ # id of generating task -string process_id +string task_id # planning scene of start state moveit_msgs/PlanningScene start_scene diff --git a/msgs/msg/TaskDescription.msg b/msgs/msg/TaskDescription.msg index 5098432b..551ff924 100644 --- a/msgs/msg/TaskDescription.msg +++ b/msgs/msg/TaskDescription.msg @@ -1,5 +1,5 @@ # unique id of this task -string process_id +string task_id # list of all stages, including the task stage itself StageDescription[] stages diff --git a/msgs/msg/TaskStatistics.msg b/msgs/msg/TaskStatistics.msg index c1fc9bdd..ec95569c 100644 --- a/msgs/msg/TaskStatistics.msg +++ b/msgs/msg/TaskStatistics.msg @@ -1,5 +1,5 @@ # unique id of generating task -string process_id +string task_id # list of all stages, including the task stage itself StageStatistics[] stages diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 831165cf..7c439529 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -242,7 +242,7 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const { // update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg) { // retrieve existing or insert new remote task for given task id - auto it_inserted = remote_tasks_.insert(std::make_pair(msg.process_id, nullptr)); + auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr)); bool created = it_inserted.second; const auto& task_it = it_inserted.first; RemoteTaskModel*& remote_task = task_it->second; @@ -274,16 +274,16 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_ // insert newly created model into this' model instance if (created) { - ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.process_id.c_str()); + ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.task_id.c_str()); insertModel(remote_task, -1); } } // process a task statistics message void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg) { - auto it = remote_tasks_.find(msg.process_id); + auto it = remote_tasks_.find(msg.task_id); if (it == remote_tasks_.cend()) { - ROS_WARN("unknown task: %s", msg.process_id.c_str()); + ROS_WARN("unknown task: %s", msg.task_id.c_str()); return; } @@ -295,7 +295,7 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_m } DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) { - auto it = remote_tasks_.find(msg.process_id); + auto it = remote_tasks_.find(msg.task_id); if (it == remote_tasks_.cend()) return DisplaySolutionPtr(); // unkown task diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index fbb779c7..67255862 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -56,10 +56,10 @@ protected: int num_updates = 0; moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name, - const std::string& process_id = std::string()) { + const std::string& task_id = std::string()) { moveit_task_constructor_msgs::TaskDescription t; uint id = 0, root_id; - t.process_id = process_id.empty() ? name : process_id; + t.task_id = task_id.empty() ? name : task_id; moveit_task_constructor_msgs::StageDescription desc; desc.parent_id = id; From 8b1c66c4b7694ae7e5816453ea2cd97fd3942585 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Oct 2020 18:34:11 +0200 Subject: [PATCH 61/67] Allow naming a Task which just names the top-level container --- core/include/moveit/task_constructor/task.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index f1ac3dfa..051f12b4 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -84,6 +84,9 @@ public: Task& operator=(Task&& other); // NOLINT(performance-noexcept-move-constructor) ~Task() override; + const std::string& name() const { return stages()->name(); } + void setName(const std::string& name) { stages()->setName(name); } + const moveit::core::RobotModelConstPtr& getRobotModel() const; /// setting the robot model also resets the task void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model); From b91d87c6eb969df9814ca75d7e95bfa68a570ece Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 22 Oct 2020 20:55:28 +0200 Subject: [PATCH 62/67] IntroSpection: indicateReset() on disable --- core/src/introspection.cpp | 9 +++++---- core/src/task.cpp | 1 + 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 9e41d409..7f1246b3 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -65,7 +65,7 @@ std::string getTaskId(const TaskPrivate* task) { class IntrospectionPrivate { public: - IntrospectionPrivate(const TaskPrivate* task) + IntrospectionPrivate(const TaskPrivate* task, Introspection* self) : nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace , task_(task) , task_id_(getTaskId(task)) { @@ -78,8 +78,11 @@ public: nh_.advertise(STATISTICS_TOPIC, 1, true); solution_publisher_ = nh_.advertise(SOLUTION_TOPIC, 1, true); + get_solution_service_ = nh_.advertiseService(GET_SOLUTION_SERVICE, &Introspection::getSolution, self); + resetMaps(); } + ~IntrospectionPrivate() { indicateReset(); } void indicateReset() { // send empty task description message to indicate reset @@ -114,9 +117,7 @@ public: boost::bimap id_solution_bimap_; }; -Introspection::Introspection(const TaskPrivate* task) : impl(new IntrospectionPrivate(task)) { - impl->get_solution_service_ = impl->nh_.advertiseService(GET_SOLUTION_SERVICE, &Introspection::getSolution, this); -} +Introspection::Introspection(const TaskPrivate* task) : impl(new IntrospectionPrivate(task, this)) {} Introspection::~Introspection() { delete impl; diff --git a/core/src/task.cpp b/core/src/task.cpp index 9d61dc7f..44318346 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -173,6 +173,7 @@ planning_pipeline::PlanningPipelinePtr Task::createPlanner(const robot_model::Ro Task::~Task() { auto impl = pimpl(); + impl->introspection_.reset(); // stop introspection clear(); // remove all stages impl->robot_model_.reset(); // only destroy loader after all references to the model are gone! From cf5031e89df984b4ba2bd799864f4a6c5efb671a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 23 Oct 2020 00:30:20 +0200 Subject: [PATCH 63/67] Create task-specific ServiceClients to fetch solutions This finally allows to have multiple tasks publishing in the same namespace. --- core/src/introspection.cpp | 3 ++- .../src/remote_task_model.cpp | 15 +++++++-------- .../src/remote_task_model.h | 12 ++++-------- .../src/task_display.cpp | 9 ++------- .../motion_planning_tasks/src/task_display.h | 2 -- .../src/task_list_model.cpp | 11 +++-------- .../src/task_list_model.h | 9 +++------ .../motion_planning_tasks/test/CMakeLists.txt | 2 +- .../test/test_task_model.cpp | 19 +++++++++++++------ .../test/test_task_model.launch | 3 --- 10 files changed, 35 insertions(+), 50 deletions(-) diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 7f1246b3..e2225bd2 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -78,7 +78,8 @@ public: nh_.advertise(STATISTICS_TOPIC, 1, true); solution_publisher_ = nh_.advertise(SOLUTION_TOPIC, 1, true); - get_solution_service_ = nh_.advertiseService(GET_SOLUTION_SERVICE, &Introspection::getSolution, self); + get_solution_service_ = + nh_.advertiseService(std::string(GET_SOLUTION_SERVICE "_") + task_id_, &Introspection::getSolution, self); resetMaps(); } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 3fb547fa..9bd7eb37 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -45,7 +45,6 @@ #include #include #include -#include #include #include @@ -181,20 +180,19 @@ QModelIndex RemoteTaskModel::index(const Node* n) const { return QModelIndex(); } -RemoteTaskModel::RemoteTaskModel(const planning_scene::PlanningSceneConstPtr& scene, +RemoteTaskModel::RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name, + const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, QObject* parent) : BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) { id_to_stage_[0] = root_; // root node has ID 0 + // service to request solutions + get_solution_client_ = nh.serviceClient(service_name); } RemoteTaskModel::~RemoteTaskModel() { delete root_; } -void RemoteTaskModel::setSolutionClient(ros::ServiceClient* client) { - get_solution_client_ = client; -} - int RemoteTaskModel::rowCount(const QModelIndex& parent) const { if (parent.column() > 0) return 0; @@ -420,15 +418,16 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) { // to avoid some communication overhead DisplaySolutionPtr result; - if (!(flags_ & IS_DESTROYED) && get_solution_client_) { + if (!(flags_ & IS_DESTROYED)) { // request solution via service moveit_task_constructor_msgs::GetSolution srv; srv.request.solution_id = id; try { - if (get_solution_client_->call(srv)) { + if (get_solution_client_.call(srv)) { id_to_solution_[id] = result = processSolutionMessage(srv.response.solution); return result; } else { // on failure mark remote task as destroyed: don't retrieve more solutions + get_solution_client_.shutdown(); flags_ |= IS_DESTROYED; } } catch (const std::exception& e) { diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index e9f07e77..8db812be 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -38,13 +38,10 @@ #include "task_list_model.h" #include +#include #include #include -namespace ros { -class ServiceClient; -} - namespace moveit_rviz_plugin { class RemoteSolutionModel; @@ -57,7 +54,7 @@ class RemoteTaskModel : public BaseTaskModel Q_OBJECT struct Node; Node* const root_; - ros::ServiceClient* get_solution_client_ = nullptr; + ros::ServiceClient get_solution_client_; std::map id_to_stage_; std::map id_to_solution_; @@ -70,12 +67,11 @@ class RemoteTaskModel : public BaseTaskModel void setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info); public: - RemoteTaskModel(const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, + RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name, + const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, QObject* parent = nullptr); ~RemoteTaskModel() override; - void setSolutionClient(ros::ServiceClient* client); - int rowCount(const QModelIndex& parent = QModelIndex()) const override; QModelIndex index(int row, int column, const QModelIndex& parent = QModelIndex()) const override; diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index cf713482..d0fc09a7 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -61,7 +61,6 @@ namespace moveit_rviz_plugin { TaskDisplay::TaskDisplay() : Display(), received_task_description_(false) { task_list_model_.reset(new TaskListModel); - task_list_model_->setSolutionClient(&get_solution_client); MetaTaskListModel::instance().insertModel(task_list_model_.get(), this); @@ -191,7 +190,8 @@ void TaskDisplay::changedRobotDescription() { void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) { mainloop_jobs_.addJob([this, msg]() { setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); - task_list_model_->processTaskDescriptionMessage(*msg); + task_list_model_->processTaskDescriptionMessage(*msg, update_nh_, + base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id); // Start listening to other topics if this is the first description // Waiting for the description ensures we do not receive data that cannot be interpreted yet @@ -229,7 +229,6 @@ void TaskDisplay::changedTaskSolutionTopic() { task_description_sub.shutdown(); task_statistics_sub.shutdown(); task_solution_sub.shutdown(); - get_solution_client.shutdown(); received_task_description_ = false; @@ -246,10 +245,6 @@ void TaskDisplay::changedTaskSolutionTopic() { // listen to task descriptions updates task_description_sub = update_nh_.subscribe(base_ns_ + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this); - // service to request solutions - get_solution_client = - update_nh_.serviceClient(base_ns_ + GET_SOLUTION_SERVICE); - setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received"); } diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index a220079a..97107933 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -45,7 +45,6 @@ #include "job_queue.h" #include #include -#include #include #include #include @@ -119,7 +118,6 @@ protected: ros::Subscriber task_solution_sub; ros::Subscriber task_description_sub; ros::Subscriber task_statistics_sub; - ros::ServiceClient get_solution_client; // handle processing of task+solution messages in Qt mainloop moveit::tools::JobQueue mainloop_jobs_; diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 7c439529..85ce5023 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -42,7 +42,6 @@ #include "icons.h" #include -#include #include #include @@ -184,10 +183,6 @@ void TaskListModel::setDisplayContext(rviz::DisplayContext* display_context) { display_context_ = display_context; } -void TaskListModel::setSolutionClient(ros::ServiceClient* client) { - get_solution_client_ = client; -} - void TaskListModel::setStageFactory(const StageFactoryPtr& factory) { stage_factory_ = factory; if (stage_factory_) @@ -240,7 +235,8 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const { // process a task description message: // update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one -void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg) { +void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg, + ros::NodeHandle& nh, const std::string& service_name) { // retrieve existing or insert new remote task for given task id auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr)); bool created = it_inserted.second; @@ -261,8 +257,7 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_ } } else if (created) { // create new task model, if ID was not known before // the model is managed by this instance via Qt's parent-child mechanism - remote_task = new RemoteTaskModel(scene_, display_context_, this); - remote_task->setSolutionClient(get_solution_client_); + remote_task = new RemoteTaskModel(nh, service_name, scene_, display_context_, this); // HACK: always use the last created model as active active_task_model_ = remote_task; diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 7e9883a5..52ea5e69 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -52,9 +53,6 @@ #include #include -namespace ros { -class ServiceClient; -} namespace rviz { class PropertyTreeModel; class DisplayContext; @@ -132,7 +130,6 @@ class TaskListModel : public utils::FlatMergeProxyModel // if task is destroyed remotely, it is marked with flag IS_DESTROYED // if task is removed locally from tasks vector, it is marked with a nullptr std::map remote_tasks_; - ros::ServiceClient* get_solution_client_ = nullptr; // factory used to create stages StageFactoryPtr stage_factory_; @@ -148,7 +145,6 @@ public: void setScene(const planning_scene::PlanningSceneConstPtr& scene); void setDisplayContext(rviz::DisplayContext* display_context); - void setSolutionClient(ros::ServiceClient* client); void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; } int columnCount(const QModelIndex& parent = QModelIndex()) const override { return 4; } @@ -157,7 +153,8 @@ public: QVariant data(const QModelIndex& index, int role) const override; /// process an incoming task description message - only call in Qt's main loop - void processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg); + void processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg, ros::NodeHandle& nh, + const std::string& service_name); /// process an incoming task description message - only call in Qt's main loop void processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg); /// process an incoming solution message - only call in Qt's main loop diff --git a/visualization/motion_planning_tasks/test/CMakeLists.txt b/visualization/motion_planning_tasks/test/CMakeLists.txt index 38592c7c..a35c0373 100644 --- a/visualization/motion_planning_tasks/test/CMakeLists.txt +++ b/visualization/motion_planning_tasks/test/CMakeLists.txt @@ -16,5 +16,5 @@ if (CATKIN_ENABLE_TESTING) add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp) target_link_libraries(${PROJECT_NAME}-test-task_model - motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest_main) + motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest) endif() diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 67255862..f231700e 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -50,6 +50,7 @@ using namespace moveit::task_constructor; class TaskListModelTest : public ::testing::Test { protected: + ros::NodeHandle nh; moveit_rviz_plugin::TaskListModel model; int children = 0; int num_inserts = 0; @@ -128,7 +129,7 @@ protected: SCOPED_TRACE("first i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskDescriptionMessage(genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task @@ -142,7 +143,7 @@ protected: SCOPED_TRACE("second i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskDescriptionMessage(genMsg("second")); // 1 notify for inserted task + model.processTaskDescriptionMessage(genMsg("second"), nh, "get_solution"); // 1 notify for inserted task if (i == 0) EXPECT_EQ(num_inserts, 1); @@ -164,7 +165,7 @@ protected: TEST_F(TaskListModelTest, remoteTaskModel) { children = 3; planning_scene::PlanningSceneConstPtr scene; - moveit_rviz_plugin::RemoteTaskModel m(scene, nullptr); + moveit_rviz_plugin::RemoteTaskModel m(nh, "get_solution", scene, nullptr); m.processStageDescriptions(genMsg("first").stages); SCOPED_TRACE("first"); validate(m, { "first" }); @@ -201,13 +202,13 @@ TEST_F(TaskListModelTest, threeChildren) { TEST_F(TaskListModelTest, visitedPopulate) { // first population without children children = 0; - model.processTaskDescriptionMessage(genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); validate(model, { "first" }); // validation visits root node EXPECT_EQ(num_inserts, 1); children = 3; num_inserts = 0; - model.processTaskDescriptionMessage(genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); validate(model, { "first" }); // second population with children should emit insert notifies for them EXPECT_EQ(num_inserts, 3); @@ -216,7 +217,7 @@ TEST_F(TaskListModelTest, visitedPopulate) { TEST_F(TaskListModelTest, deletion) { children = 3; - model.processTaskDescriptionMessage(genMsg("first")); + model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); auto m = model.getModel(model.index(0, 0)).first; int num_deletes = 0; QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; }); @@ -228,3 +229,9 @@ TEST_F(TaskListModelTest, deletion) { // EXPECT_EQ(num_deletes, 1); // TODO: event is not processed, missing event loop? EXPECT_EQ(model.rowCount(), 0); } + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "test_task_model"); + return RUN_ALL_TESTS(); +} diff --git a/visualization/motion_planning_tasks/test/test_task_model.launch b/visualization/motion_planning_tasks/test/test_task_model.launch index 932c9f83..3aebf298 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.launch +++ b/visualization/motion_planning_tasks/test/test_task_model.launch @@ -1,7 +1,4 @@ - - - From e83a5ecefaa21cc3e021dd72bd5b75a47646af56 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 23 Oct 2020 01:35:59 +0200 Subject: [PATCH 64/67] Configurable handling of old remote tasks - keep all previous tasks - replace with next task of same id - always remove --- .../src/task_list_model.cpp | 43 ++++++----- .../src/task_list_model.h | 7 +- .../motion_planning_tasks/src/task_panel.cpp | 73 +++++++++++++------ .../motion_planning_tasks/src/task_panel.h | 17 ++++- .../motion_planning_tasks/src/task_panel_p.h | 7 ++ 5 files changed, 104 insertions(+), 43 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 85ce5023..485318ef 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -38,6 +38,7 @@ #include "task_list_model.h" #include "local_task_model.h" #include "remote_task_model.h" +#include "task_panel.h" #include "factory_model.h" #include "icons.h" @@ -161,7 +162,8 @@ void TaskListModel::onRemoveModel(QAbstractItemModel* model) { it->second = nullptr; } -TaskListModel::TaskListModel(QObject* parent) : FlatMergeProxyModel(parent) { +TaskListModel::TaskListModel(QObject* parent) + : FlatMergeProxyModel(parent), old_task_handling_(TaskView::OLD_TASK_REPLACE) { ROS_DEBUG_NAMED(LOGNAME, "created TaskListModel: %p", this); setStageFactory(getStageFactory()); } @@ -201,6 +203,10 @@ Qt::ItemFlags TaskListModel::flags(const QModelIndex& index) const { return f; } +void TaskListModel::setOldTaskHandling(int mode) { + old_task_handling_ = mode; +} + void TaskListModel::highlightStage(size_t id) { if (!active_task_model_) return; @@ -239,39 +245,38 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_ ros::NodeHandle& nh, const std::string& service_name) { // retrieve existing or insert new remote task for given task id auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr)); - bool created = it_inserted.second; const auto& task_it = it_inserted.first; RemoteTaskModel*& remote_task = task_it->second; if (!msg.stages.empty() && remote_task && (remote_task->taskFlags() & BaseTaskModel::IS_DESTROYED)) { - removeModel(remote_task); - created = true; // re-create remote task after it was destroyed beforehand + // task overriding previous one that was already marked destroyed, but not yet removed from model + if (old_task_handling_ != TaskView::OLD_TASK_KEEP) + removeModel(remote_task); + remote_task = nullptr; // resetting to nullptr will trigger creation of a new task } - // empty list indicates, that this remote task is not available anymore + // empty list indicates, that this remote task was destroyed and we won't get updates for it if (msg.stages.empty()) { - if (!remote_task) { // task was already deleted locally - // we can now remove it from remote_tasks_ + // always remove destroyed RemoteTask? + if (old_task_handling_ == TaskView::OLD_TASK_REMOVE && remote_task) { + removeModel(remote_task); remote_tasks_.erase(task_it); return; } - } else if (created) { // create new task model, if ID was not known before + if (remote_task) // keep the task, but mark it as destroyed + remote_task->processStageDescriptions(msg.stages); + } else if (!remote_task) { // create new task model, if ID was not known before // the model is managed by this instance via Qt's parent-child mechanism remote_task = new RemoteTaskModel(nh, service_name, scene_, display_context_, this); + remote_task->processStageDescriptions(msg.stages); + ROS_DEBUG_NAMED(LOGNAME, "received new task: %s (%s)", msg.stages[0].name.c_str(), msg.task_id.c_str()); + // insert newly created model into this' model instance + insertModel(remote_task, -1); // HACK: always use the last created model as active active_task_model_ = remote_task; - } - if (!remote_task) - return; // task is not in use anymore - - remote_task->processStageDescriptions(msg.stages); - - // insert newly created model into this' model instance - if (created) { - ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.task_id.c_str()); - insertModel(remote_task, -1); - } + } else // normal update + remote_task->processStageDescriptions(msg.stages); } // process a task statistics message diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 52ea5e69..24f478a0 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -126,10 +126,12 @@ class TaskListModel : public utils::FlatMergeProxyModel // rviz::DisplayContext used to show (interactive) markers by the property models rviz::DisplayContext* display_context_ = nullptr; - // map from remote task IDs to tasks + // map from remote task IDs to (active) tasks // if task is destroyed remotely, it is marked with flag IS_DESTROYED // if task is removed locally from tasks vector, it is marked with a nullptr std::map remote_tasks_; + // mode reflecting the "Old task handling" setting + int old_task_handling_; // factory used to create stages StageFactoryPtr stage_factory_; @@ -172,6 +174,9 @@ public: Qt::DropActions supportedDropActions() const override; Qt::ItemFlags flags(const QModelIndex& index) const override; +public Q_SLOTS: + void setOldTaskHandling(int mode); + protected Q_SLOTS: void highlightStage(size_t id); }; diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 9428356d..c340c8bd 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -206,27 +206,8 @@ TaskViewPrivate::TaskViewPrivate(TaskView* q_ptr) : q_ptr(q_ptr), exec_action_cl if (factory) meta_model->setMimeTypes({ factory->mimeType() }); tasks_view->setModel(meta_model); - // auto-expand newly-inserted top-level items - QObject::connect(meta_model, &QAbstractItemModel::rowsInserted, - [this](const QModelIndex& parent, int first, int last) { - if (parent.isValid() && !parent.parent().isValid()) { // top-level task items inserted - int expand = this->q_ptr->initial_task_expand->getOptionInt(); - for (int row = first; row <= last; ++row) { - QModelIndex child = parent.model()->index(row, 0, parent); - if (expand != TaskView::EXPAND_NONE) { - // recursively expand all inserted items - setExpanded(tasks_view, child, true); - } - if (expand == TaskView::EXPAND_TOP) { - // collapse up to first level - setExpanded(tasks_view, child, false, 1); - // expand inserted item - setExpanded(tasks_view, child, true, 0); - } - } - tasks_view->setExpanded(parent, true); // expand parent group item - } - }); + QObject::connect(meta_model, SIGNAL(rowsInserted(QModelIndex, int, int)), q_ptr, + SLOT(_q_configureInsertedModels(QModelIndex, int, int))); tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection); tasks_view->setAcceptDrops(true); @@ -251,6 +232,40 @@ std::pair TaskViewPrivate::getTaskModel(const QMode return meta_model->getTaskModel(index); } +void TaskViewPrivate::configureTaskListModel(TaskListModel* model) { + QObject::connect(q_ptr, &TaskView::oldTaskHandlingChanged, model, &TaskListModel::setOldTaskHandling); + model->setOldTaskHandling(q_ptr->old_task_handling->getOptionInt()); +} + +void TaskViewPrivate::configureExistingModels() { + auto* meta_model = static_cast(tasks_view->model()); + for (int row = meta_model->rowCount() - 1; row >= 0; --row) + configureTaskListModel(meta_model->getTaskListModel(meta_model->index(row, 0)).first); +} + +void TaskViewPrivate::_q_configureInsertedModels(const QModelIndex& parent, int first, int last) { + if (parent.isValid() && !parent.parent().isValid()) { // top-level task items inserted + int expand = q_ptr->initial_task_expand->getOptionInt(); + for (int row = first; row <= last; ++row) { + // set expanded state of items + QModelIndex child = parent.model()->index(row, 0, parent); + if (expand != TaskView::EXPAND_NONE) { + // recursively expand all inserted items + setExpanded(tasks_view, child, true); + } + if (expand == TaskView::EXPAND_TOP) { + // collapse up to first level + setExpanded(tasks_view, child, false, 1); + // expand inserted item + setExpanded(tasks_view, child, true, 0); + } + + configureTaskListModel(getTaskListModel(child).first); + } + tasks_view->setExpanded(parent, true); // expand parent group item + } +} + void TaskViewPrivate::lock(TaskDisplay* display) { if (locked_display_ && locked_display_ != display) { locked_display_->clearMarkers(); @@ -292,8 +307,18 @@ TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root) initial_task_expand->addOption("All Expanded", EXPAND_ALL); initial_task_expand->addOption("All Closed", EXPAND_NONE); + old_task_handling = + new rviz::EnumProperty("Old task handling", "Keep", + "Configure what to do with old tasks whose solutions cannot be queried anymore", configs); + old_task_handling->addOption("Keep", OLD_TASK_KEEP); + old_task_handling->addOption("Replace", OLD_TASK_REPLACE); + old_task_handling->addOption("Remove", OLD_TASK_REMOVE); + connect(old_task_handling, &rviz::Property::changed, this, &TaskView::onOldTaskHandlingChanged); + show_time_column = new rviz::BoolProperty("Show Computation Times", true, "Show the 'time' column", configs); - connect(show_time_column, SIGNAL(changed()), this, SLOT(onShowTimeChanged())); + connect(show_time_column, &rviz::Property::changed, this, &TaskView::onShowTimeChanged); + + d_ptr->configureExistingModels(); } TaskView::~TaskView() { @@ -491,6 +516,10 @@ void TaskView::onShowTimeChanged() { d_ptr->actionShowTimeColumn->setChecked(show); } +void TaskView::onOldTaskHandlingChanged() { + Q_EMIT oldTaskHandlingChanged(old_task_handling->getOptionInt()); +} + GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz::Property* root) : q_ptr(q_ptr) { setupUi(q_ptr); diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index 6cf9a05a..e92373d9 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -124,11 +124,19 @@ protected: EXPAND_ALL, EXPAND_NONE }; - rviz::EnumProperty* initial_task_expand; + rviz::EnumProperty* initial_task_expand; + rviz::EnumProperty* old_task_handling; rviz::BoolProperty* show_time_column; public: + enum OldTaskHandling + { + OLD_TASK_KEEP = 1, + OLD_TASK_REPLACE, + OLD_TASK_REMOVE + }; + TaskView(TaskPanel* parent, rviz::Property* root); ~TaskView() override; @@ -145,6 +153,13 @@ protected Q_SLOTS: void onSolutionSelectionChanged(const QItemSelection& selected, const QItemSelection& deselected); void onExecCurrentSolution() const; void onShowTimeChanged(); + void onOldTaskHandlingChanged(); + +private: + Q_PRIVATE_SLOT(d_ptr, void _q_configureInsertedModels(QModelIndex, int, int)); + +Q_SIGNALS: + void oldTaskHandlingChanged(int); }; class GlobalSettingsWidgetPrivate; diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index 38c4d35e..d84616fc 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -78,6 +78,13 @@ public: /// retrieve TaskModel corresponding to given index inline std::pair getTaskModel(const QModelIndex& index) const; + /// configure a (new) TaskListModel + void configureTaskListModel(TaskListModel* model); + /// configure all TaskListModels that were already created when TaskView gets instantiated + void configureExistingModels(); + /// configure newly inserted models + void _q_configureInsertedModels(const QModelIndex& parent, int first, int last); + /// unlock locked_display_ if given display is different void lock(TaskDisplay* display); From bfa2a6070d578b852fbeab883ff34485aceab97c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 29 Oct 2020 18:04:48 +0100 Subject: [PATCH 65/67] MoveTo: Publish failed planning attempts --- core/src/stages/move_to.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index f9f915c3..ab7f052c 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -252,6 +252,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP } // store result + if (!robot_trajectory && storeFailures()) { + robot_trajectory = std::make_shared(robot_model, jmg); + robot_trajectory->addSuffixWayPoint(state.scene()->getCurrentState(), 0.0); + robot_trajectory->addSuffixWayPoint(scene->getCurrentState(), 1.0); + } if (robot_trajectory) { scene->setCurrentState(robot_trajectory->getLastWayPoint()); if (dir == BACKWARD) From f7ea72010b4d73b420ca6ac9ec167042e3fa2780 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 29 Oct 2020 19:12:19 +0100 Subject: [PATCH 66/67] Fix ambiguous member initialization --- .../motion_planning_tasks/src/task_panel.cpp | 18 +++++++++--------- .../utils/flat_merge_proxy_model.cpp | 2 +- .../utils/tree_merge_proxy_model.cpp | 2 +- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index c340c8bd..fe8193ff 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -149,9 +149,9 @@ void TaskPanel::decDisplayCount() { SINGLETON->deleteLater(); } -TaskPanelPrivate::TaskPanelPrivate(TaskPanel* q_ptr) : q_ptr(q_ptr) { - setupUi(q_ptr); - tool_buttons_group = new QButtonGroup(q_ptr); +TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) { + setupUi(panel); + tool_buttons_group = new QButtonGroup(panel); tool_buttons_group->setExclusive(true); button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages")); @@ -198,8 +198,8 @@ void setExpanded(QTreeView* view, const QModelIndex& index, bool expand, int dep view->setExpanded(index, expand); } -TaskViewPrivate::TaskViewPrivate(TaskView* q_ptr) : q_ptr(q_ptr), exec_action_client_("execute_task_solution") { - setupUi(q_ptr); +TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view), exec_action_client_("execute_task_solution") { + setupUi(view); MetaTaskListModel* meta_model = &MetaTaskListModel::instance(); StageFactoryPtr factory = getStageFactory(); @@ -520,10 +520,10 @@ void TaskView::onOldTaskHandlingChanged() { Q_EMIT oldTaskHandlingChanged(old_task_handling->getOptionInt()); } -GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz::Property* root) - : q_ptr(q_ptr) { - setupUi(q_ptr); - properties = new rviz::PropertyTreeModel(root, q_ptr); +GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* widget, rviz::Property* root) + : q_ptr(widget) { + setupUi(widget); + properties = new rviz::PropertyTreeModel(root, widget); view->setModel(properties); } diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp index 38d3ae1e..a8453d8a 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp @@ -107,7 +107,7 @@ public: std::vector data_; public: - FlatMergeProxyModelPrivate(FlatMergeProxyModel* q_ptr) : q_ptr(q_ptr) {} + FlatMergeProxyModelPrivate(FlatMergeProxyModel* model) : q_ptr(model) {} std::vector::iterator find(const QObject* model) { Q_ASSERT(model); diff --git a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp index b57c7a44..f57023ed 100644 --- a/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/tree_merge_proxy_model.cpp @@ -108,7 +108,7 @@ public: std::vector data_; public: - TreeMergeProxyModelPrivate(TreeMergeProxyModel* q_ptr) : q_ptr(q_ptr) {} + TreeMergeProxyModelPrivate(TreeMergeProxyModel* model) : q_ptr(model) {} std::vector::iterator find(const QObject* model) { Q_ASSERT(model); From ef9c7612a9402e6172f5d26abee081561480adfd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 29 Oct 2020 21:16:14 +0100 Subject: [PATCH 67/67] Fix (again) creation of TaskPanel --- .../src/task_display.cpp | 21 +++++++++++++----- .../motion_planning_tasks/src/task_display.h | 4 ++++ .../motion_planning_tasks/src/task_panel.cpp | 22 +++++++++++++++++-- .../motion_planning_tasks/src/task_panel.h | 4 ++-- 4 files changed, 41 insertions(+), 10 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 8bd4ac3d..1aa79463 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -59,7 +59,7 @@ namespace moveit_rviz_plugin { -TaskDisplay::TaskDisplay() : Display(), received_task_description_(false) { +TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_description_(false) { task_list_model_.reset(new TaskListModel); MetaTaskListModel::instance().insertModel(task_list_model_.get(), this); @@ -88,17 +88,24 @@ TaskDisplay::TaskDisplay() : Display(), received_task_description_(false) { } TaskDisplay::~TaskDisplay() { - if (context_) - TaskPanel::decDisplayCount(); + if (panel_requested_) + TaskPanel::release(); // Indicate that we don't need a TaskPanel anymore } void TaskDisplay::onInitialize() { Display::onInitialize(); trajectory_visual_->onInitialize(scene_node_, context_); task_list_model_->setDisplayContext(context_); - // create a new TaskPanel by default - // by post-poning this to Qt's GUI loop, we can ensure that rviz has loaded everything before - QTimer::singleShot(0, [this]() { TaskPanel::incDisplayCount(context_->getWindowManager()); }); +} + +inline void TaskDisplay::requestPanel() { + if (panel_requested_) + return; // already done + + // Create a new TaskPanel if not yet done. + // This cannot be done in initialize(), because Panel loading follows Display loading in rviz. + panel_requested_ = true; + TaskPanel::request(context_->getWindowManager()); } void TaskDisplay::loadRobotModel() { @@ -170,6 +177,7 @@ void TaskDisplay::calculateOffsetPosition() { } void TaskDisplay::update(float wall_dt, float ros_dt) { + requestPanel(); Display::update(wall_dt, ros_dt); trajectory_visual_->update(wall_dt, ros_dt); } @@ -188,6 +196,7 @@ void TaskDisplay::changedRobotDescription() { void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) { setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); + requestPanel(); task_list_model_->processTaskDescriptionMessage(*msg, update_nh_, base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id); diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index 21ee2ab2..e9e3f55e 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -100,6 +100,9 @@ protected: void fixedFrameChanged() override; void calculateOffsetPosition(); +private: + inline void requestPanel(); + private Q_SLOTS: /** * \brief Slot Event Functions @@ -123,6 +126,7 @@ protected: std::unique_ptr trajectory_visual_; // The TaskListModel storing actual task and solution data std::unique_ptr task_list_model_; + bool panel_requested_; // Load robot model rdf_loader::RDFLoaderPtr rdf_loader_; diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index fe8193ff..80042f9d 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -130,7 +130,25 @@ void TaskPanel::addSubPanel(SubPanel* w, const QString& title, const QIcon& icon connect(w, SIGNAL(configChanged()), this, SIGNAL(configChanged())); } -void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager) { +/* Realizing a singleton Panel is a nightmare with rviz... + * Formally, Panels (as a plugin class) cannot be singleton, because new instances are created on demand. + * Hence, we decided to use a true singleton for the underlying model only and a fake singleton for the panel. + * Thus, all panels (in case multiple were created) show the same content. + * The fake singleton shall ensure that only a single panel is created, even if several displays are created. + * To this end, the displays request() the need for a panel during their initialization and they release() + * this need during their destruction. This, in principle, allows to create a panel together with the first + * display and destroy it when the last display is gone. + * Obviously, the user can still decide to explicitly delete the panel (or create new ones). + + * The nightmare arises from the order of loading of displays and panels: Displays are loaded first. + * However, directly creating a panel with the first loaded display doesn't work, because panel loading + * will create another panel instance later (because there is no singleton support). + * Hence, we need to postpone the actual panel creation from displays until panel loading is finished as well. + * This was initially done, by postponing panel creation to TaskDisplay::update(). However, update() + * will never be called if the display is disabled... + */ + +void TaskPanel::request(rviz::WindowManagerInterface* window_manager) { ++DISPLAY_COUNT; rviz::VisualizationFrame* vis_frame = dynamic_cast(window_manager); @@ -143,7 +161,7 @@ void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager) { assert(dock->widget() == SINGLETON); } -void TaskPanel::decDisplayCount() { +void TaskPanel::release() { Q_ASSERT(DISPLAY_COUNT > 0); if (--DISPLAY_COUNT == 0 && SINGLETON) SINGLETON->deleteLater(); diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index e92373d9..9ada0c1d 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -91,8 +91,8 @@ public: * If not yet done, an instance is created. If use count drops to zero, * the global instance is destroyed. */ - static void incDisplayCount(rviz::WindowManagerInterface* window_manager); - static void decDisplayCount(); + static void request(rviz::WindowManagerInterface* window_manager); + static void release(); void onInitialize() override; void load(const rviz::Config& config) override;