Merge branches 'cleanup', 'wip-modular-planning', 'wip-containers', 'wip-gui' and 'wip-modify-ps'

This commit is contained in:
Robert Haschke 2018-02-12 23:54:51 +01:00
49 changed files with 2915 additions and 594 deletions

View File

@ -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<TraverseDirection dir>
template<Interface::Direction dir>
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<WrapperBase*>(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<SolutionBase>&& s);
void spawn(InterfaceState &&state, double cost) {
std::unique_ptr<SolutionBase> 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;

View File

@ -39,7 +39,6 @@
#pragma once
#include <moveit/task_constructor/container.h>
#include "utils.h"
#include "stage_p.h"
#include <map>
@ -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<const InterfaceState*, Interface::iterator> 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<SerialSolution>& 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<SerialSolution> 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<WrappedSolution> wrapped_solutions_;
// buffer for newly created (not wrapped) solutions
std::list<SubTrajectory> created_solutions_;
// buffer of created states (for use in created solutions)
std::list<InterfaceState> states_;
// cost-ordered set of solutions (pointers into wrapped or created)
ordered<SolutionBase*> solutions_;
// buffer for failures (pointers into wrapped or created)
std::list<SolutionBase*> 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<std::unique_ptr<SolutionBase>, pointerLessThan<std::unique_ptr<SolutionBase>>> solutions_;
std::list<std::unique_ptr<SolutionBase>> failures_;
std::list<InterfaceState> failure_states_;
};
PIMPL_FUNCTIONS(Wrapper)
} }

View File

@ -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 <moveit/task_constructor/solvers/planner_interface.h>
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;
};
} } }

View File

@ -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 <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/macros/class_forward.h>
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_;
};
} } }

View File

@ -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 <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h>
#include <Eigen/Geometry>
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);
} } }

View File

@ -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) {

View File

@ -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());

View File

@ -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 <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h>
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_;
};
} } }

View File

@ -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 <moveit/task_constructor/stage.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Pose.h>
#include <deque>
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<visualization_msgs::Marker>& markers) const;
void fixCollision(planning_scene::PlanningScene &scene, geometry_msgs::Pose pose, const std::string& object) const;
};
} } }

View File

@ -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 <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/type_traits.h>
#include <map>
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<std::string> Names;
typedef std::function<void(planning_scene::PlanningScenePtr scene, PropertyMap& properties)> 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 <typename T, typename E = typename std::enable_if_t<is_container<T>::value && std::is_base_of<std::string, typename T::value_type>::value>>
inline void attachObjects(const T& objects, const std::string& link) {
attachObjects(Names(objects.cbegin(), objects.cend()), link, true);
}
template <typename T, typename E = typename std::enable_if_t<is_container<T>::value && std::is_base_of<std::string, typename T::value_type>::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 <typename T1, typename T2,
typename E1 = typename std::enable_if_t<is_container<T1>::value && std::is_base_of<std::string, typename T1::value_type>::value>,
typename E2 = typename std::enable_if_t<is_container<T2>::value && std::is_base_of<std::string, typename T2::value_type>::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 <typename T, typename E = typename std::enable_if_t<is_container<T>::value && std::is_base_of<std::string, typename T::value_type>::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<std::string, std::pair<Names, bool> > attach_objects_;
// list of objects to mutually
struct CollisionMatrixPairs {
Names first;
Names second;
bool enable;
};
std::list<CollisionMatrixPairs> 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<std::string, std::pair<Names, bool> >& 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);
}
} } }

View File

@ -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 <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
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_;
};
} } }

View File

@ -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 <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
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_;
};
} } }

View File

