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 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/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h new file mode 100644 index 00000000..d61dce15 --- /dev/null +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -0,0 +1,174 @@ +/********************************************************************* + * 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 +#include + +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: + 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 ()))> + LambdaCostTerm(const Term& t) : LambdaCostTerm{ Signature{ t } } {} + + LambdaCostTerm(const SubTrajectorySignature&); + LambdaCostTerm(const SubTrajectoryShortSignature&); + + double operator()(const SubTrajectory& s, std::string& comment) const override; + +protected: + SubTrajectorySignature term_; + +private: + template + static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSignature{}); + template + static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySignature{}); +}; + +namespace cost { + +/// add a constant cost to each solution +class Constant : public CostTerm +{ +public: + 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 TrajectoryCostTerm +{ + // 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 TrajectoryCostTerm +{ +public: + double operator()(const SubTrajectory&, std::string&) const override; +}; + +/** length of Cartesian trajection of a link */ +class LinkMotion : public TrajectoryCostTerm +{ +public: + LinkMotion(std::string link_name); + + std::string link_name; + + double operator()(const SubTrajectory&, std::string&) const override; +}; + +/** inverse distance to collision + * + * \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 + * */ +class Clearance : public TrajectoryCostTerm +{ +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", + Mode mode = Mode::AUTO); + bool with_world; + bool cumulative; + std::string group_property; + + Mode mode; + + std::function distance_to_cost; + + double operator()(const SubTrajectory& s, std::string& comment) const override; +}; + +} // 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 4b4fcd30..c6ac9b57 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -138,6 +138,8 @@ private: }; std::ostream& operator<<(std::ostream& os, const InitStageException& e); +MOVEIT_CLASS_FORWARD(CostTerm); +class LambdaCostTerm; class ContainerBase; class StagePrivate; class Stage @@ -159,6 +161,7 @@ public: PARENT = 2, INTERFACE = 4, }; + virtual ~Stage(); /// auto-convert Stage to StagePrivate* when needed @@ -212,6 +215,15 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); + /** 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; size_t numFailures() const; @@ -227,8 +239,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..3982f7c6 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 @@ -154,8 +155,13 @@ public: total_compute_time_ += compute_stop_time - compute_start_time; } + /** compute cost for solution through configured CostTerm */ + void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); + protected: - Stage* me_; // associated/owning Stage instance + // associated/owning Stage instance + Stage* me_; + std::string name_; PropertyMap properties_; @@ -163,6 +169,9 @@ protected: InterfacePtr starts_; InterfacePtr ends_; + // user-configurable cost estimator + CostTermConstPtr cost_term_; + // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/include/moveit/task_constructor/stages/passthrough.h b/core/include/moveit/task_constructor/stages/passthrough.h new file mode 100644 index 00000000..61f1c40d --- /dev/null +++ b/core/include/moveit/task_constructor/stages/passthrough.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 PassThrough : public WrapperBase +{ +public: + PassThrough(const std::string& name = "PassThrough", Stage::pointer&& child = Stage::pointer()); + + void onNewSolution(const SolutionBase& s) override; +}; +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 3bf0e08f..25aa8c51 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -192,10 +192,15 @@ private: using base_type::remove_if; }; +class CostTerm; 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; @@ -206,18 +211,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_; } @@ -239,6 +248,9 @@ public: Introspection* introspection = nullptr) const = 0; void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; + /// 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 bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; } @@ -246,6 +258,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_; @@ -276,6 +300,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_; @@ -301,6 +327,8 @@ public: /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + 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(); } @@ -331,6 +359,8 @@ public: void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const override; + double computeCost(const CostTerm& cost, std::string& comment) const override; + const SolutionBase* wrapped() const { return wrapped_; } private: diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 1ee9003a..051f12b4 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -78,13 +78,14 @@ 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; - std::string id() const; + 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 @@ -112,6 +113,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/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/python/test/test_mtc.py b/core/python/test/test_mtc.py index ed878ae2..5ca8abaa 100644 --- a/core/python/test/test_mtc.py +++ b/core/python/test/test_mtc.py @@ -272,12 +272,6 @@ class TestTask(unittest.TestCase): def test(self): task = core.Task() - self.assertEqual(task.id, "") - task = core.Task("foo", True, core.SerialContainer()) - self.assertEqual(task.id, "foo") - task = core.Task("task") - self.assertEqual(task.id, "task") - current = stages.CurrentState("current") self.assertEqual(current.name, "current") current.timeout = 1.23 diff --git a/core/python/wrapper/src/core.cpp b/core/python/wrapper/src/core.cpp index c15fa092..841ccaf7 100644 --- a/core/python/wrapper/src/core.cpp +++ b/core/python/wrapper/src/core.cpp @@ -89,8 +89,8 @@ void ContainerBase_insert(ContainerBase& self, std::auto_ptr stage, int b } BOOST_PYTHON_FUNCTION_OVERLOADS(ContainerBase_insert_overloads, ContainerBase_insert, 2, 3) -Task* Task_init(const std::string& id, bool introspection, std::auto_ptr container) { - return new Task(id, introspection, std::unique_ptr{ container.release() }); +Task* Task_init(const std::string& ns, bool introspection, std::auto_ptr container) { + return new Task(ns, introspection, std::unique_ptr{ container.release() }); } void Task_add(Task& self, std::auto_ptr stage) { @@ -209,7 +209,7 @@ void export_core() { PropertyMap& (Task::*Task_getPropertyMap)() = &Task::properties; bp::class_("Task", bp::no_init) - .add_property("id", &Task::id) + .add_property("name", bp::make_function(&Task::name, bp::return_internal_reference<>()), &Task::setName) // read-only access to properties + solutions, reference returned directly as pointer .add_property("properties", bp::make_function(Task_getPropertyMap, bp::return_internal_reference<>())) .add_property("solutions", bp::make_function(&Task::solutions, bp::return_internal_reference<>())) 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/container.cpp b/core/src/container.cpp index 2a089456..a8f96d50 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) @@ -200,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) ; @@ -752,8 +757,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: @@ -801,6 +806,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: @@ -817,8 +825,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; } @@ -857,18 +865,20 @@ 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))); } -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 } @@ -914,39 +924,46 @@ 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; 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 - 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()); + 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 diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp new file mode 100644 index 00000000..a1384c0a --- /dev/null +++ b/core/src/cost_terms.cpp @@ -0,0 +1,250 @@ +/********************************************************************* + * 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 + +#include +#include + +#include + +#include + +#include + +namespace moveit { +namespace task_constructor { + +double CostTerm::operator()(const SubTrajectory& s, std::string&) const { + return s.cost(); +} + +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 { + +double Constant::operator()(const SubTrajectory&, std::string&) const { + return cost; +} + +double Constant::operator()(const SolutionSequence&, std::string&) const { + return cost; +} + +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) + 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 TrajectoryDuration::operator()(const SubTrajectory& s, std::string&) const { + return s.trajectory() ? s.trajectory()->getDuration() : 0.0; +} + +LinkMotion::LinkMotion(std::string link) : link_name{ std::move(link) } {} + +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { + 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::Vector3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; + for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) { + const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; + distance += (new_position - position).norm(); + position = new_position; + } + return distance; +} + +Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode) + : 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::operator()(const SubTrajectory& s, std::string& comment) const { + const std::string PREFIX{ "Clearance: " }; + + collision_detection::DistanceRequest request; + request.type = + cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; + + 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?). + 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(); + + // 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) +#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; + } + + 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 (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); + return std::numeric_limits::infinity(); + } + 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); + } + } 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 distance_to_cost(distance); +} +} // namespace cost +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 8873c68a..e2225bd2 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -46,26 +46,29 @@ #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 class IntrospectionPrivate { public: - IntrospectionPrivate(const TaskPrivate* task) - : nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace + IntrospectionPrivate(const TaskPrivate* task, Introspection* self) + : 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 @@ -75,14 +78,17 @@ public: nh_.advertise(STATISTICS_TOPIC, 1, true); solution_publisher_ = nh_.advertise(SOLUTION_TOPIC, 1, true); + get_solution_service_ = + nh_.advertiseService(std::string(GET_SOLUTION_SERVICE "_") + task_id_, &Introspection::getSolution, self); + resetMaps(); } + ~IntrospectionPrivate() { indicateReset(); } void indicateReset() { // send empty task description message to indicate reset ::moveit_task_constructor_msgs::TaskDescription msg; - msg.process_id = process_id_; - msg.id = task_->id(); + msg.task_id = task_id_; task_description_publisher_.publish(msg); } @@ -97,7 +103,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_; @@ -112,9 +118,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; @@ -143,8 +147,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(); + msg.task_id = impl->task_id_; } void Introspection::publishSolution(const SolutionBase& s) { @@ -242,8 +245,7 @@ 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_; + msg.task_id = impl->task_id_; return msg; } @@ -263,8 +265,7 @@ 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_; + msg.task_id = impl->task_id_; return msg; } } // namespace task_constructor diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 38761006..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; @@ -125,7 +130,6 @@ void StagePrivate::validateConnectivity() const { } bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { - solution->setCreator(me()); if (introspection_) introspection_->registerSolution(*solution); @@ -142,14 +146,21 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) { assert(nextStarts()); + + solution->setCreator(me()); + + computeCost(from, to, *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()) nextStarts()->add(*to_it); @@ -159,14 +170,20 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution) { assert(prevEnds()); + + solution->setCreator(me()); + + computeCost(from, to, *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()) prevEnds()->add(*from_it); @@ -176,14 +193,19 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) { assert(prevEnds() && nextStarts()); + + solution->setCreator(me()); + + computeCost(state, state, *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()) { prevEnds()->add(*from); @@ -194,11 +216,15 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution } void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { + solution->setCreator(me()); + + computeCost(from, to, *solution); + if (!storeSolution(solution)) return; // solution dropped - solution->setStartState(from); - solution->setEndState(to); + solution->setStartStateUnsafe(from); + solution->setEndStateUnsafe(to); newSolution(solution); } @@ -212,6 +238,30 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } +void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { + // no reason to compute costs for a failed solution + if (solution.isFailure()) + 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); + + std::string 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 + ")"); + } else if (!comment.empty()) { + solution.setComment(comment); + } +} + Stage::Stage(StagePrivate* impl) : pimpl_(impl) { assert(impl); auto& p = properties(); @@ -254,8 +304,9 @@ void Stage::reset() { } void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { - // init properties once from parent auto impl = pimpl(); + + // init properties once from parent impl->properties_.reset(); if (impl->parent()) { try { @@ -308,6 +359,13 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } +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 { return pimpl()->solutions_; } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 2a041073..967ab1a0 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -9,6 +9,7 @@ add_library(${PROJECT_NAME}_stages ${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 @@ -28,6 +29,7 @@ add_library(${PROJECT_NAME}_stages generate_grasp_pose.cpp generate_place_pose.cpp compute_ik.cpp + passthrough.cpp predicate_filter.cpp connect.cpp diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0d07f52d..f2552618 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(std::make_unique()); + auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); p.declare("path_constraints", moveit_msgs::Constraints(), @@ -82,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."); @@ -181,15 +184,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 +206,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 +227,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..4546454e 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(std::make_unique(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..068f2759 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(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 b6a2aa5a..fddc9e4d 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(std::make_unique(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..8315bc95 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(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 f57650b8..c14605a6 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(std::make_unique(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..e5266825 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(std::make_unique()); + 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..ab7f052c 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(std::make_unique()); + auto& p = properties(); p.property("timeout").setDefaultValue(1.0); p.declare("group", "name of planning group"); @@ -248,19 +252,17 @@ 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) 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(); diff --git a/core/src/stages/passthrough.cpp b/core/src/stages/passthrough.cpp new file mode 100644 index 00000000..786221e1 --- /dev/null +++ b/core/src/stages/passthrough.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 { + +PassThrough::PassThrough(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} + +void PassThrough::onNewSolution(const SolutionBase& s) { + this->liftSolution(s); +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ed53b4ec..be1690ab 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; } @@ -168,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); } @@ -207,6 +221,10 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, } } +double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) const { + return f(*this, comment); +} + void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection) const { wrapped_->fillMessage(solution, introspection); @@ -217,5 +235,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 f(*this, comment); +} + } // namespace task_constructor } // namespace moveit diff --git a/core/src/task.cpp b/core/src/task.cpp index e0a99ab9..a3b00a38 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,10 +104,9 @@ 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 // addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout))); @@ -174,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! @@ -296,8 +296,10 @@ bool Task::plan(size_t max_solutions) { init(); impl->preempt_requested_ = false; - while (ros::ok() && !impl->preempt_requested_ && canCompute() && - (max_solutions == 0 || numSolutions() < max_solutions)) { + const double available_time = timeout(); + const auto start_time = std::chrono::steady_clock::now(); + 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_) cb(*this); @@ -355,10 +357,6 @@ void Task::setProperty(const std::string& name, const boost::any& value) { wrapped()->setProperty(name, value); } -std::string Task::id() const { - return pimpl()->id(); -} - const core::RobotModelConstPtr& Task::getRobotModel() const { auto impl = pimpl(); return impl->robot_model_; 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_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); +} diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp new file mode 100644 index 00000000..ee62f0c8 --- /dev/null +++ b/core/test/test_cost_terms.cpp @@ -0,0 +1,351 @@ +#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 }; + +const double TRAJECTORY_DURATION{ 9.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)); + Generator::init(robot_model); + } + + bool canCompute() const override { return true; } + + void compute() override { + InterfaceState state(ps); + spawn(std::move(state), STAGE_COST); + } +}; + +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); + } +}; + +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)); + }; +}; + +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)); + }; +}; + +template +class Standalone : public T +{ + moveit::core::RobotModelConstPtr robot; + InterfacePtr dummy; + planning_scene::PlanningSceneConstPtr ps; + InterfaceStatePtr state_start, state_end; + +public: + 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() { + 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))); + + // 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 }; + + 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) { + impl->starts()->add(*state_start); + } + if (flags & READS_END) { + impl->ends()->add(*state_end); + } + } + + void runCompute() { this->pimpl()->runCompute(); } + + 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(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 CostTermPtr& cost_term) { + this->setCostTerm(cost_term); + computeWithStages(stages); + } +}; + +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() }; + + Standalone container(robot); + + container.computeWithStages({ std::make_unique() }); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default"; + + container.computeWithStageCost({ std::make_unique() }, nullptr); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; + + 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"; +} + +TEST(CostTerm, StageTypes) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + + Standalone container(robot); + + 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"; + + 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) << "custom cost works for backward propagator"; +} + +TEST(CostTerm, PassThroughUsesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + Standalone container(robot); + + auto stage{ std::make_unique() }; + 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->cost) << "PassThrough forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "Child cost equals PassThrough cost"; +} + +TEST(CostTerm, PassThroughOverwritesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + Standalone container(robot); + + auto stage{ std::make_unique() }; + auto constant_inner{ std::make_shared(84.0) }; + stage->setCostTerm(constant_inner); + + 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_outer->cost) << "PassThrough can apply custom cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_inner->cost) << "child's cost is not affected"; +} + +TEST(CostTerm, PassThroughCanModifyCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + Standalone container(robot); + + auto stage{ std::make_unique() }; + 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"; +} + +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() }; + 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) + << "mix of explicit and implicit cost terms works"; + } + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + auto c1{ std::make_unique() }; + 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() }; + 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) + << "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(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"; +} 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; } diff --git a/msgs/msg/Solution.msg b/msgs/msg/Solution.msg index d4f330e2..ebdffff9 100644 --- a/msgs/msg/Solution.msg +++ b/msgs/msg/Solution.msg @@ -1,7 +1,4 @@ -# id of generating process -string process_id - -# id of associated task +# id of generating task string task_id # planning scene of start state diff --git a/msgs/msg/TaskDescription.msg b/msgs/msg/TaskDescription.msg index f9386744..551ff924 100644 --- a/msgs/msg/TaskDescription.msg +++ b/msgs/msg/TaskDescription.msg @@ -1,8 +1,5 @@ -# id of generating process -string process_id - -# unique ID of this task -string id +# unique id of this task +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 3fc62ce3..ec95569c 100644 --- a/msgs/msg/TaskStatistics.msg +++ b/msgs/msg/TaskStatistics.msg @@ -1,8 +1,5 @@ -# id of generating process -string process_id - -# unique of this task -string id +# unique id of generating task +string task_id # list of all stages, including the task stage itself StageStatistics[] stages 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; diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 565037c8..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; @@ -353,7 +351,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)); } } } @@ -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 149e3e9f..1aa79463 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -59,9 +59,8 @@ 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); - task_list_model_->setSolutionClient(&get_solution_client); MetaTaskListModel::instance().insertModel(task_list_model_.get(), this); @@ -89,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 main loop, we can ensure that rviz has loaded everything before - mainloop_jobs_.addJob([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() { @@ -171,8 +177,8 @@ void TaskDisplay::calculateOffsetPosition() { } void TaskDisplay::update(float wall_dt, float ros_dt) { + requestPanel(); Display::update(wall_dt, ros_dt); - mainloop_jobs_.executeJobs(); trajectory_visual_->update(wall_dt, ros_dt); } @@ -188,49 +194,32 @@ 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]() { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); - task_list_model_->processTaskDescriptionMessage(id, *msg); + setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); + requestPanel(); + 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 - 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(*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(*msg); + if (s) + trajectory_visual_->showTrajectory(s, false); + return; } void TaskDisplay::changedTaskSolutionTopic() { @@ -241,7 +230,6 @@ void TaskDisplay::changedTaskSolutionTopic() { task_description_sub.shutdown(); task_statistics_sub.shutdown(); task_solution_sub.shutdown(); - get_solution_client.shutdown(); received_task_description_ = false; @@ -256,11 +244,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); - - // service to request solutions - get_solution_client = - update_nh_.serviceClient(base_ns_ + GET_SOLUTION_SERVICE); + task_description_sub = update_nh_.subscribe(base_ns_ + DESCRIPTION_TOPIC, 10, &TaskDisplay::taskDescriptionCB, this); 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..e9e3f55e 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 @@ -101,6 +100,9 @@ protected: void fixedFrameChanged() override; void calculateOffsetPosition(); +private: + inline void requestPanel(); + private Q_SLOTS: /** * \brief Slot Event Functions @@ -119,15 +121,12 @@ 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_; // The trajectory playback component 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_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 7336af2b..485318ef 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -38,11 +38,11 @@ #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" #include -#include #include #include @@ -162,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()); } @@ -184,10 +185,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_) @@ -206,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; @@ -240,52 +241,49 @@ 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)); - bool created = it_inserted.second; +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)); 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(scene_, display_context_, this); - remote_task->setSolutionClient(get_solution_client_); + 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.id.c_str()); - insertModel(remote_task, -1); - } + } else // normal update + remote_task->processStageDescriptions(msg.stages); } // 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.task_id); if (it == remote_tasks_.cend()) { - ROS_WARN("unknown task: %s", id.c_str()); + ROS_WARN("unknown task: %s", msg.task_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.task_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..24f478a0 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; @@ -128,11 +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_; - ros::ServiceClient* get_solution_client_ = nullptr; + // mode reflecting the "Old task handling" setting + int old_task_handling_; // factory used to create stages StageFactoryPtr stage_factory_; @@ -148,7 +147,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,11 +155,12 @@ 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, ros::NodeHandle& nh, + const std::string& service_name); /// 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); @@ -175,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 08cf33bd..80042f9d 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -130,12 +130,30 @@ 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); 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", @@ -143,15 +161,15 @@ 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(); } -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,35 +216,16 @@ 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(); 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 +250,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 +325,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() { @@ -407,12 +450,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 +463,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) { @@ -489,10 +534,14 @@ void TaskView::onShowTimeChanged() { d_ptr->actionShowTimeColumn->setChecked(show); } -GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz::Property* root) - : q_ptr(q_ptr) { - setupUi(q_ptr); - properties = new rviz::PropertyTreeModel(root, q_ptr); +void TaskView::onOldTaskHandlingChanged() { + Q_EMIT oldTaskHandlingChanged(old_task_handling->getOptionInt()); +} + +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/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index 6cf9a05a..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; @@ -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); 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 461f0bba..f231700e 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -50,15 +50,17 @@ 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; 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& task_id = std::string()) { moveit_task_constructor_msgs::TaskDescription t; - t.id = name; uint id = 0, root_id; + t.task_id = task_id.empty() ? name : task_id; moveit_task_constructor_msgs::StageDescription desc; desc.parent_id = id; @@ -127,7 +129,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"), nh, "get_solution"); if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task @@ -141,7 +143,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"), nh, "get_solution"); // 1 notify for inserted task if (i == 0) EXPECT_EQ(num_inserts, 1); @@ -163,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" }); @@ -200,13 +202,13 @@ TEST_F(TaskListModelTest, threeChildren) { TEST_F(TaskListModelTest, visitedPopulate) { // first population without children children = 0; - model.processTaskDescriptionMessage("1", 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("1", 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); @@ -215,7 +217,7 @@ TEST_F(TaskListModelTest, visitedPopulate) { TEST_F(TaskListModelTest, deletion) { children = 3; - model.processTaskDescriptionMessage("1", 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; }); @@ -227,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 @@ - - - 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); 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;