diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 22178bc3..f884e026 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -108,7 +108,7 @@ protected: /// until the end, i.e. until there are no more subsolutions in the given direction /// For each solution path, callback the given processor passing /// the full trace (from start to end, but not including start) and its accumulated costs - template + template void traverse(const SolutionBase &start, const SolutionProcessor &cb, solution_container &trace, double trace_cost = 0); @@ -135,102 +135,101 @@ public: void reset() override; void init(const planning_scene::PlanningSceneConstPtr &scene) override; + size_t numSolutions() const override; + void processSolutions(const SolutionProcessor &processor) const override; + size_t numFailures() const override; + void processFailures(const SolutionProcessor &processor) const override; + protected: ParallelContainerBase(ParallelContainerBasePrivate* impl); + /// method called if any child found a new (internal) solution virtual void onNewSolution(const SolutionBase& s) override; - /// callback for new start states (received externally) - virtual void onNewStartState(Interface::iterator external, bool updated) = 0; - /// callback for new end states (received externally) - virtual void onNewEndState(Interface::iterator external, bool updated) = 0; + /// lift unmodified child solution (useful for simple filtering) + void liftSolution(const SolutionBase* solution) { + liftSolution(solution, solution->cost()); + } + /// lift child solution, but allow for modifying costs + void liftSolution(const SolutionBase* solution, double cost); + + /// spawn a new solution with given state as start and end + void spawn(InterfaceState &&state, SubTrajectory&& trajectory); + /// convience method spawning an empty SubTrajectory with given cost + void spawn(InterfaceState &&state, double cost) { + SubTrajectory trajectory; + trajectory.setCost(cost); + spawn(std::move(state), std::move(trajectory)); + } }; -/** Wrap an existing solution - for use in containers. +/** Plan for different alternatives in parallel. * - * This essentially wraps a solution of a child and thus allows - * for new new clones of start / end states, which in turn will - * have separate incoming/outgoing trajectories */ -class WrappedSolution : public SolutionBase { + * Solution of all children are reported - sorted by cost. + */ +class Alternatives : public ParallelContainerBase +{ public: - explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped) - : SolutionBase(creator, wrapped->cost()), wrapped_(wrapped) - {} - void fillMessage(moveit_task_constructor_msgs::Solution &solution, - Introspection* introspection = nullptr) const override { - wrapped_->fillMessage(solution, introspection); - } + Alternatives(const std::string &name) : ParallelContainerBase(name) {} -private: - const SolutionBase* wrapped_; + bool canCompute() const override; + bool compute() override; +}; + + +/** Plan for different alternatives in sequence. + * + * Try to find feasible solutions using first child. Only if this fails, + * proceed to the next child trying an alternative planning strategy. + * All solutions of the last active child are reported. + */ +class Fallbacks : public ParallelContainerBase +{ + mutable Stage* active_child_ = nullptr; + +public: + Fallbacks(const std::string &name) : ParallelContainerBase(name) {} + + void reset() override; + void init(const planning_scene::PlanningSceneConstPtr &scene) override; + bool canCompute() const override; + bool compute() override; }; class WrapperBasePrivate; -/** Base class for Wrapper and Task +/** A wrapper wraps a single child stage, which can be accessed via wrapped(). * - * WrapperBase ensures that only a single child is wrapped in a container. - * This child can be accessed via wrapped(). + * Implementations of this interface need to implement onNewSolution(), which is + * called when the child has generated a new solution. + * The wrapper may reject the solution or create one or multiple derived solutions, + * potentially adapting the cost, as well as its start and end states. + * + * Care needs to be taken to not modify pulled states, but only pushed ones! + * liftFor each new solution, liftSolution() should be called, which comes in various + * flavours to allow for handing in new states (or not). */ -class WrapperBase : public ContainerBase +class WrapperBase : public ParallelContainerBase { public: PRIVATE_CLASS(WrapperBase) WrapperBase(const std::string &name, pointer &&child = Stage::pointer()); - void reset() override; - void init(const planning_scene::PlanningSceneConstPtr &scene) override; - - size_t numSolutions() const override; - /// insertion is only allowed if children() is empty bool insert(Stage::pointer&& stage, int before = -1) override; - /// access the single wrapped child + /// access the single wrapped child, NULL if still empty Stage* wrapped(); inline const Stage* wrapped() const { return const_cast(this)->wrapped(); } -protected: - virtual void onNewSolution(const SolutionBase& s) = 0; - - WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer()); -}; - - -class WrapperPrivate; -/** A wrapper wraps a single generator-style stage (and acts itself as a generator). - * - * The wrapped stage must act as a generator, i.e. only spawn new states - * in its external interfaces. It's intended, e.g. to filter or clone - * a generated solution of its wrapped generator. - */ -class Wrapper : public WrapperBase -{ -public: - PRIVATE_CLASS(Wrapper) - Wrapper(const std::string &name, pointer &&child = Stage::pointer()); - - void reset() override; bool canCompute() const override; bool compute() override; - size_t numSolutions() const override; - size_t numFailures() const override; - void processSolutions(const SolutionProcessor &processor) const override; - void processFailures(const SolutionProcessor &processor) const override; - - void spawn(InterfaceState &&state, std::unique_ptr&& s); - void spawn(InterfaceState &&state, double cost) { - std::unique_ptr s(new SubTrajectory()); - s->setCost(cost); - spawn(std::move(state), std::move(s)); - } - protected: - Wrapper(WrapperPrivate* impl, pointer &&child = Stage::pointer()); + WrapperBase(WrapperBasePrivate* impl, pointer &&child = Stage::pointer()); /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase &s) override = 0; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 66fadb12..9fbd339a 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -39,7 +39,6 @@ #pragma once #include -#include "utils.h" #include "stage_p.h" #include @@ -47,13 +46,16 @@ namespace moveit { namespace task_constructor { -/* A container needs to decouple its interfaces from those of its children. - * Both, the container and the children have their own starts_ and ends_. - * The container needs to forward states received in its interfaces to - * the interfaces of the children. - * Solutions found by the children, then need to be connected to the - * container's interface states. To this end, we remember the mapping - * from internal to external states. +/* A container needs to decouple its own (external) interfaces + * from those (internal) of its children. + * Both, the container and the children have their own pull interfaces: starts_ and ends_. + * The container forwards states received in its pull interfaces to the + * corresponding interfaces of the children. + * States pushed by children are temporarily stored in pending_backward_ and pending_forward_. + * + * Solutions found by the children need to be lifted from the internal level to the + * external level. To this end, we remember the mapping from internal to external states. + * * Note, that there might be many solutions connecting a single start-end pair. * These solutions might origin from different children (ParallelContainer) * or from different solution paths in a SerialContainer. @@ -92,19 +94,40 @@ public: bool canCompute() const override; bool compute() override; + InterfacePtr pendingBackward() const { return pending_backward_; } + InterfacePtr pendingForward() const { return pending_forward_; } + protected: - ContainerBasePrivate(ContainerBase *me, const std::string &name) - : StagePrivate(me, name) - {} + ContainerBasePrivate(ContainerBase *me, const std::string &name); + + // Get push interface to be used for children: If our own push interface is not set, + // don't set children's interface either: pushing is not supported. + // Otherwise return pending_* buffer. + InterfacePtr getPushBackwardInterface() { + return prevEnds() ? pending_backward_ : InterfacePtr(); + } + InterfacePtr getPushForwardInterface() { + return nextStarts() ? pending_forward_ : InterfacePtr(); + } /// copy external_state to a child's interface and remember the link in internal_to map - void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated); + void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); + /// lift solution from internal to external level + void liftSolution(SolutionBase& solution, + const InterfaceState *internal_from, const InterfaceState *internal_to); protected: container_type children_; // map start/end states of children (internal) to corresponding states in our external interfaces std::map internal_to_external_; + + /* TODO: these interfaces don't need to be priority-sorted. + * Introduce base class UnsortedInterface (which is a plain list) for this use case. */ + // interface to receive children's sendBackward() states + InterfacePtr pending_backward_; + // interface to receive children's sendForward() states + InterfacePtr pending_forward_; }; PIMPL_FUNCTIONS(ContainerBase) @@ -134,69 +157,74 @@ private: * The solution of a single child stage is usually disconnected to the container's start or end. * Only if all the children in the chain have found a coherent solution from start to end, * this solution can be announced as a solution of the SerialContainer. - * - * Particularly, the first/last stage's sendBackward()/sendForward() call - * cannot directly propagate their associated state to the previous/next stage of this container, - * because we cannot provide a full solution yet. Hence, the first/last stage - * propagate to the pending_backward_/pending_forward_ interface first. - * If eventually a full solution is found, it is propagated to prevEnds()/nextStarts() - - * together with the solution. */ + */ class SerialContainerPrivate : public ContainerBasePrivate { friend class SerialContainer; public: SerialContainerPrivate(SerialContainer* me, const std::string &name); - void storeNewSolution(SerialSolution&& s); - const ordered& solutions() const { return solutions_; } - private: void connect(StagePrivate *prev, StagePrivate *next); - // interface to buffer first child's sendBackward() states - InterfacePtr pending_backward_; - // interface to buffer last child's sendForward() states - InterfacePtr pending_forward_; - // set of all solutions ordered solutions_; }; PIMPL_FUNCTIONS(SerialContainer) +/** Wrap an existing solution - for use in parallel containers and wrappers. + * + * This essentially wraps a solution of a child and thus allows + * for new clones of start / end states, which in turn will + * have separate incoming/outgoing trajectories */ +class WrappedSolution : public SolutionBase { +public: + explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost) + : SolutionBase(creator, cost), wrapped_(wrapped) + {} + explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped) + : WrappedSolution(creator, wrapped, wrapped->cost()) + {} + void fillMessage(moveit_task_constructor_msgs::Solution &solution, + Introspection* introspection = nullptr) const override; + +private: + const SolutionBase* wrapped_; +}; + + class ParallelContainerBasePrivate : public ContainerBasePrivate { friend class ParallelContainerBase; public: ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name); + +private: + /// callback for new externally received states + void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated); + + // buffer for wrapped solutions + std::list wrapped_solutions_; + // buffer for newly created (not wrapped) solutions + std::list created_solutions_; + // buffer of created states (for use in created solutions) + std::list states_; + + // cost-ordered set of solutions (pointers into wrapped or created) + ordered solutions_; + // buffer for failures (pointers into wrapped or created) + std::list failures_; }; PIMPL_FUNCTIONS(ParallelContainerBase) -class WrapperBasePrivate : public ContainerBasePrivate { +class WrapperBasePrivate : public ParallelContainerBasePrivate { friend class WrapperBase; public: WrapperBasePrivate(WrapperBase* me, const std::string& name); - -private: - InterfacePtr dummy_starts_; - InterfacePtr dummy_ends_; }; PIMPL_FUNCTIONS(WrapperBase) - -class WrapperPrivate : public WrapperBasePrivate { - friend class Wrapper; - -public: - WrapperPrivate(Wrapper* me, const std::string& name); - -private: - ordered, pointerLessThan>> solutions_; - std::list> failures_; - std::list failure_states_; -}; -PIMPL_FUNCTIONS(Wrapper) - } } diff --git a/core/include/moveit/task_constructor/solvers/cartesian_path.h b/core/include/moveit/task_constructor/solvers/cartesian_path.h new file mode 100644 index 00000000..18ffced5 --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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 Goerner, Robert Haschke + Desc: generate and validate a straight-line Cartesian path +*/ + +#pragma once + +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +/** Use RobotState's computeCartesianPath() to generate a straigh-line path between to scenes */ +class CartesianPath : public PlannerInterface { +public: + CartesianPath(); + + void setGroup(const std::string &group) { setProperty("group", group); } + void setTimeout(double timeout) { setProperty("timeout", timeout); } + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) override; +}; + +} } } diff --git a/core/include/moveit/task_constructor/solvers/pipeline_planner.h b/core/include/moveit/task_constructor/solvers/pipeline_planner.h new file mode 100644 index 00000000..804c800d --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/pipeline_planner.h @@ -0,0 +1,78 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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: Robert Haschke + Desc: plan using MoveIt's PlanningPipeline +*/ + +#pragma once + +#include +#include + +namespace planning_pipeline { +MOVEIT_CLASS_FORWARD(PlanningPipeline) +} + +namespace moveit { namespace task_constructor { namespace solvers { + +/** Use MoveIt's PlanningPipeline to plan a trajectory between to scenes */ +class PipelinePlanner : public PlannerInterface { +public: + PipelinePlanner(); + + void setGroup(const std::string &group) { setProperty("group", group); } + void setPlannerId(const std::string &planner) { setProperty("planner", planner); } + void setTimeout(double timeout) { setProperty("timeout", timeout); } + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target, + const core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) override; + +protected: + planning_pipeline::PlanningPipelinePtr planner_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/solvers/planner_interface.h b/core/include/moveit/task_constructor/solvers/planner_interface.h new file mode 100644 index 00000000..197f5ae0 --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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: Robert Haschke + Desc: planner interface +*/ + +#pragma once + +#include +#include +#include + +namespace planning_scene { +MOVEIT_CLASS_FORWARD(PlanningScene) +} +namespace robot_trajectory { +MOVEIT_CLASS_FORWARD(RobotTrajectory) +} +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(LinkModel) +MOVEIT_CLASS_FORWARD(RobotModel) +MOVEIT_CLASS_FORWARD(JointModelGroup) +} } + +namespace moveit { namespace task_constructor { namespace solvers { + +MOVEIT_CLASS_FORWARD(PlannerInterface) +class PlannerInterface { + // these properties take precedence over stage properties + PropertyMap properties_; + +public: + PlannerInterface() {} + + PropertyMap& properties() { return properties_; } + const PropertyMap& properties() const { return properties_; } + + void setProperty(const std::string& name, const boost::any& value) { + properties_.set(name, value); + } + + virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; + + /// plan trajectory between to robot states + virtual bool plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) = 0; + + /// plan trajectory from current robot state to Cartesian target + virtual bool plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) = 0; +}; + +std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg); + +} } } diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 7791d041..a9d2ab80 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -80,6 +80,12 @@ public: inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); } inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); } + /// templated access to pull/push interfaces + inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; } + inline InterfacePtr pushInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); } + inline InterfaceConstPtr pullInterface(Interface::Direction dir) const { return dir == Interface::FORWARD ? starts_ : ends_; } + inline InterfaceConstPtr pushInterface(Interface::Direction dir) const { return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); } + /// the following methods should be called only by a container /// to setup the connection structure of their children inline void setHierarchy(ContainerBase* parent, container_type::iterator it) { diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index 4a480a78..f2448c3e 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -15,7 +15,7 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) } namespace moveit { namespace task_constructor { namespace stages { -class ComputeIK : public Wrapper { +class ComputeIK : public WrapperBase { public: ComputeIK(const std::string &name, pointer &&child = Stage::pointer()); diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h new file mode 100644 index 00000000..1d7ee2b4 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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 Goerner, Robert Haschke + Desc: Connect arbitrary states by motion planning +*/ + +#pragma once + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +class Connect : public Connecting { +public: + Connect(std::string name, const solvers::PlannerInterfacePtr &planner); + + void init(const planning_scene::PlanningSceneConstPtr &scene); + bool compute(const InterfaceState &from, const InterfaceState &to); + +protected: + solvers::PlannerInterfacePtr planner_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/stages/fix_collision_objects.h b/core/include/moveit/task_constructor/stages/fix_collision_objects.h new file mode 100644 index 00000000..66b63edb --- /dev/null +++ b/core/include/moveit/task_constructor/stages/fix_collision_objects.h @@ -0,0 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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: Elham Iravani, Robert Haschke + Desc: Fix collisions in input scene +*/ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +class FixCollisionObjects : public PropagatingEitherWay { +public: + FixCollisionObjects(const std::string& name = "fix collisions of objects"); + + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + void setMaxPenetration(double penetration); + +private: + bool fixCollisions(planning_scene::PlanningScene &scene, std::deque& markers) const; + void fixCollision(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object) const; +}; + +} } } diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h new file mode 100644 index 00000000..a30e1c7c --- /dev/null +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -0,0 +1,140 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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: Robert Haschke + Desc: Modify planning scene +*/ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { namespace core { +MOVEIT_CLASS_FORWARD(JointModelGroup) +} } + +namespace moveit { namespace task_constructor { namespace stages { + +/** Allow modification of planning scene + * + * This stage takes the incoming planning scene and applies previously scheduled + * changes to it, for example: + * - Modify allowed collision matrix, enabling or disabling collision pairs. + * - Attach or detach objects to robot links. + * - Spawn or remove objects. + */ +class ModifyPlanningScene : public PropagatingEitherWay { +public: + typedef std::vector Names; + typedef std::function ApplyCallback; + ModifyPlanningScene(const std::string& name = "modify planning scene"); + + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + /// call an arbitrary function + void setCallback(const ApplyCallback& cb) { callback_ = cb; } + + /// attach or detach a list of objects to the given link + void attachObjects(const Names& objects, const std::string& attach_link, bool attach); + + /// conviency methods accepting a single object name + inline void attachObject(const std::string& object, const std::string& link); + inline void detachObject(const std::string& object, const std::string& link); + + /// conviency methods accepting any container of object names + template ::value && std::is_base_of::value>> + inline void attachObjects(const T& objects, const std::string& link) { + attachObjects(Names(objects.cbegin(), objects.cend()), link, true); + } + template ::value && std::is_base_of::value>> + inline void detachObjects(const T& objects, const std::string& link) { + attachObjects(Names(objects.cbegin(), objects.cend()), link, false); + } + + /// enable / disable collisions for each combination of pairs in first and second lists + void enableCollisions(const Names& first, const Names& second, bool enable_collision); + /// enable / disable all collisions for given object + void enableCollisions(const std::string& object, bool enable_collision) { + enableCollisions(Names({object}), Names(), enable_collision); + } + + /// conveniency method accepting arbitrary container types + template ::value && std::is_base_of::value>, + typename E2 = typename std::enable_if_t::value && std::is_base_of::value>> + inline void enableCollisions(const T1& first, const T2& second, bool enable_collision) { + enableCollisions(Names(first.cbegin(), first.cend()), Names(second.cbegin(), second.cend()), enable_collision); + } + /// conveniency method accepting std::string and an arbitrary container of names + template ::value && std::is_base_of::value>> + inline void enableCollisions(const std::string& first, const T& second, bool enable_collision) { + enableCollisions(Names({first}), Names(second.cbegin(), second.cend()), enable_collision); + } + /// conveniency method accepting std::string and JointModelGroup + void enableCollisions(const std::string& first, const moveit::core::JointModelGroup& jmg, bool enable_collision); + +protected: + // list of objects to attach (true) / detach (false) to a given link + std::map > attach_objects_; + + // list of objects to mutually + struct CollisionMatrixPairs { + Names first; + Names second; + bool enable; + }; + std::list collision_matrix_edits_; + ApplyCallback callback_; + +protected: + // apply stored modifications to scene + InterfaceState apply(const InterfaceState &from, bool invert); + void attachObjects(planning_scene::PlanningScene &scene, const std::pair >& pair, bool invert); + void enableCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert); +}; + + +inline void ModifyPlanningScene::attachObject(const std::string &object, const std::string &link) { + attachObjects(Names({object}), link, true); +} + +inline void ModifyPlanningScene::detachObject(const std::string &object, const std::string &link) { + attachObjects(Names({object}), link, false); +} + +} } } diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h new file mode 100644 index 00000000..308cba43 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -0,0 +1,78 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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 Goerner, Robert Haschke + Desc: Move to joint-state or Cartesian goal pose +*/ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +/** Perform a Cartesian motion relative to some link */ +class MoveRelative : public PropagatingEitherWay { +public: + MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner); + + void init(const planning_scene::PlanningSceneConstPtr &scene) override; + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + void setGroup(const std::string& group); + void setLink(const std::string& link); + + /// set minimum / maximum distance to move + void setMinDistance(double distance); + void setMaxDistance(double distance); + void setMinMaxDistance(double min_distance, double max_distance); + + /// perform twist motion on specified link + void along(const geometry_msgs::TwistStamped& twist); + /// translate link along given direction + void along(const geometry_msgs::Vector3Stamped& direction); + +protected: + bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, + SubTrajectory &trajectory, Direction dir); + +protected: + solvers::PlannerInterfacePtr planner_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h new file mode 100644 index 00000000..08743580 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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 Goerner, Robert Haschke + Desc: Move to joint-state or Cartesian goal pose +*/ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +class MoveTo : public PropagatingEitherWay { +public: + MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner); + + void init(const planning_scene::PlanningSceneConstPtr &scene) override; + bool computeForward(const InterfaceState& from) override; + bool computeBackward(const InterfaceState& to) override; + + void setGroup(const std::string& group); + void setLink(const std::string& link); + + /// move link to given pose + void setGoal(const geometry_msgs::PoseStamped& pose); + /// move link to given point, keeping current orientation + void setGoal(const geometry_msgs::PointStamped& point); + /// move joint model group to given named pose + void setGoal(const std::string& joint_pose); + +protected: + bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, + SubTrajectory &trajectory, Direction dir); + +protected: + solvers::PlannerInterfacePtr planner_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 9a3a47a3..0386ff3f 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -142,6 +142,7 @@ private: /** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */ class Interface : public ordered { public: + enum Direction { FORWARD, BACKWARD, START=FORWARD, END=BACKWARD }; typedef std::function NotifyFunction; Interface(const NotifyFunction ¬ify = NotifyFunction()); @@ -170,6 +171,9 @@ public: inline const InterfaceState* start() const { return start_; } inline const InterfaceState* end() const { return end_; } + template + inline const InterfaceState::Solutions& trajectories() const; + inline void setStartState(const InterfaceState& state){ assert(start_ == NULL); // only allow setting once (by Stage) start_ = &state; @@ -234,27 +238,34 @@ public: {} robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } + void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; } void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection* introspection = nullptr) const override; private: // actual trajectory, might be empty - const robot_trajectory::RobotTrajectoryPtr trajectory_; + robot_trajectory::RobotTrajectoryPtr trajectory_; }; -enum TraverseDirection { FORWARD, BACKWARD }; -template -const InterfaceState::Solutions& trajectories(const SolutionBase &start); - template <> inline -const InterfaceState::Solutions& trajectories(const SolutionBase &solution) { - return solution.end()->outgoingTrajectories(); +const InterfaceState::Solutions& SolutionBase::trajectories() const { + return end_->outgoingTrajectories(); } template <> inline -const InterfaceState::Solutions& trajectories(const SolutionBase &solution) { - return solution.start()->incomingTrajectories(); +const InterfaceState::Solutions& SolutionBase::trajectories() const { + return start_->incomingTrajectories(); } } } + +namespace std { +// comparison for pointers to SolutionBase: compare based on value +template<> struct less { + bool operator()(const moveit::task_constructor::SolutionBase* x, + const moveit::task_constructor::SolutionBase* y) { + return *x < *y; + } +}; +} diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 931ea828..0be076b9 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -62,9 +62,9 @@ MOVEIT_CLASS_FORWARD(Task) /** A Task is the root of a tree of stages. * - * Actually a tasks wraps a single container, which serves as the root of all stages. - * The wrapped container spawns its solutions into the prevEnds(), nextStarts() interfaces, - * which are provided by the wrappers end_ and start_ and interfaces respectively. */ + * Actually a tasks wraps a single container (by default a SerialContainer), + * which serves as the root of all stages. + */ class Task : protected WrapperBase { public: Task(const std::string& id = "", @@ -76,6 +76,7 @@ public: ~Task(); std::string id() const; + const moveit::core::RobotModelPtr getRobotModel() const; void add(Stage::pointer &&stage); void clear() override; diff --git a/core/include/moveit/task_constructor/type_traits.h b/core/include/moveit/task_constructor/type_traits.h new file mode 100644 index 00000000..2404f7a5 --- /dev/null +++ b/core/include/moveit/task_constructor/type_traits.h @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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: Robert Haschke + Desc: type traits for SFINAE-based templating +*/ + +#pragma once + +#include + +namespace moveit { namespace task_constructor { + +// detect STL-like containers by presence of cbegin(), cend() methods +template +struct is_container : std::false_type {}; + +template +struct is_container_helper {}; + +template +struct is_container< + T, + std::conditional_t< + false, + is_container_helper< + typename T::const_iterator, + decltype(std::declval().cbegin()), + decltype(std::declval().cend()) + >, + void + > + > : public std::true_type {}; + +} } diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index 657f4e5c..667da900 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -94,12 +94,3 @@ private: }; #define DECLARE_FLAGS(Flags, Enum) typedef QFlags Flags; - - -// compare two pointers by comparing their referenced values -template -struct pointerLessThan : std::enable_if::value> { - inline bool operator()(const T& x, const T& y) const { - return *x < *y; - } -}; diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 0a87c55f..b5d02dc8 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -11,6 +11,10 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/task.h ${PROJECT_INCLUDE}/utils.h + ${PROJECT_INCLUDE}/solvers/planner_interface.h + ${PROJECT_INCLUDE}/solvers/cartesian_path.h + ${PROJECT_INCLUDE}/solvers/pipeline_planner.h + container.cpp introspection.cpp marker_tools.cpp @@ -18,6 +22,10 @@ add_library(${PROJECT_NAME} stage.cpp storage.cpp task.cpp + + solvers/planner_interface.cpp + solvers/cartesian_path.cpp + solvers/pipeline_planner.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} diff --git a/core/src/container.cpp b/core/src/container.cpp index 0fd19e25..19373791 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -49,6 +49,13 @@ using namespace std::placeholders; namespace moveit { namespace task_constructor { +ContainerBasePrivate::ContainerBasePrivate(ContainerBase *me, const std::string &name) + : StagePrivate(me, name) +{ + pending_backward_.reset(new Interface); + pending_forward_.reset(new Interface); +} + ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) const { const_iterator position = children_.begin(); if (index > 0) { @@ -90,17 +97,42 @@ bool ContainerBasePrivate::compute() return static_cast(me_)->compute(); } -void ContainerBasePrivate::copyState(Interface::iterator external, - Stage &child, bool to_start, bool updated) { - if (to_start) { - InterfaceState& internal = *child.pimpl()->starts()->clone(*external); - internal_to_external_.insert(std::make_pair(&internal, external)); +void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) { + // TODO need to update existing mapping? + + // create a clone of external state within target interface (child's starts() or ends()) + InterfaceState& internal = *target->clone(*external); + // and remember the mapping between them + internal_to_external_.insert(std::make_pair(&internal, external)); +} + +void ContainerBasePrivate::liftSolution(SolutionBase& solution, + const InterfaceState *internal_from, const InterfaceState *internal_to) +{ + // add solution to existing or new start state + auto it = internal_to_external_.find(internal_from); + if (it != internal_to_external_.end()) { + // connect solution to existing start state + solution.setStartState(*it->second); } else { - InterfaceState& internal = *child.pimpl()->ends()->clone(*external); - internal_to_external_.insert(std::make_pair(&internal, external)); + // spawn a new state in previous stage + Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); + internal_to_external_.insert(std::make_pair(internal_from, external)); + } + + // add solution to existing or new end state + it = internal_to_external_.find(internal_to); + if (it != internal_to_external_.end()) { + // connect solution to existing start state + solution.setEndState(*it->second); + } else { + // spawn a new state in next stage + Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); + internal_to_external_.insert(std::make_pair(internal_to, external)); } } + ContainerBase::ContainerBase(ContainerBasePrivate *impl) : Stage(impl) { @@ -139,6 +171,7 @@ bool ContainerBase::insert(Stage::pointer &&stage, int before) bool ContainerBase::remove(int pos) { ContainerBasePrivate::const_iterator it = pimpl()->position(pos); + (*it)->pimpl()->setHierarchy(nullptr, ContainerBasePrivate::iterator()); pimpl()->children_.erase(it); return true; } @@ -156,7 +189,10 @@ void ContainerBase::reset() for (auto& child: impl->children()) child->reset(); - // clear mapping + // clear buffer interfaces + impl->pending_backward_->clear(); + impl->pending_forward_->clear(); + // ... and state mapping impl->internal_to_external_.clear(); Stage::reset(); @@ -190,15 +226,6 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) } -SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) - : ContainerBasePrivate(me, name) -{ - // these lists don't need a notify function, connections are handled by onNewSolution() - pending_backward_.reset(new Interface(Interface::NotifyFunction())); - pending_forward_.reset(new Interface(Interface::NotifyFunction())); -} - - struct SolutionCollector { SolutionCollector(size_t max_depth) : max_depth(max_depth) {} @@ -232,11 +259,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) // find all incoming solution pathes ending at current solution SolutionCollector incoming(num_before); - traverse(current, std::ref(incoming), trace); + traverse(current, std::ref(incoming), trace); // find all outgoing solution pathes starting at current solution SolutionCollector outgoing(num_after); - traverse(current, std::ref(outgoing), trace); + traverse(current, std::ref(outgoing), trace); // collect (and sort) all solutions spanning from start to end of this container ordered sorted; @@ -268,42 +295,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t) } // store new solutions (in sorted) - for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) - impl->storeNewSolution(std::move(*it)); -} - -void SerialContainerPrivate::storeNewSolution(SerialSolution &&s) -{ - const InterfaceState *internal_from = s.internalStart(); - const InterfaceState *internal_to = s.internalEnd(); - - // create new SerialSolution and get a reference to it - SerialSolution& solution = *solutions_.insert(std::move(s)); - - // add solution to existing or new start state - auto it = internal_to_external_.find(internal_from); - if (it != internal_to_external_.end()) { - // connect solution to existing start state - solution.setStartState(*it->second); - } else { - // spawn a new state in previous stage - Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution); - internal_to_external_.insert(std::make_pair(internal_from, external)); + for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) { + auto inserted = impl->solutions_.insert(std::move(*it)); + impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd()); + impl->newSolution(*inserted); } - - // add solution to existing or new end state - it = internal_to_external_.find(internal_to); - if (it != internal_to_external_.end()) { - // connect solution to existing start state - solution.setEndState(*it->second); - } else { - // spawn a new state in next stage - Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL); - internal_to_external_.insert(std::make_pair(internal_to, external)); - } - - // perform default stage action on new solution - newSolution(solution); } @@ -320,13 +316,15 @@ void SerialContainer::reset() // clear queues impl->solutions_.clear(); - impl->pending_backward_->clear(); - impl->pending_forward_->clear(); // recursively reset children ContainerBase::reset(); } +SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name) + : ContainerBasePrivate(me, name) +{} + void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) { prev->setNextStarts(next->starts()); next->setPrevEnds(prev->ends()); @@ -342,11 +340,11 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) /*** connect children ***/ // first stage sends backward to pending_backward_ auto start = impl->children().begin(); - (*start)->pimpl()->setPrevEnds(impl->pending_backward_); + (*start)->pimpl()->setPrevEnds(impl->getPushBackwardInterface()); // last stage sends forward to pending_forward_ auto last = --impl->children().end(); - (*last)->pimpl()->setNextStarts(impl->pending_forward_); + (*last)->pimpl()->setNextStarts(impl->getPushForwardInterface()); auto cur = start; auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage @@ -365,19 +363,10 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene) ContainerBase::init(scene); // initialize starts_ and ends_ interfaces - Stage* child = start->get(); - if (child->pimpl()->starts()) - impl->starts_.reset(new Interface([impl, child](Interface::iterator external, bool updated){ - // new external state in our starts_ interface is copied to first child - impl->copyState(external, *child, true, updated); - })); - - child = last->get(); - if (child->pimpl()->ends()) - impl->ends_.reset(new Interface([impl, child](Interface::iterator external, bool updated){ - // new external state in our ends_ interface is copied to last child - impl->copyState(external, *child, false, updated); - })); + if (const InterfacePtr& target = (*start)->pimpl()->starts()) + impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); + if (const InterfacePtr& target = (*last)->pimpl()->ends()) + impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2))); // validate connectivity of this if (!impl->nextStarts()) @@ -421,16 +410,16 @@ size_t SerialContainer::numSolutions() const void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const { - for(const SolutionBase& s : pimpl()->solutions()) + for(const SolutionBase& s : pimpl()->solutions_) if (!processor(s)) break; } -template +template void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb, solution_container &trace, double trace_cost) { - const InterfaceState::Solutions& solutions = trajectories(start); + const InterfaceState::Solutions& solutions = start.trajectories(); if (solutions.empty()) // if we reached the end, call the callback cb(trace, trace_cost); else for (SolutionBase* successor : solutions) { @@ -445,7 +434,7 @@ void SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso } void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, - Introspection* introspection = nullptr) const + Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; sub_msg.id = introspection ? introspection->solutionId(*this) : 0; @@ -467,11 +456,20 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg, } +void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution, + Introspection *introspection) const +{ + wrapped_->fillMessage(solution, introspection); +} + ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name) : ContainerBasePrivate(me, name) { - starts_.reset(new Interface(std::bind(&ParallelContainerBase::onNewStartState, me, _1, _2))); - ends_.reset(new Interface(std::bind(&ParallelContainerBase::onNewEndState, me, _1, _2))); +} + +void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) { + for (const Stage::pointer& stage : children()) + copyState(external, stage->pimpl()->pullInterface(dir), updated); } @@ -486,54 +484,139 @@ void ParallelContainerBase::reset() { // recursively reset children ContainerBase::reset(); + // clear buffers + auto impl = pimpl(); + impl->solutions_.clear(); + impl->failures_.clear(); + impl->wrapped_solutions_.clear(); + impl->created_solutions_.clear(); + impl->states_.clear(); } +/* States received by the container need to be copied to all children's pull interfaces. + * States generated by children can be directly forwarded into the container's push interfaces. + */ void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) { InitStageException errors; auto impl = pimpl(); - // connect children such that they directly send to this' prevEnds() / nextStarts() + // initialize push connections of children + InterfacePtr push_prev = impl->getPushBackwardInterface(); + InterfacePtr push_next = impl->getPushForwardInterface(); for (const Stage::pointer& stage : impl->children()) { StagePrivate *child = stage->pimpl(); - child->setPrevEnds(impl->prevEnds()); - child->setNextStarts(impl->nextStarts()); + child->setPrevEnds(push_prev); + child->setNextStarts(push_next); + } + // recursively init + validate all children + // this needs to be done *after* initializing push connections + ContainerBase::init(scene); + + bool pulls[2]; + for (const Stage::pointer& stage : impl->children()) { + StagePrivate *child = stage->pimpl(); + // is there any child reading from starts() or ends() ? + pulls[Interface::START] |= bool(child->pullInterface(Interface::START)); + pulls[Interface::END] |= bool(child->pullInterface(Interface::END)); } - // recursively init + validate all children - // this needs to be done *after* initializing the connections - ContainerBase::init(scene); + // initialize this' pull connections + for (Interface::Direction dir : { Interface::START, Interface::END }) { + if (pulls[dir]) + impl->pullInterface(dir).reset(new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl, dir, _1, _2))); + else + impl->pullInterface(dir).reset(); + } + + if (impl->children().empty()) + errors.push_back(*this, "no children"); if (errors) throw errors; } -void ParallelContainerBase::onNewSolution(const SolutionBase &s) +size_t ParallelContainerBase::numSolutions() const { - // update state priorities - InterfaceState::Priority prio(1, s.cost()); - InterfaceState* start = const_cast(s.start()); - start->owner()->updatePriority(*start, prio); - InterfaceState* end = const_cast(s.end()); - end->owner()->updatePriority(*end, prio); + return pimpl()->solutions_.size(); +} - pimpl()->newSolution(s); +void ParallelContainerBase::processSolutions(const Stage::SolutionProcessor &processor) const +{ + for(const SolutionBase* s : pimpl()->solutions_) + if (!processor(*s)) + break; +} + +size_t ParallelContainerBase::numFailures() const +{ + return pimpl()->failures_.size(); +} + +void ParallelContainerBase::processFailures(const Stage::SolutionProcessor &processor) const +{ + for(const SolutionBase* f : pimpl()->failures_) + if (!processor(*f)) + break; +} + +void ParallelContainerBase::onNewSolution(const SolutionBase& s) +{ + liftSolution(&s); +} + +void ParallelContainerBase::liftSolution(const SolutionBase* solution, double cost) +{ + auto impl = pimpl(); + // create new WrappedSolution instance + auto wit = impl->wrapped_solutions_.insert(impl->wrapped_solutions_.end(), WrappedSolution(impl, solution, cost)); + + if (wit->isFailure()) { + wit->setStartState(*solution->start()); + wit->setEndState(*solution->end()); + impl->failures_.push_back(&*wit); + } else { + impl->solutions_.insert(&*wit); + impl->liftSolution(*wit, solution->start(), solution->end()); + } + impl->newSolution(*wit); +} + +void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t) +{ + auto impl = pimpl(); + assert(impl->prevEnds() && impl->nextStarts()); + + t.setCreator(impl); + // store newly created solution (otherwise it's lost) + auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t)); + + if (it->isFailure()) { + auto state_it = impl->states_.insert(impl->states_.end(), std::move(state)); + it->setStartState(*state_it); + it->setEndState(*state_it); + impl->failures_.push_back(&*it); + } else { + // directly spawn states in push interfaces + impl->prevEnds()->add(InterfaceState(state), NULL, &*it); + impl->nextStarts()->add(std::move(state), &*it, NULL); + impl->solutions_.insert(&*it); + } + impl->newSolution(*it); } WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name) - : ContainerBasePrivate(me, name) -{ - dummy_starts_.reset(new Interface(Interface::NotifyFunction())); - dummy_ends_.reset(new Interface(Interface::NotifyFunction())); -} + : ParallelContainerBasePrivate(me, name) +{} + WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child) : WrapperBase(new WrapperBasePrivate(this, name), std::move(child)) {} WrapperBase::WrapperBase(WrapperBasePrivate *impl, Stage::pointer &&child) - : ContainerBase(impl) + : ParallelContainerBase(impl) { if (child) insert(std::move(child)); } @@ -543,38 +626,7 @@ bool WrapperBase::insert(Stage::pointer &&stage, int before) // restrict num of children to one if (numChildren() > 0) return false; - return ContainerBase::insert(std::move(stage), before); -} - -void WrapperBase::reset() -{ - pimpl()->dummy_starts_->clear(); - pimpl()->dummy_ends_->clear(); -} - -void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene) -{ - auto impl = pimpl(); - - if (numChildren() != 1) - throw InitStageException(*this, "no wrapped child"); - - // as a generator-like stage, we don't accept inputs - assert(!impl->starts()); - assert(!impl->ends()); - - // provide a dummy interface to receive interface states of wrapped child - wrapped()->pimpl()->setPrevEnds(impl->dummy_ends_); - wrapped()->pimpl()->setNextStarts(impl->dummy_starts_); - - // init + validate children - ContainerBase::init(scene); -} - -size_t WrapperBase::numSolutions() const -{ - // dummy implementation needed to allow insert() in constructor - return 0; + return ParallelContainerBase::insert(std::move(stage), before); } Stage* WrapperBase::wrapped() @@ -582,88 +634,65 @@ Stage* WrapperBase::wrapped() return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get(); } -void WrapperBase::onNewSolution(const SolutionBase &s) -{ - // update state priorities - InterfaceState::Priority prio(1, s.cost()); - InterfaceState* start = const_cast(s.start()); - start->owner()->updatePriority(*start, prio); - InterfaceState* end = const_cast(s.end()); - end->owner()->updatePriority(*end, prio); - - pimpl()->newSolution(s); -} - - -WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name) - : WrapperBasePrivate(me, name) -{} - -Wrapper::Wrapper(WrapperPrivate *impl, Stage::pointer &&child) - : WrapperBase(impl, std::move(child)) -{} -Wrapper::Wrapper(const std::string &name, Stage::pointer &&child) - : Wrapper(new WrapperPrivate(this, name), std::move(child)) -{} - -void Wrapper::reset() -{ - WrapperBase::reset(); - pimpl()->solutions_.clear(); -} - -bool Wrapper::canCompute() const +bool WrapperBase::canCompute() const { return wrapped()->pimpl()->canCompute(); } -bool Wrapper::compute() +bool WrapperBase::compute() { size_t num_before = numSolutions(); wrapped()->pimpl()->compute(); return numSolutions() > num_before; } -size_t Wrapper::numSolutions() const + +bool Alternatives::canCompute() const { - return pimpl()->solutions_.size(); + for (const auto& stage : pimpl()->children()) + if (stage->pimpl()->canCompute()) + return true; + return false; } -size_t Wrapper::numFailures() const +bool Alternatives::compute() { - return pimpl()->failures_.size(); + bool success = false; + for (const auto& stage : pimpl()->children()) + success |= stage->pimpl()->compute(); + return success; } -void Wrapper::processSolutions(const Stage::SolutionProcessor &processor) const + +void Fallbacks::reset() { - for(const auto& s : pimpl()->solutions_) - if (!processor(*s)) - break; + active_child_ = nullptr; + ParallelContainerBase::reset(); } -void Wrapper::processFailures(const Stage::SolutionProcessor &processor) const +void Fallbacks::init(const planning_scene::PlanningSceneConstPtr &scene) { - for(const auto& s : pimpl()->failures_) - if (!processor(*s)) - break; + ParallelContainerBase::init(scene); + active_child_ = pimpl()->children().front().get(); } -void Wrapper::spawn(InterfaceState &&state, std::unique_ptr&& s) +bool Fallbacks::canCompute() const { - auto impl = pimpl(); - s->setCreator(impl); - SolutionBase* solution = s.get(); - if (s->isFailure()) { - impl->failure_states_.emplace_back(std::move(state)); - s->setStartState(impl->failure_states_.back()); - s->setEndState(impl->failure_states_.back()); - impl->failures_.emplace_back(std::move(s)); - } else { - impl->solutions_.insert(std::move(s)); - impl->prevEnds()->add(InterfaceState(state), NULL, solution); - impl->nextStarts()->add(std::move(state), solution, NULL); + while (active_child_) { + StagePrivate* child = active_child_->pimpl(); + if (child->canCompute()) return true; + + // active child failed, continue with next + auto next = child->it(); ++next; + active_child_ = next->get(); } - impl->newSolution(*solution); + return false; +} + +bool Fallbacks::compute() +{ + if (!active_child_) return false; + return active_child_->pimpl()->compute(); } } } diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp new file mode 100644 index 00000000..163d0af2 --- /dev/null +++ b/core/src/solvers/cartesian_path.cpp @@ -0,0 +1,114 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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 Goerner, Robert Haschke + Desc: generate and validate a straight-line Cartesian path +*/ + +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +CartesianPath::CartesianPath() +{ + auto& p = properties(); + p.declare("step_size", 0.01, "step size between consecutive waypoints"); + p.declare("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step"); + p.declare("min_fraction", 1.0, "fraction of motion required for success"); + p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); + p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); +} + +void CartesianPath::init(const core::RobotModelConstPtr &robot_model) +{ +} + +bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) +{ + const moveit::core::LinkModel* link; + const std::string& link_name = solvers::getEndEffectorLink(jmg); + if (!(link = from->getRobotModel()->getLinkModel(link_name))) { + ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName()); + return false; + } + + // reach pose of forward kinematics + return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result); +} + +bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d &target, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr &result) +{ + const auto& props = properties(); + planning_scene::PlanningScenePtr sandbox_scene = from->diff(); + + auto isValid = [&sandbox_scene](moveit::core::RobotState* state, + const moveit::core::JointModelGroup* jmg, + const double* joint_positions) { + state->setJointGroupPositions(jmg, joint_positions); + state->update(); + return !sandbox_scene->isStateColliding(const_cast(*state), jmg->getName()); + }; + + std::vector trajectory; + double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath( + jmg, trajectory, &link, target, true, + props.get("step_size"), + props.get("jump_threshold"), + isValid); + if (achieved_fraction >= props.get("min_fraction")) { + result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg)); + for (const auto& waypoint : trajectory) + result->addSuffixWayPoint(waypoint, 0.0); + + trajectory_processing::IterativeParabolicTimeParameterization timing; + timing.computeTimeStamps(*result, + props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); + return true; + } + return false; +} + +} } } diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp new file mode 100644 index 00000000..c5c7fa54 --- /dev/null +++ b/core/src/solvers/pipeline_planner.cpp @@ -0,0 +1,137 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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: Robert Haschke + Desc: plan using MoveIt's PlanningPipeline +*/ + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +PipelinePlanner::PipelinePlanner() +{ + auto& p = properties(); + p.declare("planner", "", "planner id"); + + p.declare("num_planning_attempts", 1u, "number of planning attempts"); + p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); + p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); + p.declare("workspace_parameters", moveit_msgs::WorkspaceParameters(), "allowed workspace of mobile base?"); + + p.declare("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals"); + p.declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); + p.declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); +} + +void PipelinePlanner::init(const core::RobotModelConstPtr &robot_model) +{ + planner_ = Task::createPlanner(robot_model); +} + +void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, + const PropertyMap& p, + const moveit::core::JointModelGroup *jmg, + double timeout) +{ + req.group_name = jmg->getName(); + req.planner_id = p.get("planner"); + req.allowed_planning_time = timeout; + + req.num_planning_attempts = p.get("num_planning_attempts"); + req.max_velocity_scaling_factor = p.get("max_velocity_scaling_factor"); + req.max_acceleration_scaling_factor = p.get("max_acceleration_scaling_factor"); + req.workspace_parameters = p.get("workspace_parameters"); +} + +bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) +{ + const auto& props = properties(); + moveit_msgs::MotionPlanRequest req; + initMotionPlanRequest(req, props, jmg, timeout); + + req.goal_constraints.resize(1); + req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( + to->getCurrentState(), jmg, + props.get("goal_joint_tolerance")); + + ::planning_interface::MotionPlanResponse res; + if(!planner_->generatePlan(from, req, res)) + return false; + + result = res.trajectory_; + return true; +} + +bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target_eigen, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result) + +{ + const auto& props = properties(); + moveit_msgs::MotionPlanRequest req; + initMotionPlanRequest(req, props, jmg, timeout); + + geometry_msgs::PoseStamped target; + target.header.frame_id = from->getPlanningFrame(); + tf::poseEigenToMsg(target_eigen, target.pose); + + req.goal_constraints.resize(1); + req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints( + link.getName(), target, + props.get("goal_position_tolerance"), + props.get("goal_orientation_tolerance")); + + ::planning_interface::MotionPlanResponse res; + if(!planner_->generatePlan(from, req, res)) + return false; + + result = res.trajectory_; + return true; +} + +} } } diff --git a/core/src/solvers/planner_interface.cpp b/core/src/solvers/planner_interface.cpp new file mode 100644 index 00000000..64cfa93b --- /dev/null +++ b/core/src/solvers/planner_interface.cpp @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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: Robert Haschke + Desc: planner interface +*/ + +#include +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg) +{ + // TODO: directly return LinkModel* + const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip(); + return link ? link->getName() : std::string(); +} + +} } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 8e0845f7..5a7f57ae 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -216,8 +216,8 @@ SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) { return *solutions_.insert(std::move(trajectory)); } else if (me()->storeFailures()) { // only store failures when introspection is enabled - failures_.emplace_back(std::move(trajectory)); - return failures_.back(); + auto it = failures_.insert(failures_.end(), std::move(trajectory)); + return *it; } else return trajectory; } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index cfb89072..f7e88f7c 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -1,16 +1,28 @@ add_library(${PROJECT_NAME}_stages - ${PROJECT_INCLUDE}/stages/cartesian_position_motion.h ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h + ${PROJECT_INCLUDE}/stages/connect.h + ${PROJECT_INCLUDE}/stages/move_to.h + ${PROJECT_INCLUDE}/stages/move_relative.h + + ${PROJECT_INCLUDE}/stages/cartesian_position_motion.h ${PROJECT_INCLUDE}/stages/gripper.h + ${PROJECT_INCLUDE}/stages/modify_planning_scene.h + ${PROJECT_INCLUDE}/stages/fix_collision_objects.h ${PROJECT_INCLUDE}/stages/move.h - cartesian_position_motion.cpp current_state.cpp compute_ik.cpp generate_grasp_pose.cpp + connect.cpp + move_to.cpp + move_relative.cpp + + cartesian_position_motion.cpp gripper.cpp + modify_planning_scene.cpp + fix_collision_objects.cpp move.cpp ) target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 8ae8fbda..f5f2e0d4 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -20,7 +20,7 @@ namespace moveit { namespace task_constructor { namespace stages { ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) - : Wrapper(name, std::move(child)) + : WrapperBase(name, std::move(child)) { auto& p = properties(); p.declare("timeout", 1.0); @@ -218,17 +218,17 @@ void ComputeIK::onNewSolution(const SolutionBase &s) start_time = now; planning_scene::PlanningSceneConstPtr scene = s.start()->scene(); - std::unique_ptr solution(new SubTrajectory()); + SubTrajectory solution; // include markers from original solution - std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution->markers())); + std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution.markers())); // frame at target pose target_pose_msg.header.frame_id = scene->getPlanningFrame(); - rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame"); if (succeeded) { - solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); + solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); // create a new scene for each solution as they will have different robot states planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff(); robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst(); @@ -240,11 +240,11 @@ void ComputeIK::onNewSolution(const SolutionBase &s) auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) { marker.ns = "ik solution"; marker.color.a *= 0.5; - solution->markers().push_back(marker); + solution.markers().push_back(marker); }; generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames()); } else { - solution->setCost(std::numeric_limits::infinity()); + solution.setCost(std::numeric_limits::infinity()); } spawn(InterfaceState(scene), std::move(solution)); diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp new file mode 100644 index 00000000..2a11d5f0 --- /dev/null +++ b/core/src/stages/connect.cpp @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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 Goerner, Robert Haschke + Desc: Connect arbitrary states by motion planning +*/ + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner) + : Connecting(name) + , planner_(planner) +{ + auto& p = properties(); + p.declare("timeout", 10.0, "planning timeout"); + p.declare("group", "name of planning group"); +} + +void Connect::init(const planning_scene::PlanningSceneConstPtr &scene) +{ + Connecting::init(scene); + planner_->init(scene->getRobotModel()); +} + +bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { + const auto& props = properties(); + const std::string& group = props.get("group"); + double timeout = props.get("timeout"); + const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group); + + robot_trajectory::RobotTrajectoryPtr trajectory; + if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory)) + return false; + + connect(from, to, trajectory); + return true; +} + +} } } diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp new file mode 100644 index 00000000..9b9fdf61 --- /dev/null +++ b/core/src/stages/fix_collision_objects.cpp @@ -0,0 +1,140 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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: Elham Iravani, Robert Haschke + Desc: Fix collisions in input scene +*/ + +#include + +#include +#include +#include +#include +#include +#include + +namespace vm = visualization_msgs; + +namespace moveit { namespace task_constructor { namespace stages { + +FixCollisionObjects::FixCollisionObjects(const std::string &name) + : PropagatingEitherWay(name) +{ + auto& p = properties(); + p.declare("max_penetration", "maximally corrected penetration depth"); +} + +void FixCollisionObjects::setMaxPenetration(double penetration) +{ + setProperty("max_penetration", penetration); +} + +bool FixCollisionObjects::computeForward(const InterfaceState &from) +{ + planning_scene::PlanningScenePtr to = from.scene()->diff(); + SubTrajectory solution; + bool success = fixCollisions(*to, solution.markers()); + if (!success) solution.setCost(std::numeric_limits::infinity()); + sendForward(from, InterfaceState(to), std::move(solution)); + return success; +} + +bool FixCollisionObjects::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from = to.scene()->diff(); + SubTrajectory solution; + bool success = fixCollisions(*from, solution.markers()); + if (!success) solution.setCost(std::numeric_limits::infinity()); + sendBackward(InterfaceState(from), to, std::move(solution)); + return success; +} + +bool FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene, std::deque &markers) const +{ + const auto& props = properties(); + double penetration = props.get("max_penetration"); + + collision_detection::CollisionRequest req; + collision_detection::CollisionResult res; + req.group_name = ""; // check collisions for complete robot + req.contacts = true; + req.max_contacts = 100; + req.max_contacts_per_pair = 5; + req.verbose = false; + req.distance = false; + + scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), scene.getCurrentState(), + scene.getAllowedCollisionMatrix()); + + geometry_msgs::Pose pose; + + if (!res.collision) + return true; + + vm::Marker m; + m.header.frame_id = scene.getPlanningFrame(); + m.ns = "collisions"; + rviz_marker_tools::setColor(m.color, rviz_marker_tools::RED); + + ROS_INFO_THROTTLE(1, "collision(s) detected"); + for (const auto& info : res.contacts) { + for (const auto& contact : info.second) { + tf::poseEigenToMsg(Eigen::Translation3d(contact.pos) * Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), contact.normal), m.pose); + rviz_marker_tools::makeArrow(m, contact.depth); + markers.push_back(m); + } + } + + // check again + scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(), scene.getCurrentState(), + scene.getAllowedCollisionMatrix()); + return !res.collision; +} + +void FixCollisionObjects::fixCollision(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object) const +{ + moveit_msgs::CollisionObject collision_obj; + collision_obj.header.frame_id = scene.getPlanningFrame(); + collision_obj.id = object; + collision_obj.operation = moveit_msgs::CollisionObject::MOVE; + + collision_obj.primitive_poses.resize(1); + collision_obj.primitive_poses[0] = pose; + + if(!scene.processCollisionObjectMsg(collision_obj)) + std::cout<<"Moving FAILED"< +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +ModifyPlanningScene::ModifyPlanningScene(const std::string &name) + : PropagatingEitherWay(name) +{} + +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))); + Names &o = it_inserted.first->second.first; + o.insert(o.end(), objects.begin(), objects.end()); +} + +void ModifyPlanningScene::enableCollisions(const Names& first, const Names& second, bool enable_collision) { + collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, enable_collision})); +} + +void ModifyPlanningScene::enableCollisions(const std::string &first, const moveit::core::JointModelGroup &jmg, bool enable_collision) +{ + const auto& links = jmg.getLinkModelNamesWithCollisionGeometry(); + if (!links.empty()) + enableCollisions(Names({first}), links, enable_collision); +} + +bool ModifyPlanningScene::computeForward(const InterfaceState &from){ + sendForward(from, apply(from, false), robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +bool ModifyPlanningScene::computeBackward(const InterfaceState &to) +{ + sendBackward(apply(to, true), to, robot_trajectory::RobotTrajectoryPtr()); + return true; +} + +void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene, + const std::pair >& pair, + bool invert) +{ + moveit_msgs::AttachedCollisionObject obj; + obj.link_name = pair.first; + bool attach = pair.second.second; + if (invert) attach = !attach; + obj.object.operation = attach ? (int8_t) moveit_msgs::CollisionObject::ADD + : (int8_t) moveit_msgs::CollisionObject::REMOVE; + for (const std::string& name : pair.second.first) { + obj.object.id = name; + scene.processAttachedCollisionObjectMsg(obj); + } +} + +void ModifyPlanningScene::enableCollisions(planning_scene::PlanningScene &scene, + const CollisionMatrixPairs& pairs, + bool invert) +{ + collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst(); + bool enable = invert ? !pairs.enable : pairs.enable; + if (pairs.second.empty()) { + for (const auto &name : pairs.first) + acm.setEntry(name, enable); + } else + acm.setEntry(pairs.first, pairs.second, enable); +} + +// invert indicates, whether to detach instead of attach (and vice versa) +// as well as to disable instead of enable collision (and vice versa) +InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) +{ + planning_scene::PlanningScenePtr scene = from.scene()->diff(); + InterfaceState result(scene); + // initialize properties from input state + result.properties() = from.properties(); + + // attach/detach objects + for (const auto &pair : attach_objects_) + attachObjects(*scene, pair, invert); + + // enable/disable collisions + for (const auto &pairs : collision_matrix_edits_) + enableCollisions(*scene, pairs, invert); + + if (callback_) + callback_(scene, result.properties()); + + return result; +} + +} } } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp new file mode 100644 index 00000000..ec94f96b --- /dev/null +++ b/core/src/stages/move_relative.cpp @@ -0,0 +1,274 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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 Goerner, Robert Haschke + Desc: Move link along Cartesian direction +*/ + +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +MoveRelative::MoveRelative(std::string name, const solvers::PlannerInterfacePtr& planner) + : PropagatingEitherWay(name) + , planner_(planner) +{ + auto& p = properties(); + p.declare("timeout", 10.0, "planning timeout"); + p.declare("marker_ns", "", "marker namespace"); + p.declare("group", "name of planning group"); + p.declare("link", "", "link to move (default is tip of jmg)"); + p.declare("min_distance", "minimum distance to move"); + p.declare("max_distance", "maximum distance to move"); + + p.declare("twist", "Cartesian twist transform"); + p.declare("direction", "Cartesian translation direction"); +} + +void MoveRelative::setGroup(const std::string& group){ + setProperty("group", group); +} + +void MoveRelative::setLink(const std::string& link){ + setProperty("link", link); +} + +void MoveRelative::setMinDistance(double distance){ + setProperty("min_distance", distance); +} + +void MoveRelative::setMaxDistance(double distance){ + setProperty("max_distance", distance); +} + +void MoveRelative::setMinMaxDistance(double min_distance, double max_distance){ + setProperty("min_distance", min_distance); + setProperty("max_distance", max_distance); +} + +void MoveRelative::along(const geometry_msgs::TwistStamped &twist) +{ + setProperty("twist", twist); +} + +void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction) +{ + setProperty("direction", direction); +} + +void MoveRelative::init(const planning_scene::PlanningSceneConstPtr &scene) +{ + PropagatingEitherWay::init(scene); + planner_->init(scene->getRobotModel()); +} + +bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, + SubTrajectory &trajectory, Direction dir) { + scene = state.scene()->diff(); + assert(scene->getRobotModel()); + + const auto& props = properties(); + double timeout = props.get("timeout"); + const std::string& group = props.get("group"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); + if (!jmg) { + ROS_WARN_STREAM("MoveTo: invalid joint model group: " << group); + return false; + } + + // only allow single target + size_t count_goals = props.countDefined({"twist", "direction"}); + if (count_goals != 1) { + if (count_goals == 0) ROS_WARN("MoveTo: no goal defined"); + else ROS_WARN("MoveTo: cannot plan to multiple goals"); + return false; + } + + // Cartesian targets require the link name + std::string link_name = props.get("link"); + const moveit::core::LinkModel* link; + if (link_name.empty()) + link_name = solvers::getEndEffectorLink(jmg); + if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { + ROS_WARN_STREAM("MoveTo: no or invalid link name specified: " << link_name); + return false; + } + + Property prop = props.property("max_distance"); + double max_distance = prop.value().empty() ? 0.0 : std::abs(boost::any_cast(prop.value())); + prop = props.property("min_distance"); + double min_distance = prop.value().empty() ? -1.0 : boost::any_cast(prop.value()); + + bool use_rotation_distance = false; // measure achieved distance as rotation? + Eigen::Vector3d linear; // linear translation + Eigen::Vector3d angular; // angular rotation + double linear_norm = 0.0, angular_norm = 0.0; + + Eigen::Affine3d target_eigen; + const Eigen::Affine3d& link_pose = scene->getFrameTransform(link_name); + + boost::any goal = props.get("twist"); + if (!goal.empty()) { + const geometry_msgs::TwistStamped& target = boost::any_cast(goal); + const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id); + tf::vectorMsgToEigen(target.twist.linear, linear); + tf::vectorMsgToEigen(target.twist.angular, angular); + + linear_norm = linear.norm(); + angular_norm = angular.norm(); + angular /= angular_norm; // normalize angular + use_rotation_distance = linear_norm < std::numeric_limits::epsilon(); + + // use max distance? + if (max_distance > 0.0) { + double scale; + if (!use_rotation_distance) // non-zero linear motion defines distance + scale = max_distance / linear_norm; + else if (angular_norm > std::numeric_limits::epsilon()) + scale = max_distance / angular_norm; + linear *= scale; + linear_norm *= scale; + angular_norm *= scale; + } + + // invert direction? + if (dir == BACKWARD) { + linear *= -1.0; + angular *= -1.0; + } + + // compute absolute transform for link + linear = frame_pose.linear() * linear; + angular = frame_pose.linear() * angular; + target_eigen = link_pose; + target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular); + target_eigen.translation() += linear; + } + + goal = props.get("direction"); + if (!goal.empty()) { + const geometry_msgs::Vector3Stamped& target = boost::any_cast(goal); + const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id); + tf::vectorMsgToEigen(target.vector, linear); + + // use max distance? + if (max_distance > 0.0) { + linear.normalize(); + linear *= max_distance; + } + linear_norm = linear.norm(); + + // invert direction? + if (dir == BACKWARD) + linear *= -1.0; + + // compute absolute transform for link + linear = frame_pose.linear() * linear; + target_eigen = link_pose; + target_eigen.translation() += linear; + } + + robot_trajectory::RobotTrajectoryPtr robot_trajectory; + bool success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); + + // min_distance reached? + if (success && min_distance > 0.0) { + const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint(); + Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link); + if (use_rotation_distance) { + Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); + success = rotation.angle() > min_distance; + } else + success = (reached_pose.translation() - link_pose.translation()).norm() > min_distance; + } + + // store result + if (success || (robot_trajectory && storeFailures())) { + scene->setCurrentState(robot_trajectory->getLastWayPoint()); + if (dir == BACKWARD) robot_trajectory->reverse(); + trajectory.setTrajectory(robot_trajectory); + } + + // add an arrow marker + visualization_msgs::Marker m; + m.ns = props.get("marker_ns"); + if (!m.ns.empty()) { + m.header.frame_id = scene->getPlanningFrame(); + if (linear_norm > 1e-3) { + rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN + : rviz_marker_tools::RED); + rviz_marker_tools::makeArrow(m, linear_norm); + auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear); + Eigen::Vector3d pos(link_pose.translation()); + if (dir == BACKWARD) { + // flip arrow direction + quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()); + // arrow tip at goal_pose + pos += quat * Eigen::Vector3d(-linear_norm, 0, 0); + } + tf::pointEigenToMsg(pos, m.pose.position); + tf::quaternionEigenToMsg(quat, m.pose.orientation); + trajectory.markers().push_back(m); + } + } + + return success; +} + +bool MoveRelative::computeForward(const InterfaceState &from) +{ + planning_scene::PlanningScenePtr to; + SubTrajectory trajectory; + + bool success = compute(from, to, trajectory, FORWARD); + if (!success) trajectory.setCost(std::numeric_limits::infinity()); + sendForward(from, InterfaceState(to), std::move(trajectory)); + return success; +} + +bool MoveRelative::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from; + SubTrajectory trajectory; + + bool success = compute(to, from, trajectory, BACKWARD); + if (!success) trajectory.setCost(std::numeric_limits::infinity()); + sendBackward(InterfaceState(from), to, std::move(trajectory)); + return success; +} + +} } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp new file mode 100644 index 00000000..5903b089 --- /dev/null +++ b/core/src/stages/move_to.cpp @@ -0,0 +1,193 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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 Goerner, Robert Haschke + Desc: Move to joint-state or Cartesian goal pose +*/ + +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +MoveTo::MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner) + : PropagatingEitherWay(name) + , planner_(planner) +{ + auto& p = properties(); + p.declare("timeout", 10.0, "planning timeout"); + p.declare("group", "name of planning group"); + p.declare("link", "", "link to move (default is tip of jmg)"); + + p.declare("pose", "Cartesian target pose"); + p.declare("point", "Cartesian target point"); + p.declare("joint_pose", "named joint pose"); +} + +void MoveTo::setGroup(const std::string& group){ + setProperty("group", group); +} + +void MoveTo::setLink(const std::string& link){ + setProperty("link", link); +} + +void MoveTo::setGoal(const geometry_msgs::PoseStamped &pose) +{ + setProperty("pose", pose); +} + +void MoveTo::setGoal(const geometry_msgs::PointStamped &point) +{ + setProperty("point", point); +} + +void MoveTo::setGoal(const std::string &joint_pose) +{ + setProperty("joint_pose", joint_pose); +} + +void MoveTo::init(const planning_scene::PlanningSceneConstPtr &scene) +{ + PropagatingEitherWay::init(scene); + planner_->init(scene->getRobotModel()); +} + +bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, + SubTrajectory &trajectory, Direction dir) { + scene = state.scene()->diff(); + assert(scene->getRobotModel()); + + const auto& props = properties(); + double timeout = props.get("timeout"); + const std::string& group = props.get("group"); + const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); + if (!jmg) { + ROS_WARN_STREAM("MoveTo: invalid joint model group: " << group); + return false; + } + + // only allow single target + size_t count_goals = props.countDefined({"joint_pose", "pose", "point"}); + if (count_goals != 1) { + if (count_goals == 0) ROS_WARN("MoveTo: no goal defined"); + else ROS_WARN("MoveTo: cannot plan to multiple goals"); + return false; + } + + robot_trajectory::RobotTrajectoryPtr robot_trajectory; + bool success = false; + + boost::any goal = props.get("joint_pose"); + if (!goal.empty()) { + const std::string& named_joint_pose = boost::any_cast(goal); + if (!scene->getCurrentStateNonConst().setToDefaultValues(jmg, named_joint_pose)) { + ROS_WARN("MoveTo: unknown joint pose '%s' for jmg '%s'", named_joint_pose.c_str(), group.c_str()); + return false; + } + success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory); + } else { + // Cartesian targets require the link name + std::string link_name = props.get("link"); + const moveit::core::LinkModel* link; + if (link_name.empty()) + link_name = solvers::getEndEffectorLink(jmg); + if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { + ROS_WARN_STREAM("MoveTo: no or invalid link name specified: " << link_name); + return false; + } + + Eigen::Affine3d target_eigen; + + goal = props.get("pose"); + if (!goal.empty()) { + const geometry_msgs::PoseStamped& target = boost::any_cast(goal); + tf::poseMsgToEigen(target.pose, target_eigen); + + // transform target into global frame + const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id); + target_eigen = frame * target_eigen; + } + + goal = props.get("point"); + if (!goal.empty()) { + const geometry_msgs::PointStamped& target = boost::any_cast(goal); + Eigen::Vector3d target_point; + tf::pointMsgToEigen(target.point, target_point); + + // transform target into global frame + const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id); + target_point = frame * target_point; + + // retain link orientation + Eigen::Affine3d target_eigen = scene->getCurrentState().getGlobalLinkTransform(link_name); + target_eigen.translation() = target_point; + } + + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); + } + + // store result + if (success || (robot_trajectory && storeFailures())) { + scene->setCurrentState(robot_trajectory->getLastWayPoint()); + if (dir == BACKWARD) robot_trajectory->reverse(); + trajectory.setTrajectory(robot_trajectory); + } + + return success; +} + +bool MoveTo::computeForward(const InterfaceState &from){ + planning_scene::PlanningScenePtr to; + SubTrajectory trajectory; + + bool success = compute(from, to, trajectory, FORWARD); + if (!success) trajectory.setCost(std::numeric_limits::infinity()); + sendForward(from, InterfaceState(to), std::move(trajectory)); + return success; +} + +bool MoveTo::computeBackward(const InterfaceState &to) +{ + planning_scene::PlanningScenePtr from; + SubTrajectory trajectory; + + bool success = compute(to, from, trajectory, BACKWARD); + if (!success) trajectory.setCost(std::numeric_limits::infinity()); + sendBackward(InterfaceState(from), to, std::move(trajectory)); + return success; +} + +} } } diff --git a/core/src/task.cpp b/core/src/task.cpp index eddce014..aa0a7ca3 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -69,6 +69,10 @@ void Task::initModel () { throw Exception("Task failed to construct RobotModel"); } +const moveit::core::RobotModelPtr Task::getRobotModel() const { + return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr(); +} + planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) { assert(rml_); @@ -182,10 +186,19 @@ void Task::reset() void Task::init(const planning_scene::PlanningSceneConstPtr &scene) { - WrapperBase::init(scene); + auto impl = pimpl(); + // initialize push connections of wrapped child + StagePrivate *child = wrapped()->pimpl(); + child->setPrevEnds(impl->pendingBackward()); + child->setNextStarts(impl->pendingForward()); + + // explicitly call ContainerBase::init(), not WrapperBase::init() + // to keep the just set push interface for the wrapped child + ContainerBase::init(scene); + // provide introspection instance to all stages - pimpl()->setIntrospection(introspection_.get()); - pimpl()->traverseStages([this](Stage& stage, int) { + impl->setIntrospection(introspection_.get()); + impl->traverseStages([this](Stage& stage, int) { stage.pimpl()->setIntrospection(introspection_.get()); return true; }, 1, UINT_MAX); @@ -224,9 +237,7 @@ bool Task::plan() size_t Task::numSolutions() const { - auto w = stages(); - // during initial insert() we call numSolutions(), but wrapped() is not yet defined - return w ? w->numSolutions() : 0; + return stages()->numSolutions(); } void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) const @@ -242,7 +253,7 @@ void Task::publishAllSolutions(bool wait) void Task::onNewSolution(const SolutionBase &s) { - WrapperBase::onNewSolution(s); + // no need to call WrapperBase::onNewSolution! if (introspection_) introspection_->publishSolution(s); } diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index d179c136..29b5d2f5 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -55,7 +55,7 @@ TEST_F(BaseTest, serialContainer) { SerialContainer c("serial"); SerialContainerPrivate *cp = c.pimpl(); // pretend, that the container is connected - InterfacePtr dummy(new Interface(Interface::NotifyFunction())); + InterfacePtr dummy(new Interface); cp->setNextStarts(dummy); cp->setPrevEnds(dummy); planning_scene::PlanningScenePtr scene; diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index 928ea53d..bac4a247 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -18,8 +18,8 @@ public: robot_model_loader::RobotModelLoader loader; ps.reset(new planning_scene::PlanningScene(loader.getModel())); - prev.reset(new Interface(Interface::NotifyFunction())); - next.reset(new Interface(Interface::NotifyFunction())); + prev.reset(new Interface); + next.reset(new Interface); pimpl()->setPrevEnds(prev); pimpl()->setNextStarts(next); } diff --git a/visualization/motion_planning_tasks/CMakeLists.txt b/visualization/motion_planning_tasks/CMakeLists.txt index b64a811b..633ca748 100644 --- a/visualization/motion_planning_tasks/CMakeLists.txt +++ b/visualization/motion_planning_tasks/CMakeLists.txt @@ -1,3 +1,4 @@ add_subdirectory(utils) +add_subdirectory(properties) add_subdirectory(src) add_subdirectory(test) diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt new file mode 100644 index 00000000..4986363c --- /dev/null +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -0,0 +1,17 @@ +set(MOVEIT_LIB_NAME motion_planning_tasks_properties) + +set(SOURCES + property_factory.cpp +) +add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) + +target_link_libraries(${MOVEIT_LIB_NAME} + ${QT_LIBRARIES} +) +target_include_directories(${MOVEIT_LIB_NAME} + PUBLIC $ +) + +install(TARGETS ${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp new file mode 100644 index 00000000..ed5bc290 --- /dev/null +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -0,0 +1,100 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#include "property_factory.h" +#include +#include + +#include +#include +#include + +using namespace moveit::task_constructor; + +namespace moveit_rviz_plugin { + +/// TODO: We also need to provide methods to sync both properties in both directions. +/// In our Property we could store a callback function to update the rviz::Property. +/// The rviz::Property can simply use a lambda-function slot to update our Property. +/// However, we need to avoid infinite loops in doing so. Our Property cannot compare values... +template +RVIZProp* helper(const QString& name, Property* prop) { + T value = prop->defined() ? boost::any_cast(prop->value()) : T(); + return new RVIZProp(name, value, QString::fromStdString(prop->description())); +} + +PropertyFactory::PropertyFactory() +{ + // registe some standard types + registerType(&helper); + registerType(&helper); +} + +PropertyFactory& PropertyFactory::instance() +{ + static PropertyFactory instance_; + return instance_; +} + +void PropertyFactory::registerType(const std::string &type_name, const FactoryFunction &f) +{ + registry_.insert(std::make_pair(type_name, f)); +} + +rviz::Property* PropertyFactory::create(const std::string& prop_name, Property* prop) const +{ + auto it = registry_.find(prop->typeName()); + if (it == registry_.end()) return nullptr; + return it->second(QString::fromStdString(prop_name), prop); +} + +rviz::PropertyTreeModel* createPropertyTreeModel(PropertyMap& properties, QObject* parent) { + PropertyFactory& factory = PropertyFactory::instance(); + + rviz::Property* root = new rviz::Property(); + rviz::PropertyTreeModel *model = new rviz::PropertyTreeModel(root, parent); + for (auto& prop : properties) { + rviz::Property* rviz_prop = factory.create(prop.first, &prop.second); + if (!rviz_prop) rviz_prop = new rviz::Property(QString::fromStdString(prop.first)); + rviz_prop->setParent(root); + } + // just to see something, when no properties are defined + if (model->rowCount() == 0) + new rviz::Property("no properties", QVariant(), QString(), root); + return model; +} + +} diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h new file mode 100644 index 00000000..6176445b --- /dev/null +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld 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 Bielefeld 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. + *********************************************************************/ + +/* Author: Robert Haschke */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace rviz { +class Property; +class PropertyTreeModel; +} +namespace moveit { namespace task_constructor { +class Property; +class PropertyMap; +} } + +namespace moveit_rviz_plugin { + +class PropertyFactory +{ +public: + static PropertyFactory& instance(); + + typedef std::function FactoryFunction; + + /// register a new factory function for type T + template + void registerType(const FactoryFunction& f) { registerType(std::type_index(typeid(T)).name(), f); } + + /// retrieve rviz property for given task_constructor property + rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property *prop) const; + +private: + std::map registry_; + + /// class is singleton + PropertyFactory(); + PropertyFactory(const PropertyFactory&) = delete; + void operator=(const PropertyFactory&) = delete; + + void registerType(const std::string& type_name, const FactoryFunction& f); +}; + +/// turn a PropertyMap into an rviz::PropertyTreeModel +rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::PropertyMap &properties, QObject *parent = nullptr); + +} diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index 830b58c1..c64d6114 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -1,6 +1,10 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_rviz_plugin) -qt_wrap_ui(UIC_FILES task_panel.ui) +qt_wrap_ui(UIC_FILES + task_panel.ui + task_view.ui + task_settings.ui +) add_library(${MOVEIT_LIB_NAME} factory_model.cpp @@ -22,7 +26,7 @@ add_library(${MOVEIT_LIB_NAME} set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} - motion_planning_tasks_utils moveit_task_visualization_tools + motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools ${catkin_LIBRARIES} ${QT_LIBRARIES} ) target_include_directories(${MOVEIT_LIB_NAME} diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index a5821631..b756d1c6 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -36,6 +36,7 @@ #include "local_task_model.h" #include "factory_model.h" +#include "properties/property_factory.h" #include #include @@ -232,4 +233,14 @@ DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex &index) return DisplaySolutionPtr(); } +rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex &index) +{ + Node *n = node(index); + if (!n) return nullptr; + auto it_inserted = properties_.insert(std::make_pair(n, nullptr)); + if (it_inserted.second) // newly inserted, create new model + it_inserted.first->second = createPropertyTreeModel(n->me()->properties(), this); + return it_inserted.first->second; +} + } diff --git a/visualization/motion_planning_tasks/src/local_task_model.h b/visualization/motion_planning_tasks/src/local_task_model.h index 9e9290c3..b9873a80 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -49,6 +49,7 @@ class LocalTaskModel typedef moveit::task_constructor::StagePrivate Node; Node *root_; StageFactoryPtr stage_factory_; + std::map properties_; inline Node* node(const QModelIndex &index) const; QModelIndex index(Node *n) const; @@ -73,6 +74,8 @@ public: QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex &index) override; + + rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 0b200282..59a14801 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -40,6 +40,8 @@ #include #include #include +#include +#include #include #include @@ -64,9 +66,11 @@ struct RemoteTaskModel::Node { InterfaceFlags interface_flags_; NodeFlags node_flags_; std::unique_ptr solutions_; + std::unique_ptr properties_; inline Node(Node *parent) : parent_(parent) { solutions_.reset(new RemoteSolutionModel()); + properties_.reset(new rviz::PropertyTreeModel(new rviz::Property())); } bool setName(const QString& name) { @@ -369,6 +373,13 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index) return it->second; } +rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex &index) +{ + Node *n = node(index); + if (!n) return nullptr; + return n->properties_.get(); +} + namespace detail { // method used by sorted_ container (requiring an additional dereference to access id) template diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index aa5ed3df..dd770cec 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -87,6 +87,8 @@ public: QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex &index) override; + + rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 73149d6c..ac8e63fb 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -264,7 +264,7 @@ void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last TaskListModel* m = static_cast(sender()); for (; first <= last; ++first) { QModelIndex idx = m->index(first, 0, parent); - tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data())); + tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data()), first); } } diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index b6b6f6f4..29950221 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -50,6 +50,7 @@ #include namespace ros { class ServiceClient; } +namespace rviz { class PropertyTreeModel; } namespace moveit_rviz_plugin { @@ -84,8 +85,13 @@ public: Qt::ItemFlags flags(const QModelIndex &index) const override; unsigned int taskFlags() const { return flags_; } + /// get solution model for given stage index virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0; + /// get solution for given solution index virtual DisplaySolutionPtr getSolution(const QModelIndex &index) = 0; + + /// get property model for given stage index + virtual rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0; }; diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 18160db4..ae6072c6 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -79,13 +79,15 @@ TaskPanel::TaskPanel(QWidget* parent) : rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this)) { Q_D(TaskPanel); - // connect signals - connect(d->actionRemoveTaskTreeRows, SIGNAL(triggered()), this, SLOT(removeSelectedStages())); - connect(d->actionAddLocalTask, SIGNAL(triggered()), this, SLOT(addTask())); - connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget())); - connect(d->tasks_view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), - this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex))); + d->tasks_widget = new TaskView(this); + d->settings_widget = new TaskSettings(this); + layout()->addWidget(d->tasks_widget); + layout()->addWidget(d->settings_widget); + + connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget())); + connect(d->button_show_settings, SIGNAL(toggled(bool)), d->settings_widget, SLOT(setVisible(bool))); + d->settings_widget->setVisible(d->button_show_settings->isChecked()); // if still undefined, this becomes the global instance if (TaskPanelPrivate::global_instance_.isNull()) { @@ -127,14 +129,41 @@ void TaskPanel::decUseCount() TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr) : q_ptr(q_ptr) - , settings(new rviz::PropertyTreeModel(new rviz::Property, q_ptr)) { setupUi(q_ptr); - // init tasks view + button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); +} + +void TaskPanel::onInitialize() +{ + d_ptr->window_manager_ = vis_manager_->getWindowManager(); +} + +void TaskPanel::save(rviz::Config config) const +{ + rviz::Panel::save(config); +} + +void TaskPanel::load(const rviz::Config& config) +{ + rviz::Panel::load(config); +} + +void TaskPanel::showStageDockWidget() +{ + rviz::PanelDockWidget *dock = getStageDockWidget(d_ptr->window_manager_); + if (dock) dock->show(); +} + + +TaskViewPrivate::TaskViewPrivate(TaskView *q_ptr) + : q_ptr(q_ptr) +{ + setupUi(q_ptr); + MetaTaskListModel *meta_model = &MetaTaskListModel::instance(); StageFactoryPtr factory = getStageFactory(); if (factory) meta_model->setMimeTypes( { factory->mimeType() } ); - else button_show_stage_dock_widget->setEnabled(false); tasks_view->setModel(meta_model); tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection); @@ -147,99 +176,86 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr) solutions_view->setStretchSection(2); // init actions - tasks_view->addActions({actionRemoveTaskTreeRows, actionAddLocalTask}); - - initSettings(settings->getRoot()); - settings_view->setModel(settings); -} - -void TaskPanelPrivate::initSettings(rviz::Property* root) -{ + tasks_view->addActions({actionAddLocalTask, actionRemoveTaskTreeRows}); } std::pair -TaskPanelPrivate::getTaskListModel(const QModelIndex &index) const +TaskViewPrivate::getTaskListModel(const QModelIndex &index) const { auto *meta_model = static_cast(tasks_view->model()); return meta_model->getTaskListModel(index); } std::pair -TaskPanelPrivate::getTaskModel(const QModelIndex &index) const +TaskViewPrivate::getTaskModel(const QModelIndex &index) const { auto *meta_model = static_cast(tasks_view->model()); return meta_model->getTaskModel(index); } -void TaskPanelPrivate::unlock(TaskDisplay* display) +void TaskViewPrivate::lock(TaskDisplay* display) { if (locked_display_ && locked_display_ != display) { locked_display_->clearMarkers(); locked_display_->visualization()->unlock(); - locked_display_ = display; } + locked_display_ = display; } -void TaskPanel::onInitialize() +TaskView::TaskView(QWidget *parent) + : QWidget(parent), d_ptr(new TaskViewPrivate(this)) { - d_ptr->window_manager_ = vis_manager_->getWindowManager(); + Q_D(TaskView); + + // connect signals + connect(d->actionRemoveTaskTreeRows, SIGNAL(triggered()), this, SLOT(removeSelectedStages())); + connect(d->actionAddLocalTask, SIGNAL(triggered()), this, SLOT(addTask())); + + connect(d->tasks_view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)), + this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex))); + + onCurrentStageChanged(d->tasks_view->currentIndex(), QModelIndex()); } -void TaskPanel::save(rviz::Config config) const +void TaskView::addTask() { - rviz::Panel::save(config); - d_ptr->settings->getRoot()->save(config); -} - -void TaskPanel::load(const rviz::Config& config) -{ - rviz::Panel::load(config); - d_ptr->settings->getRoot()->load(config); -} - -void TaskPanel::addTask() -{ - TaskListModel* task_list_model = nullptr; QModelIndex current = d_ptr->tasks_view->currentIndex(); - if (!current.isValid()) { - // create new TaskDisplay - TaskDisplay *display = new TaskDisplay(); - display->setName("Motion Planning Task"); - vis_manager_->getRootDisplayGroup()->addDisplay(display); - display->initialize(vis_manager_); - display->setEnabled(true); + if (!current.isValid()) return; + bool is_top_level = !current.parent().isValid(); - task_list_model = &display->getTaskListModel(); - } else - task_list_model = d_ptr->getTaskListModel(current).first; + TaskListModel* task_list_model = d_ptr->getTaskListModel(current).first; + task_list_model->insertModel(new LocalTaskModel(task_list_model), is_top_level ? -1 : current.row()); - task_list_model->insertModel(new LocalTaskModel(task_list_model), current.row()); + // select and edit newly inserted model + if (is_top_level) current = current.model()->index(task_list_model->rowCount()-1, 0, current); + d_ptr->tasks_view->scrollTo(current); + d_ptr->tasks_view->setCurrentIndex(current); + d_ptr->tasks_view->edit(current); } -void TaskPanel::showStageDockWidget() -{ - rviz::PanelDockWidget *dock = getStageDockWidget(d_ptr->window_manager_); - if (dock) dock->show(); -} - -void TaskPanel::removeSelectedStages() +void TaskView::removeSelectedStages() { auto *m = d_ptr->tasks_view->model(); for (const auto &range : d_ptr->tasks_view->selectionModel()->selection()) m->removeRows(range.top(), range.bottom()-range.top()+1, range.parent()); } -void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous) +void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous) { + // adding task is allowed on top-level items and sub-top-level items + d_ptr->actionAddLocalTask->setEnabled(current.isValid() && + (!current.parent().isValid() || !current.parent().parent().isValid())); + // removing stuff is allowed if there is any selection / any curren item + d_ptr->actionRemoveTaskTreeRows->setEnabled(current.isValid()); + BaseTaskModel *task; QModelIndex task_index; std::tie(task, task_index) = d_ptr->getTaskModel(current); - d_ptr->actionRemoveTaskTreeRows->setEnabled(task != nullptr); - // TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second; - d_ptr->unlock(nullptr); + d_ptr->lock(nullptr); // unlocks any locked_display_ - auto *view = d_ptr->solutions_view; + // update the SolutionModel + QTreeView *view = d_ptr->solutions_view; QItemSelectionModel *sm = view->selectionModel(); QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr; view->sortByColumn(-1); @@ -253,12 +269,19 @@ void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIn connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)), this, SLOT(onSolutionSelectionChanged(QItemSelection, QItemSelection))); } + + // update the PropertyModel + view = d_ptr->property_view; + sm = view->selectionModel(); + m = task ? task->getPropertyModel(task_index) : nullptr; + view->setModel(m); + delete sm; // we don't store the selection model } -void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous) +void TaskView::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous) { TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second; - d_ptr->unlock(display); + d_ptr->lock(display); if (!display || !current.isValid()) return; @@ -266,7 +289,6 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode BaseTaskModel *task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first; Q_ASSERT(task); - d_ptr->locked_display_ = display; TaskSolutionVisualization* vis = display->visualization(); const DisplaySolutionPtr& solution = task->getSolution(current); display->setSolutionStatus(bool(solution)); @@ -274,7 +296,7 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode vis->showTrajectory(solution, true); } -void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected) +void TaskView::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected) { QItemSelectionModel *sm = d_ptr->solutions_view->selectionModel(); const QModelIndexList& selected_rows = sm->selectedRows(); @@ -292,6 +314,19 @@ void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const } } + +TaskSettingsPrivate::TaskSettingsPrivate(TaskSettings *q_ptr) + : q_ptr(q_ptr) +{ + setupUi(q_ptr); +} + +TaskSettings::TaskSettings(QWidget *parent) + : QWidget(parent), d_ptr(new TaskSettingsPrivate(this)) +{ + Q_D(TaskSettings); +} + } #include "moc_task_panel.cpp" diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index ace98e0d..845712f9 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -82,9 +82,23 @@ public: void load(const rviz::Config& config) override; void save(rviz::Config config) const override; +protected Q_SLOTS: + void showStageDockWidget(); +}; + + +class MetaTaskListModel; +class TaskViewPrivate; +class TaskView : public QWidget { + Q_OBJECT + Q_DECLARE_PRIVATE(TaskView) + TaskViewPrivate *d_ptr; + +public: + TaskView(QWidget* parent = 0); + public Q_SLOTS: void addTask(); - void showStageDockWidget(); protected Q_SLOTS: void removeSelectedStages(); @@ -93,4 +107,15 @@ protected Q_SLOTS: void onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected); }; + +class TaskSettingsPrivate; +class TaskSettings : public QWidget { + Q_OBJECT + Q_DECLARE_PRIVATE(TaskSettings) + TaskSettingsPrivate *d_ptr; + +public: + TaskSettings(QWidget* parent = 0); +}; + } diff --git a/visualization/motion_planning_tasks/src/task_panel.ui b/visualization/motion_planning_tasks/src/task_panel.ui index 7fa9a4d6..8d178229 100644 --- a/visualization/motion_planning_tasks/src/task_panel.ui +++ b/visualization/motion_planning_tasks/src/task_panel.ui @@ -14,9 +14,6 @@ Tasks Panel - - 0 - 0 @@ -30,193 +27,49 @@ 0 - - - 0 - - - - - :/icons/tasks.png:/icons/tasks.png - - - - - - - 0 + + + + + ... - - 0 + + + :/icons/settings.png:/icons/settings.png - - 0 + + true - - 0 + + + + + + Qt::Horizontal - - 0 + + + 40 + 20 + - - - - 6 - - - - - Task Tree - - - - - - - - 18 - 18 - - - - - - - - :/icons/new-stage.png:/icons/new-stage.png - - - - - - - - - - 0 - 0 - - - - Qt::Horizontal - - - - - 2 - 0 - - - - Qt::ActionsContextMenu - - - false - - - - - - 1 - 0 - - - - QAbstractItemView::ExtendedSelection - - - false - - - true - - - true - - - false - - - - - - - - - - :/icons/settings.png:/icons/settings.png - - - - - - - 0 + + + + + + ... - - 0 + + + :/icons/new-stage.png:/icons/new-stage.png - - 0 - - - 0 - - - 0 - - - - - Settings - - - - - - - - - + + + - - - Remove - - - Remove selected rows - - - Del - - - Qt::WidgetShortcut - - - - - Add task - - - - - rviz::PropertyTreeWidget - QTreeView -
rviz/properties/property_tree_widget.h
-
- - moveit_rviz_plugin::TaskListView - QTreeView -
task_list_model.h
-
- - moveit_rviz_plugin::AutoAdjustingTreeView - QTreeView -
task_list_model.h
-
-
diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index af56ae55..3cba49b6 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -40,6 +40,8 @@ #include "task_panel.h" #include "ui_task_panel.h" +#include "ui_task_view.h" +#include "ui_task_settings.h" #include #include @@ -55,7 +57,19 @@ class TaskPanelPrivate : public Ui_TaskPanel { public: TaskPanelPrivate(TaskPanel *q_ptr); - void initSettings(rviz::Property *root); + TaskPanel* q_ptr; + TaskView* tasks_widget; + TaskSettings* settings_widget; + + rviz::WindowManagerInterface* window_manager_; + static QPointer global_instance_; + static uint global_use_count_; +}; + + +class TaskViewPrivate : public Ui_TaskView { +public: + TaskViewPrivate(TaskView *q_ptr); /// retrieve TaskListModel corresponding to given index inline std::pair @@ -66,15 +80,18 @@ public: getTaskModel(const QModelIndex& index) const; /// unlock locked_display_ if given display is different - void unlock(TaskDisplay *display); + void lock(TaskDisplay *display); - TaskPanel* q_ptr; - rviz::PropertyTreeModel* settings; + TaskView *q_ptr; QPointer locked_display_; - rviz::WindowManagerInterface* window_manager_; +}; - static QPointer global_instance_; - static uint global_use_count_; + +class TaskSettingsPrivate : public Ui_TaskSettings { +public: + TaskSettingsPrivate(TaskSettings *q_ptr); + + TaskSettings *q_ptr; }; } diff --git a/visualization/motion_planning_tasks/src/task_settings.ui b/visualization/motion_planning_tasks/src/task_settings.ui new file mode 100644 index 00000000..3a76384c --- /dev/null +++ b/visualization/motion_planning_tasks/src/task_settings.ui @@ -0,0 +1,69 @@ + + + moveit_rviz_plugin::TaskSettings + + + + 0 + 0 + 400 + 300 + + + + Settings + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Settings + + + + + + + + + + + + + rviz::PropertyTreeWidget + QTreeView +
rviz/properties/property_tree_widget.h
+
+
+ + +
diff --git a/visualization/motion_planning_tasks/src/task_view.ui b/visualization/motion_planning_tasks/src/task_view.ui new file mode 100644 index 00000000..8ae8aa59 --- /dev/null +++ b/visualization/motion_planning_tasks/src/task_view.ui @@ -0,0 +1,157 @@ + + + moveit_rviz_plugin::TaskView + + + + 0 + 0 + 400 + 300 + + + + Tasks Panel + + + + 0 + + + 0 + + + 0 + + + 0 + + + + + Qt::Vertical + + + + + 0 + + + + + Task Tree + + + + + + + + 0 + 0 + + + + Qt::Horizontal + + + + + 2 + 0 + + + + Qt::ActionsContextMenu + + + false + + + + + + 1 + 0 + + + + QAbstractItemView::ExtendedSelection + + + false + + + true + + + true + + + false + + + + + + + + + + 0 + + + + + Properties + + + + + + + + + + + + + + Remove + + + Remove selected rows + + + Del + + + Qt::WidgetShortcut + + + + + Add task + + + + + + moveit_rviz_plugin::TaskListView + QTreeView +
task_list_model.h
+
+ + moveit_rviz_plugin::AutoAdjustingTreeView + QTreeView +
task_list_model.h
+
+ + rviz::PropertyTreeWidget + QTreeView +
rviz/properties/property_tree_widget.h
+
+
+ + +