@ -142,6 +142,7 @@ private:
/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
class Interface : public ordered<InterfaceState> {
public:
enum Direction { FORWARD, BACKWARD, START=FORWARD, END=BACKWARD };
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
Interface(const NotifyFunction &notify = NotifyFunction());
@ -170,6 +171,9 @@ public:
inline const InterfaceState* start() const { return start_; }
inline const InterfaceState* end() const { return end_; }
template <Interface::Direction dir>
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 <TraverseDirection dir>
const InterfaceState::Solutions& trajectories(const SolutionBase &start);
template <> inline
const InterfaceState::Solutions& trajectories<FORWARD>(const SolutionBase &solution) {
return solution.end()->outgoingTrajectories();
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
return end_->outgoingTrajectories();
}
template <> inline
const InterfaceState::Solutions& trajectories<BACKWARD>(const SolutionBase &solution) {
return solution.start()->incomingTrajectories();
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::BACKWARD>() const {
return start_->incomingTrajectories();
}
} }
namespace std {
// comparison for pointers to SolutionBase: compare based on value
template<> struct less<moveit::task_constructor::SolutionBase*> {
bool operator()(const moveit::task_constructor::SolutionBase* x,
const moveit::task_constructor::SolutionBase* y) {
return *x < *y;
}
};
}

View File

@ -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;

View File

@ -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 <type_traits>
namespace moveit { namespace task_constructor {
// detect STL-like containers by presence of cbegin(), cend() methods
template<typename T, typename _ = void>
struct is_container : std::false_type {};
template<typename... Ts>
struct is_container_helper {};
template<typename T>
struct is_container<
T,
std::conditional_t<
false,
is_container_helper<
typename T::const_iterator,
decltype(std::declval<T>().cbegin()),
decltype(std::declval<T>().cend())
>,
void
>
> : public std::true_type {};
} }

View File

@ -94,12 +94,3 @@ private:
};
#define DECLARE_FLAGS(Flags, Enum) typedef QFlags<Enum> Flags;
// compare two pointers by comparing their referenced values
template <typename T>
struct pointerLessThan : std::enable_if<std::is_pointer<T>::value> {
inline bool operator()(const T& x, const T& y) const {
return *x < *y;
}
};

View File

@ -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}

View File

@ -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<ContainerBase*>(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);
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 &current)
// find all incoming solution pathes ending at current solution
SolutionCollector incoming(num_before);
traverse<BACKWARD>(current, std::ref(incoming), trace);
traverse<Interface::BACKWARD>(current, std::ref(incoming), trace);
// find all outgoing solution pathes starting at current solution
SolutionCollector outgoing(num_after);
traverse<FORWARD>(current, std::ref(outgoing), trace);
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
// collect (and sort) all solutions spanning from start to end of this container
ordered<SerialSolution> sorted;
@ -268,42 +295,11 @@ void SerialContainer::onNewSolution(const SolutionBase &current)
}
// store new solutions (in sorted)
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it)
impl->storeNewSolution(std::move(*it));
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);
}
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));
}
// 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 <TraverseDirection dir>
template <Interface::Direction dir>
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
solution_container &trace, double trace_cost)
{
const InterfaceState::Solutions& solutions = trajectories<dir>(start);
const InterfaceState::Solutions& solutions = start.trajectories<dir>();
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;
}
size_t ParallelContainerBase::numSolutions() const
{
return pimpl()->solutions_.size();
}
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)
{
// update state priorities
InterfaceState::Priority prio(1, s.cost());
InterfaceState* start = const_cast<InterfaceState*>(s.start());
start->owner()->updatePriority(*start, prio);
InterfaceState* end = const_cast<InterfaceState*>(s.end());
end->owner()->updatePriority(*end, prio);
liftSolution(&s);
}
pimpl()->newSolution(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<InterfaceState*>(s.start());
start->owner()->updatePriority(*start, prio);
InterfaceState* end = const_cast<InterfaceState*>(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<SolutionBase>&& 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();
}
} }

View File

