From 0dab499533dbb252d7ffc177494a376bc536cb21 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Jun 2018 13:53:00 +0200 Subject: [PATCH] Stage: generally allow forwarding of interface properties --- core/include/moveit/task_constructor/stage.h | 7 +++++++ .../moveit/task_constructor/stages/compute_ik.h | 4 ---- core/src/stage.cpp | 15 +++++++++++++++ core/src/stages/compute_ik.cpp | 8 +------- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index ef56e267..7cf0d380 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -159,6 +159,13 @@ public: void setTimeout(double timeout) { setProperty("timeout", timeout); } double timeout() const { return properties().get("timeout"); } + /// forwarding of properties between interface states + void forwardProperties(const InterfaceState& source, InterfaceState& dest); + std::set forwardedProperties() const + { return properties().get>("forwarded_properties"); } + void setForwardedProperties(const std::set& names) + { setProperty("forwarded_properties", names); } + typedef std::function SolutionCallback; typedef std::list SolutionCallbackList; /// add function to be called for every newly found solution or failure diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index d982b55e..39cdf09a 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -104,10 +104,6 @@ public: void setIgnoreCollisions(bool flag) { setProperty("ignore_collisions", flag); } - - void setForwardedProperties(const std::set& names) { - setProperty("forward_properties", names); - } }; } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 58e08960..cfb2da0e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -105,6 +105,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, assert(nextStarts()); if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(from, to); auto to_it = states_.insert(states_.end(), std::move(to)); @@ -122,6 +123,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, assert(prevEnds()); if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(to, from); auto from_it = states_.insert(states_.end(), std::move(from)); @@ -181,6 +183,8 @@ Stage::Stage(StagePrivate *impl) assert(impl); auto& p = properties(); p.declare("timeout", "timeout per run (s)"); + p.declare>("forwarded_properties", std::set(), + "set of interface properties to forward"); p.declare("marker_ns", "marker namespace"); } @@ -246,6 +250,17 @@ void Stage::setName(const std::string& name) pimpl_->name_ = name; } +void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) +{ + const PropertyMap& src = source.properties(); + PropertyMap& dst = dest.properties(); + for (const auto& name : forwardedProperties()) { + if (!src.hasProperty(name)) + continue; + dst.set(name, src.get(name)); + } +} + Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb) { auto impl = pimpl(); diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 8f2a4828..85d93092 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -61,7 +61,6 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) p.declare("default_pose", "", "default joint pose of active group (defines cost of IK)"); p.declare("max_ik_solutions", 1); p.declare("ignore_collisions", false); - p.declare>("forward_properties", "to-be-forwarded properties from input"); // ik_frame and target_pose are read from the interface p.declare("ik_frame", "frame to be moved towards goal pose"); @@ -370,12 +369,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) robot_state.update(); InterfaceState state(scene); - const boost::any &forwards = props.get("forward_properties"); - if (!forwards.empty()) { - auto p = s.start()->properties(); - p.exposeTo(state.properties(), boost::any_cast>(forwards)); - } - + forwardProperties(*s.start(), state); spawn(std::move(state), std::move(solution)); }