@ -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 <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
namespace moveit { namespace task_constructor { namespace solvers {
CartesianPath::CartesianPath()
{
auto& p = properties();
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("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<const robot_state::RobotState&>(*state), jmg->getName());
};
std::vector<moveit::core::RobotStatePtr> trajectory;
double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath(
jmg, trajectory, &link, target, true,
props.get<double>("step_size"),
props.get<double>("jump_threshold"),
isValid);
if (achieved_fraction >= props.get<double>("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<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return true;
}
return false;
}
} } }

View File

@ -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 <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit/kinematic_constraints/utils.h>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace solvers {
PipelinePlanner::PipelinePlanner()
{
auto& p = properties();
p.declare<std::string>("planner", "", "planner id");
p.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
p.declare<moveit_msgs::WorkspaceParameters>("workspace_parameters", moveit_msgs::WorkspaceParameters(), "allowed workspace of mobile base?");
p.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
p.declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
p.declare<double>("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<std::string>("planner");
req.allowed_planning_time = timeout;
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("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<double>("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<double>("goal_position_tolerance"),
props.get<double>("goal_orientation_tolerance"));
::planning_interface::MotionPlanResponse res;
if(!planner_->generatePlan(from, req, res))
return false;
result = res.trajectory_;
return true;
}
} } }

View File

@ -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 <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/robot_model/joint_model_group.h>
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();
}
} } }

View File

@ -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;
}

View File

@ -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})

View File

@ -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<double>("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<SolutionBase> 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<double>::infinity());
solution.setCost(std::numeric_limits<double>::infinity());
}
spawn(InterfaceState(scene), std::move(solution));

View File

@ -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 <moveit/task_constructor/stages/connect.h>
#include <moveit/planning_scene/planning_scene.h>
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<double>("timeout", 10.0, "planning timeout");
p.declare<std::string>("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<std::string>("group");
double timeout = props.get<double>("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;
}
} } }

View File

@ -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 <moveit/task_constructor/stages/fix_collision_objects.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <ros/console.h>
namespace vm = visualization_msgs;
namespace moveit { namespace task_constructor { namespace stages {
FixCollisionObjects::FixCollisionObjects(const std::string &name)
: PropagatingEitherWay(name)
{
auto& p = properties();
p.declare<double>("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<double>::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<double>::infinity());
sendBackward(InterfaceState(from), to, std::move(solution));
return success;
}
bool FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene, std::deque<visualization_msgs::Marker> &markers) const
{
const auto& props = properties();
double penetration = props.get<double>("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"<<std::endl;
}
} } }

View File

@ -0,0 +1,130 @@
/*********************************************************************
* 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
*/
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.h>
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<std::string, std::pair<Names, bool> >& 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;
}
} } }

View File

@ -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 <moveit/task_constructor/stages/move_relative.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
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<double>("timeout", 10.0, "planning timeout");
p.declare<std::string>("marker_ns", "", "marker namespace");
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
p.declare<double>("min_distance", "minimum distance to move");
p.declare<double>("max_distance", "maximum distance to move");
p.declare<geometry_msgs::TwistStamped>("twist", "Cartesian twist transform");
p.declare<geometry_msgs::Vector3Stamped>("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<double>("timeout");
const std::string& group = props.get<std::string>("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<std::string>("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<double>(prop.value()));
prop = props.property("min_distance");
double min_distance = prop.value().empty() ? -1.0 : boost::any_cast<double>(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<geometry_msgs::TwistStamped>(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<double>::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<double>::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<geometry_msgs::Vector3Stamped>(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<std::string>("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<double>::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<double>::infinity());
sendBackward(InterfaceState(from), to, std::move(trajectory));
return success;
}
} } }

193
core/src/stages/move_to.cpp Normal file
View File

@ -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 <moveit/task_constructor/stages/move_to.h>
#include <moveit/planning_scene/planning_scene.h>
#include <eigen_conversions/eigen_msg.h>
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<double>("timeout", 10.0, "planning timeout");
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
p.declare<geometry_msgs::PoseStamped>("pose", "Cartesian target pose");
p.declare<geometry_msgs::PointStamped>("point", "Cartesian target point");
p.declare<std::string>("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<double>("timeout");
const std::string& group = props.get<std::string>("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<std::string>(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<std::string>("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<geometry_msgs::PoseStamped>(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<geometry_msgs::PointStamped>(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<double>::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<double>::infinity());
sendBackward(InterfaceState(from), to, std::move(trajectory));
return success;
}
} } }

View File

@ -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);
}

View File

@ -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;

View File

@ -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);
}

View File

@ -1,3 +1,4 @@
add_subdirectory(utils)
add_subdirectory(properties)
add_subdirectory(src)
add_subdirectory(test)

View File

@ -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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
)
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

View File

@ -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 <boost/functional/factory.hpp>
#include <moveit/task_constructor/properties.h>
#include <rviz/properties/property_tree_model.h>
#include <rviz/properties/string_property.h>
#include <rviz/properties/float_property.h>
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 <typename T, typename RVIZProp>
RVIZProp* helper(const QString& name, Property* prop) {
T value = prop->defined() ? boost::any_cast<T>(prop->value()) : T();
return new RVIZProp(name, value, QString::fromStdString(prop->description()));
}
PropertyFactory::PropertyFactory()
{
// registe some standard types
registerType<double>(&helper<double, rviz::FloatProperty>);
registerType<QString>(&helper<QString, rviz::StringProperty>);
}
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;
}
}

View File

@ -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 <QObject>
#include <QString>
#include <map>
#include <functional>
#include <typeindex>
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<rviz::Property*(const QString& name, moveit::task_constructor::Property*)> FactoryFunction;
/// register a new factory function for type T
template <typename T>
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<std::string, FactoryFunction> 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);
}

View File

@ -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}

View File

@ -36,6 +36,7 @@
#include "local_task_model.h"
#include "factory_model.h"
#include "properties/property_factory.h"
#include <moveit/task_constructor/container_p.h>
#include <ros/console.h>
@ -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;
}
}

View File

@ -49,6 +49,7 @@ class LocalTaskModel
typedef moveit::task_constructor::StagePrivate Node;
Node *root_;
StageFactoryPtr stage_factory_;
std::map<Node*, rviz::PropertyTreeModel*> 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;
};
}

View File

@ -40,6 +40,8 @@
#include <moveit/task_constructor/container.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_task_constructor_msgs/GetSolution.h>
#include <rviz/properties/property_tree_model.h>
#include <rviz/properties/property.h>
#include <ros/console.h>
#include <ros/service_client.h>
@ -64,9 +66,11 @@ struct RemoteTaskModel::Node {
InterfaceFlags interface_flags_;
NodeFlags node_flags_;
std::unique_ptr<RemoteSolutionModel> solutions_;
std::unique_ptr<rviz::PropertyTreeModel> 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 <class T>

View File

@ -87,6 +87,8 @@ public:
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
};

View File

@ -264,7 +264,7 @@ void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last
TaskListModel* m = static_cast<TaskListModel*>(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);
}
}

View File

@ -50,6 +50,7 @@
#include <memory>
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;
};

View File

@ -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<TaskListModel*, TaskDisplay*>
TaskPanelPrivate::getTaskListModel(const QModelIndex &index) const
TaskViewPrivate::getTaskListModel(const QModelIndex &index) const
{
auto *meta_model = static_cast<MetaTaskListModel*>(tasks_view->model());
return meta_model->getTaskListModel(index);
}
std::pair<BaseTaskModel*, QModelIndex>
TaskPanelPrivate::getTaskModel(const QModelIndex &index) const
TaskViewPrivate::getTaskModel(const QModelIndex &index) const
{
auto *meta_model = static_cast<MetaTaskListModel*>(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;
}
TaskView::TaskView(QWidget *parent)
: QWidget(parent), d_ptr(new TaskViewPrivate(this))
{
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::onInitialize()
void TaskView::addTask()
{
d_ptr->window_manager_ = vis_manager_->getWindowManager();
}
void TaskPanel::save(rviz::Config config) const
{
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 &current, const QModelIndex &previous)
void TaskView::onCurrentStageChanged(const QModelIndex &current, 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 &current, 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 &current, const QModelIndex &previous)
void TaskView::onCurrentSolutionChanged(const QModelIndex &current, 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 &current, 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 &current, 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"

View File

@ -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);
};
}

View File

@ -14,9 +14,6 @@
<string>Tasks Panel</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
@ -30,56 +27,38 @@
<number>0</number>
</property>
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="tab_tasks">
<attribute name="icon">
<iconset resource="resources.qrc">
<normaloff>:/icons/tasks.png</normaloff>:/icons/tasks.png</iconset>
</attribute>
<attribute name="title">
<string/>
</attribute>
<layout class="QVBoxLayout" name="tab_tasks_layout">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<layout class="QHBoxLayout" name="tasks_view_label_layout">
<property name="spacing">
<number>6</number>
</property>
<item>
<widget class="QLabel" name="tasks_view_label">
<widget class="QToolButton" name="button_show_settings">
<property name="text">
<string>Task Tree</string>
<string>...</string>
</property>
<property name="icon">
<iconset resource="resources.qrc">
<normaloff>:/icons/settings.png</normaloff>:/icons/settings.png</iconset>
</property>
<property name="checkable">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QToolButton" name="button_show_stage_dock_widget">
<property name="maximumSize">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>18</width>
<height>18</height>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QToolButton" name="button_show_stage_dock_widget">
<property name="text">
<string/>
<string>...</string>
</property>
<property name="icon">
<iconset resource="resources.qrc">
@ -89,134 +68,8 @@
</item>
</layout>
</item>
<item>
<widget class="QSplitter" name="splitter">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<widget class="moveit_rviz_plugin::TaskListView" name="tasks_view">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>2</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="contextMenuPolicy">
<enum>Qt::ActionsContextMenu</enum>
</property>
<attribute name="headerStretchLastSection">
<bool>false</bool>
</attribute>
</widget>
<widget class="moveit_rviz_plugin::AutoAdjustingTreeView" name="solutions_view">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>1</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="selectionMode">
<enum>QAbstractItemView::ExtendedSelection</enum>
</property>
<property name="rootIsDecorated">
<bool>false</bool>
</property>
<property name="uniformRowHeights">
<bool>true</bool>
</property>
<property name="sortingEnabled">
<bool>true</bool>
</property>
<attribute name="headerStretchLastSection">
<bool>false</bool>
</attribute>
</widget>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab_settings">
<attribute name="icon">
<iconset resource="resources.qrc">
<normaloff>:/icons/settings.png</normaloff>:/icons/settings.png</iconset>
</attribute>
<attribute name="title">
<string/>
</attribute>
<layout class="QVBoxLayout" name="tab_settings_layout">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="settings_view_label">
<property name="text">
<string>Settings</string>
</property>
</widget>
</item>
<item>
<widget class="rviz::PropertyTreeWidget" name="settings_view"/>
</item>
</layout>
</widget>
</widget>
</item>
</layout>
<action name="actionRemoveTaskTreeRows">
<property name="text">
<string>Remove</string>
</property>
<property name="toolTip">
<string>Remove selected rows</string>
</property>
<property name="shortcut">
<string>Del</string>
</property>
<property name="shortcutContext">
<enum>Qt::WidgetShortcut</enum>
</property>
</action>
<action name="actionAddLocalTask">
<property name="text">
<string>Add task</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
<class>rviz::PropertyTreeWidget</class>
<extends>QTreeView</extends>
<header location="global">rviz/properties/property_tree_widget.h</header>
</customwidget>
<customwidget>
<class>moveit_rviz_plugin::TaskListView</class>
<extends>QTreeView</extends>
<header>task_list_model.h</header>
</customwidget>
<customwidget>
<class>moveit_rviz_plugin::AutoAdjustingTreeView</class>
<extends>QTreeView</extends>
<header>task_list_model.h</header>
</customwidget>
</customwidgets>
<resources>
<include location="resources.qrc"/>
</resources>

View File

@ -40,6 +40,8 @@
#include "task_panel.h"
#include "ui_task_panel.h"
#include "ui_task_view.h"
#include "ui_task_settings.h"
#include <rviz/panel.h>
#include <rviz/properties/property_tree_model.h>
@ -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<TaskPanel> 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<TaskListModel*, TaskDisplay*>
@ -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<TaskDisplay> locked_display_;
rviz::WindowManagerInterface* window_manager_;
};
static QPointer<TaskPanel> global_instance_;
static uint global_use_count_;
class TaskSettingsPrivate : public Ui_TaskSettings {
public:
TaskSettingsPrivate(TaskSettings *q_ptr);
TaskSettings *q_ptr;
};
}

View File

@ -0,0 +1,69 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>moveit_rviz_plugin::TaskSettings</class>
<widget class="QWidget" name="moveit_rviz_plugin::TaskSettings">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<height>300</height>
</rect>
</property>
<property name="windowTitle">
<string>Settings</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<layout class="QVBoxLayout" name="tab_settings_layout">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="settings_view_label">
<property name="text">
<string>Settings</string>
</property>
</widget>
</item>
<item>
<widget class="rviz::PropertyTreeWidget" name="settings_view"/>
</item>
</layout>
</item>
</layout>
</widget>
<customwidgets>
<customwidget>
<class>rviz::PropertyTreeWidget</class>
<extends>QTreeView</extends>
<header location="global">rviz/properties/property_tree_widget.h</header>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>

View File

@ -0,0 +1,157 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>moveit_rviz_plugin::TaskView</class>
<widget class="QWidget" name="moveit_rviz_plugin::TaskView">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>400</width>
<height>300</height>
</rect>
</property>
<property name="windowTitle">
<string>Tasks Panel</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
<widget class="QSplitter" name="tasks_property_splitter">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<widget class="QWidget" name="">
<layout class="QVBoxLayout" name="verticalLayout1">
<property name="spacing">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="tasks_view_label">
<property name="text">
<string>Task Tree</string>
</property>
</widget>
</item>
<item>
<widget class="QSplitter" name="tasks_solutions_splitter">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<widget class="moveit_rviz_plugin::TaskListView" name="tasks_view">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>2</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="contextMenuPolicy">
<enum>Qt::ActionsContextMenu</enum>
</property>
<attribute name="headerStretchLastSection">
<bool>false</bool>
</attribute>
</widget>
<widget class="moveit_rviz_plugin::AutoAdjustingTreeView" name="solutions_view">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Expanding">
<horstretch>1</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="selectionMode">
<enum>QAbstractItemView::ExtendedSelection</enum>
</property>
<property name="rootIsDecorated">
<bool>false</bool>
</property>
<property name="uniformRowHeights">
<bool>true</bool>
</property>
<property name="sortingEnabled">
<bool>true</bool>
</property>
<attribute name="headerStretchLastSection">
<bool>false</bool>
</attribute>
</widget>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="">
<layout class="QVBoxLayout" name="verticalLayout2">
<property name="spacing">
<number>0</number>
</property>
<item>
<widget class="QLabel" name="property_view_label">
<property name="text">
<string>Properties</string>
</property>
</widget>
</item>
<item>
<widget class="rviz::PropertyTreeWidget" name="property_view"/>
</item>
</layout>
</widget>
</widget>
</item>
</layout>
<action name="actionRemoveTaskTreeRows">
<property name="text">
<string>Remove</string>
</property>
<property name="toolTip">
<string>Remove selected rows</string>
</property>
<property name="shortcut">
<string>Del</string>
</property>
<property name="shortcutContext">
<enum>Qt::WidgetShortcut</enum>
</property>
</action>
<action name="actionAddLocalTask">
<property name="text">
<string>Add task</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
<class>moveit_rviz_plugin::TaskListView</class>
<extends>QTreeView</extends>
<header>task_list_model.h</header>
</customwidget>
<customwidget>
<class>moveit_rviz_plugin::AutoAdjustingTreeView</class>
<extends>QTreeView</extends>
<header>task_list_model.h</header>
</customwidget>
<customwidget>
<class>rviz::PropertyTreeWidget</class>
<extends>QTreeView</extends>
<header location="global">rviz/properties/property_tree_widget.h</header>
</customwidget>
</customwidgets>
<resources/>
<connections/>
</ui>