mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'cleanup', 'wip-modular-planning', 'wip-containers', 'wip-gui' and 'wip-modify-ps'
This commit is contained in:
commit
4be373592a
@ -108,7 +108,7 @@ protected:
|
|||||||
/// until the end, i.e. until there are no more subsolutions in the given direction
|
/// until the end, i.e. until there are no more subsolutions in the given direction
|
||||||
/// For each solution path, callback the given processor passing
|
/// For each solution path, callback the given processor passing
|
||||||
/// the full trace (from start to end, but not including start) and its accumulated costs
|
/// 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,
|
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||||
solution_container &trace, double trace_cost = 0);
|
solution_container &trace, double trace_cost = 0);
|
||||||
|
|
||||||
@ -135,102 +135,101 @@ public:
|
|||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const planning_scene::PlanningSceneConstPtr &scene) 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:
|
protected:
|
||||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||||
|
|
||||||
|
/// method called if any child found a new (internal) solution
|
||||||
virtual void onNewSolution(const SolutionBase& s) override;
|
virtual void onNewSolution(const SolutionBase& s) override;
|
||||||
|
|
||||||
/// callback for new start states (received externally)
|
/// lift unmodified child solution (useful for simple filtering)
|
||||||
virtual void onNewStartState(Interface::iterator external, bool updated) = 0;
|
void liftSolution(const SolutionBase* solution) {
|
||||||
/// callback for new end states (received externally)
|
liftSolution(solution, solution->cost());
|
||||||
virtual void onNewEndState(Interface::iterator external, bool updated) = 0;
|
}
|
||||||
|
/// 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
|
* Solution of all children are reported - sorted by cost.
|
||||||
* for new new clones of start / end states, which in turn will
|
*/
|
||||||
* have separate incoming/outgoing trajectories */
|
class Alternatives : public ParallelContainerBase
|
||||||
class WrappedSolution : public SolutionBase {
|
{
|
||||||
public:
|
public:
|
||||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped)
|
Alternatives(const std::string &name) : ParallelContainerBase(name) {}
|
||||||
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
|
|
||||||
{}
|
|
||||||
void fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
|
||||||
Introspection* introspection = nullptr) const override {
|
|
||||||
wrapped_->fillMessage(solution, introspection);
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
bool canCompute() const override;
|
||||||
const SolutionBase* wrapped_;
|
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;
|
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.
|
* Implementations of this interface need to implement onNewSolution(), which is
|
||||||
* This child can be accessed via wrapped().
|
* 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:
|
public:
|
||||||
PRIVATE_CLASS(WrapperBase)
|
PRIVATE_CLASS(WrapperBase)
|
||||||
WrapperBase(const std::string &name, pointer &&child = Stage::pointer());
|
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
|
/// insertion is only allowed if children() is empty
|
||||||
bool insert(Stage::pointer&& stage, int before = -1) override;
|
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();
|
Stage* wrapped();
|
||||||
inline const Stage* wrapped() const {
|
inline const Stage* wrapped() const {
|
||||||
return const_cast<WrapperBase*>(this)->wrapped();
|
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 canCompute() const override;
|
||||||
bool compute() 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:
|
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
|
/// called by a (direct) child when a new solution becomes available
|
||||||
void onNewSolution(const SolutionBase &s) override = 0;
|
void onNewSolution(const SolutionBase &s) override = 0;
|
||||||
|
|||||||
@ -39,7 +39,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/task_constructor/container.h>
|
#include <moveit/task_constructor/container.h>
|
||||||
#include "utils.h"
|
|
||||||
#include "stage_p.h"
|
#include "stage_p.h"
|
||||||
|
|
||||||
#include <map>
|
#include <map>
|
||||||
@ -47,13 +46,16 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
/* A container needs to decouple its interfaces from those of its children.
|
/* A container needs to decouple its own (external) interfaces
|
||||||
* Both, the container and the children have their own starts_ and ends_.
|
* from those (internal) of its children.
|
||||||
* The container needs to forward states received in its interfaces to
|
* Both, the container and the children have their own pull interfaces: starts_ and ends_.
|
||||||
* the interfaces of the children.
|
* The container forwards states received in its pull interfaces to the
|
||||||
* Solutions found by the children, then need to be connected to the
|
* corresponding interfaces of the children.
|
||||||
* container's interface states. To this end, we remember the mapping
|
* States pushed by children are temporarily stored in pending_backward_ and pending_forward_.
|
||||||
* from internal to external states.
|
*
|
||||||
|
* 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.
|
* Note, that there might be many solutions connecting a single start-end pair.
|
||||||
* These solutions might origin from different children (ParallelContainer)
|
* These solutions might origin from different children (ParallelContainer)
|
||||||
* or from different solution paths in a SerialContainer.
|
* or from different solution paths in a SerialContainer.
|
||||||
@ -92,19 +94,40 @@ public:
|
|||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
|
|
||||||
|
InterfacePtr pendingBackward() const { return pending_backward_; }
|
||||||
|
InterfacePtr pendingForward() const { return pending_forward_; }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
ContainerBasePrivate(ContainerBase *me, const std::string &name)
|
ContainerBasePrivate(ContainerBase *me, const std::string &name);
|
||||||
: StagePrivate(me, 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
|
/// 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:
|
protected:
|
||||||
container_type children_;
|
container_type children_;
|
||||||
|
|
||||||
// map start/end states of children (internal) to corresponding states in our external interfaces
|
// map start/end states of children (internal) to corresponding states in our external interfaces
|
||||||
std::map<const InterfaceState*, Interface::iterator> internal_to_external_;
|
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)
|
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.
|
* 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,
|
* 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.
|
* 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 {
|
class SerialContainerPrivate : public ContainerBasePrivate {
|
||||||
friend class SerialContainer;
|
friend class SerialContainer;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
SerialContainerPrivate(SerialContainer* me, const std::string &name);
|
SerialContainerPrivate(SerialContainer* me, const std::string &name);
|
||||||
|
|
||||||
void storeNewSolution(SerialSolution&& s);
|
|
||||||
const ordered<SerialSolution>& solutions() const { return solutions_; }
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void connect(StagePrivate *prev, StagePrivate *next);
|
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
|
// set of all solutions
|
||||||
ordered<SerialSolution> solutions_;
|
ordered<SerialSolution> solutions_;
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(SerialContainer)
|
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 {
|
class ParallelContainerBasePrivate : public ContainerBasePrivate {
|
||||||
friend class ParallelContainerBase;
|
friend class ParallelContainerBase;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name);
|
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)
|
PIMPL_FUNCTIONS(ParallelContainerBase)
|
||||||
|
|
||||||
|
|
||||||
class WrapperBasePrivate : public ContainerBasePrivate {
|
class WrapperBasePrivate : public ParallelContainerBasePrivate {
|
||||||
friend class WrapperBase;
|
friend class WrapperBase;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
WrapperBasePrivate(WrapperBase* me, const std::string& name);
|
WrapperBasePrivate(WrapperBase* me, const std::string& name);
|
||||||
|
|
||||||
private:
|
|
||||||
InterfacePtr dummy_starts_;
|
|
||||||
InterfacePtr dummy_ends_;
|
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(WrapperBase)
|
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)
|
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -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;
|
||||||
|
};
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -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_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -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);
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -80,6 +80,12 @@ public:
|
|||||||
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
||||||
inline InterfaceConstPtr nextStarts() const { return next_starts_.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
|
/// the following methods should be called only by a container
|
||||||
/// to setup the connection structure of their children
|
/// to setup the connection structure of their children
|
||||||
inline void setHierarchy(ContainerBase* parent, container_type::iterator it) {
|
inline void setHierarchy(ContainerBase* parent, container_type::iterator it) {
|
||||||
|
|||||||
@ -15,7 +15,7 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
class ComputeIK : public Wrapper {
|
class ComputeIK : public WrapperBase {
|
||||||
public:
|
public:
|
||||||
ComputeIK(const std::string &name, pointer &&child = Stage::pointer());
|
ComputeIK(const std::string &name, pointer &&child = Stage::pointer());
|
||||||
|
|
||||||
|
|||||||
57
core/include/moveit/task_constructor/stages/connect.h
Normal file
57
core/include/moveit/task_constructor/stages/connect.h
Normal 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_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -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;
|
||||||
|
};
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -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);
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
78
core/include/moveit/task_constructor/stages/move_relative.h
Normal file
78
core/include/moveit/task_constructor/stages/move_relative.h
Normal 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_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} } }
|
||||||
74
core/include/moveit/task_constructor/stages/move_to.h
Normal file
74
core/include/moveit/task_constructor/stages/move_to.h
Normal 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_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -142,6 +142,7 @@ private:
|
|||||||
/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
|
/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
|
||||||
class Interface : public ordered<InterfaceState> {
|
class Interface : public ordered<InterfaceState> {
|
||||||
public:
|
public:
|
||||||
|
enum Direction { FORWARD, BACKWARD, START=FORWARD, END=BACKWARD };
|
||||||
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
|
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
|
||||||
Interface(const NotifyFunction ¬ify = NotifyFunction());
|
Interface(const NotifyFunction ¬ify = NotifyFunction());
|
||||||
|
|
||||||
@ -170,6 +171,9 @@ public:
|
|||||||
inline const InterfaceState* start() const { return start_; }
|
inline const InterfaceState* start() const { return start_; }
|
||||||
inline const InterfaceState* end() const { return end_; }
|
inline const InterfaceState* end() const { return end_; }
|
||||||
|
|
||||||
|
template <Interface::Direction dir>
|
||||||
|
inline const InterfaceState::Solutions& trajectories() const;
|
||||||
|
|
||||||
inline void setStartState(const InterfaceState& state){
|
inline void setStartState(const InterfaceState& state){
|
||||||
assert(start_ == NULL); // only allow setting once (by Stage)
|
assert(start_ == NULL); // only allow setting once (by Stage)
|
||||||
start_ = &state;
|
start_ = &state;
|
||||||
@ -234,27 +238,34 @@ public:
|
|||||||
{}
|
{}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
||||||
|
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
|
||||||
|
|
||||||
void fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
void fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||||
Introspection* introspection = nullptr) const override;
|
Introspection* introspection = nullptr) const override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// actual trajectory, might be empty
|
// 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
|
template <> inline
|
||||||
const InterfaceState::Solutions& trajectories<FORWARD>(const SolutionBase &solution) {
|
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
|
||||||
return solution.end()->outgoingTrajectories();
|
return end_->outgoingTrajectories();
|
||||||
}
|
}
|
||||||
template <> inline
|
template <> inline
|
||||||
const InterfaceState::Solutions& trajectories<BACKWARD>(const SolutionBase &solution) {
|
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::BACKWARD>() const {
|
||||||
return solution.start()->incomingTrajectories();
|
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;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|||||||
@ -62,9 +62,9 @@ MOVEIT_CLASS_FORWARD(Task)
|
|||||||
|
|
||||||
/** A Task is the root of a tree of stages.
|
/** 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.
|
* Actually a tasks wraps a single container (by default a SerialContainer),
|
||||||
* The wrapped container spawns its solutions into the prevEnds(), nextStarts() interfaces,
|
* which serves as the root of all stages.
|
||||||
* which are provided by the wrappers end_ and start_ and interfaces respectively. */
|
*/
|
||||||
class Task : protected WrapperBase {
|
class Task : protected WrapperBase {
|
||||||
public:
|
public:
|
||||||
Task(const std::string& id = "",
|
Task(const std::string& id = "",
|
||||||
@ -76,6 +76,7 @@ public:
|
|||||||
~Task();
|
~Task();
|
||||||
|
|
||||||
std::string id() const;
|
std::string id() const;
|
||||||
|
const moveit::core::RobotModelPtr getRobotModel() const;
|
||||||
|
|
||||||
void add(Stage::pointer &&stage);
|
void add(Stage::pointer &&stage);
|
||||||
void clear() override;
|
void clear() override;
|
||||||
|
|||||||
66
core/include/moveit/task_constructor/type_traits.h
Normal file
66
core/include/moveit/task_constructor/type_traits.h
Normal 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 {};
|
||||||
|
|
||||||
|
} }
|
||||||
@ -94,12 +94,3 @@ private:
|
|||||||
};
|
};
|
||||||
|
|
||||||
#define DECLARE_FLAGS(Flags, Enum) typedef QFlags<Enum> Flags;
|
#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;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|||||||
@ -11,6 +11,10 @@ add_library(${PROJECT_NAME}
|
|||||||
${PROJECT_INCLUDE}/task.h
|
${PROJECT_INCLUDE}/task.h
|
||||||
${PROJECT_INCLUDE}/utils.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
|
container.cpp
|
||||||
introspection.cpp
|
introspection.cpp
|
||||||
marker_tools.cpp
|
marker_tools.cpp
|
||||||
@ -18,6 +22,10 @@ add_library(${PROJECT_NAME}
|
|||||||
stage.cpp
|
stage.cpp
|
||||||
storage.cpp
|
storage.cpp
|
||||||
task.cpp
|
task.cpp
|
||||||
|
|
||||||
|
solvers/planner_interface.cpp
|
||||||
|
solvers/cartesian_path.cpp
|
||||||
|
solvers/pipeline_planner.cpp
|
||||||
)
|
)
|
||||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||||
target_include_directories(${PROJECT_NAME}
|
target_include_directories(${PROJECT_NAME}
|
||||||
|
|||||||
@ -49,6 +49,13 @@ using namespace std::placeholders;
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor {
|
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 {
|
ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) const {
|
||||||
const_iterator position = children_.begin();
|
const_iterator position = children_.begin();
|
||||||
if (index > 0) {
|
if (index > 0) {
|
||||||
@ -90,17 +97,42 @@ bool ContainerBasePrivate::compute()
|
|||||||
return static_cast<ContainerBase*>(me_)->compute();
|
return static_cast<ContainerBase*>(me_)->compute();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ContainerBasePrivate::copyState(Interface::iterator external,
|
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
|
||||||
Stage &child, bool to_start, bool updated) {
|
// TODO need to update existing mapping?
|
||||||
if (to_start) {
|
|
||||||
InterfaceState& internal = *child.pimpl()->starts()->clone(*external);
|
// 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));
|
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 {
|
} else {
|
||||||
InterfaceState& internal = *child.pimpl()->ends()->clone(*external);
|
// spawn a new state in previous stage
|
||||||
internal_to_external_.insert(std::make_pair(&internal, external));
|
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)
|
ContainerBase::ContainerBase(ContainerBasePrivate *impl)
|
||||||
: Stage(impl)
|
: Stage(impl)
|
||||||
{
|
{
|
||||||
@ -139,6 +171,7 @@ bool ContainerBase::insert(Stage::pointer &&stage, int before)
|
|||||||
bool ContainerBase::remove(int pos)
|
bool ContainerBase::remove(int pos)
|
||||||
{
|
{
|
||||||
ContainerBasePrivate::const_iterator it = pimpl()->position(pos);
|
ContainerBasePrivate::const_iterator it = pimpl()->position(pos);
|
||||||
|
(*it)->pimpl()->setHierarchy(nullptr, ContainerBasePrivate::iterator());
|
||||||
pimpl()->children_.erase(it);
|
pimpl()->children_.erase(it);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
@ -156,7 +189,10 @@ void ContainerBase::reset()
|
|||||||
for (auto& child: impl->children())
|
for (auto& child: impl->children())
|
||||||
child->reset();
|
child->reset();
|
||||||
|
|
||||||
// clear mapping
|
// clear buffer interfaces
|
||||||
|
impl->pending_backward_->clear();
|
||||||
|
impl->pending_forward_->clear();
|
||||||
|
// ... and state mapping
|
||||||
impl->internal_to_external_.clear();
|
impl->internal_to_external_.clear();
|
||||||
|
|
||||||
Stage::reset();
|
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 {
|
struct SolutionCollector {
|
||||||
SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
|
SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
|
||||||
|
|
||||||
@ -232,11 +259,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
|
|
||||||
// find all incoming solution pathes ending at current solution
|
// find all incoming solution pathes ending at current solution
|
||||||
SolutionCollector incoming(num_before);
|
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
|
// find all outgoing solution pathes starting at current solution
|
||||||
SolutionCollector outgoing(num_after);
|
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
|
// collect (and sort) all solutions spanning from start to end of this container
|
||||||
ordered<SerialSolution> sorted;
|
ordered<SerialSolution> sorted;
|
||||||
@ -268,42 +295,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// store new solutions (in sorted)
|
// store new solutions (in sorted)
|
||||||
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it)
|
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) {
|
||||||
impl->storeNewSolution(std::move(*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
|
// clear queues
|
||||||
impl->solutions_.clear();
|
impl->solutions_.clear();
|
||||||
impl->pending_backward_->clear();
|
|
||||||
impl->pending_forward_->clear();
|
|
||||||
|
|
||||||
// recursively reset children
|
// recursively reset children
|
||||||
ContainerBase::reset();
|
ContainerBase::reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
|
||||||
|
: ContainerBasePrivate(me, name)
|
||||||
|
{}
|
||||||
|
|
||||||
void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) {
|
void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) {
|
||||||
prev->setNextStarts(next->starts());
|
prev->setNextStarts(next->starts());
|
||||||
next->setPrevEnds(prev->ends());
|
next->setPrevEnds(prev->ends());
|
||||||
@ -342,11 +340,11 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
|
|||||||
/*** connect children ***/
|
/*** connect children ***/
|
||||||
// first stage sends backward to pending_backward_
|
// first stage sends backward to pending_backward_
|
||||||
auto start = impl->children().begin();
|
auto start = impl->children().begin();
|
||||||
(*start)->pimpl()->setPrevEnds(impl->pending_backward_);
|
(*start)->pimpl()->setPrevEnds(impl->getPushBackwardInterface());
|
||||||
|
|
||||||
// last stage sends forward to pending_forward_
|
// last stage sends forward to pending_forward_
|
||||||
auto last = --impl->children().end();
|
auto last = --impl->children().end();
|
||||||
(*last)->pimpl()->setNextStarts(impl->pending_forward_);
|
(*last)->pimpl()->setNextStarts(impl->getPushForwardInterface());
|
||||||
|
|
||||||
auto cur = start;
|
auto cur = start;
|
||||||
auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage
|
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);
|
ContainerBase::init(scene);
|
||||||
|
|
||||||
// initialize starts_ and ends_ interfaces
|
// initialize starts_ and ends_ interfaces
|
||||||
Stage* child = start->get();
|
if (const InterfacePtr& target = (*start)->pimpl()->starts())
|
||||||
if (child->pimpl()->starts())
|
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
|
||||||
impl->starts_.reset(new Interface([impl, child](Interface::iterator external, bool updated){
|
if (const InterfacePtr& target = (*last)->pimpl()->ends())
|
||||||
// new external state in our starts_ interface is copied to first child
|
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
|
||||||
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);
|
|
||||||
}));
|
|
||||||
|
|
||||||
// validate connectivity of this
|
// validate connectivity of this
|
||||||
if (!impl->nextStarts())
|
if (!impl->nextStarts())
|
||||||
@ -421,16 +410,16 @@ size_t SerialContainer::numSolutions() const
|
|||||||
|
|
||||||
void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const
|
void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const
|
||||||
{
|
{
|
||||||
for(const SolutionBase& s : pimpl()->solutions())
|
for(const SolutionBase& s : pimpl()->solutions_)
|
||||||
if (!processor(s))
|
if (!processor(s))
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
template <TraverseDirection dir>
|
template <Interface::Direction dir>
|
||||||
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||||
solution_container &trace, double trace_cost)
|
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
|
if (solutions.empty()) // if we reached the end, call the callback
|
||||||
cb(trace, trace_cost);
|
cb(trace, trace_cost);
|
||||||
else for (SolutionBase* successor : solutions) {
|
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,
|
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||||
Introspection* introspection = nullptr) const
|
Introspection* introspection) const
|
||||||
{
|
{
|
||||||
moveit_task_constructor_msgs::SubSolution sub_msg;
|
moveit_task_constructor_msgs::SubSolution sub_msg;
|
||||||
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
|
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)
|
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name)
|
||||||
: ContainerBasePrivate(me, 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
|
// recursively reset children
|
||||||
ContainerBase::reset();
|
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)
|
void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
{
|
{
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
auto impl = pimpl();
|
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()) {
|
for (const Stage::pointer& stage : impl->children()) {
|
||||||
StagePrivate *child = stage->pimpl();
|
StagePrivate *child = stage->pimpl();
|
||||||
child->setPrevEnds(impl->prevEnds());
|
child->setPrevEnds(push_prev);
|
||||||
child->setNextStarts(impl->nextStarts());
|
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
|
// initialize this' pull connections
|
||||||
// this needs to be done *after* initializing the connections
|
for (Interface::Direction dir : { Interface::START, Interface::END }) {
|
||||||
ContainerBase::init(scene);
|
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)
|
if (errors)
|
||||||
throw errors;
|
throw errors;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ParallelContainerBase::onNewSolution(const SolutionBase &s)
|
size_t ParallelContainerBase::numSolutions() const
|
||||||
{
|
{
|
||||||
// update state priorities
|
return pimpl()->solutions_.size();
|
||||||
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);
|
void ParallelContainerBase::processSolutions(const Stage::SolutionProcessor &processor) const
|
||||||
|
{
|
||||||
|
for(const SolutionBase* s : pimpl()->solutions_)
|
||||||
|
if (!processor(*s))
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t ParallelContainerBase::numFailures() const
|
||||||
|
{
|
||||||
|
return pimpl()->failures_.size();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParallelContainerBase::processFailures(const Stage::SolutionProcessor &processor) const
|
||||||
|
{
|
||||||
|
for(const SolutionBase* f : pimpl()->failures_)
|
||||||
|
if (!processor(*f))
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParallelContainerBase::onNewSolution(const SolutionBase& s)
|
||||||
|
{
|
||||||
|
liftSolution(&s);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParallelContainerBase::liftSolution(const SolutionBase* solution, double cost)
|
||||||
|
{
|
||||||
|
auto impl = pimpl();
|
||||||
|
// create new WrappedSolution instance
|
||||||
|
auto wit = impl->wrapped_solutions_.insert(impl->wrapped_solutions_.end(), WrappedSolution(impl, solution, cost));
|
||||||
|
|
||||||
|
if (wit->isFailure()) {
|
||||||
|
wit->setStartState(*solution->start());
|
||||||
|
wit->setEndState(*solution->end());
|
||||||
|
impl->failures_.push_back(&*wit);
|
||||||
|
} else {
|
||||||
|
impl->solutions_.insert(&*wit);
|
||||||
|
impl->liftSolution(*wit, solution->start(), solution->end());
|
||||||
|
}
|
||||||
|
impl->newSolution(*wit);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t)
|
||||||
|
{
|
||||||
|
auto impl = pimpl();
|
||||||
|
assert(impl->prevEnds() && impl->nextStarts());
|
||||||
|
|
||||||
|
t.setCreator(impl);
|
||||||
|
// store newly created solution (otherwise it's lost)
|
||||||
|
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
|
||||||
|
|
||||||
|
if (it->isFailure()) {
|
||||||
|
auto state_it = impl->states_.insert(impl->states_.end(), std::move(state));
|
||||||
|
it->setStartState(*state_it);
|
||||||
|
it->setEndState(*state_it);
|
||||||
|
impl->failures_.push_back(&*it);
|
||||||
|
} else {
|
||||||
|
// directly spawn states in push interfaces
|
||||||
|
impl->prevEnds()->add(InterfaceState(state), NULL, &*it);
|
||||||
|
impl->nextStarts()->add(std::move(state), &*it, NULL);
|
||||||
|
impl->solutions_.insert(&*it);
|
||||||
|
}
|
||||||
|
impl->newSolution(*it);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
|
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
|
||||||
: ContainerBasePrivate(me, name)
|
: ParallelContainerBasePrivate(me, name)
|
||||||
{
|
{}
|
||||||
dummy_starts_.reset(new Interface(Interface::NotifyFunction()));
|
|
||||||
dummy_ends_.reset(new Interface(Interface::NotifyFunction()));
|
|
||||||
}
|
|
||||||
|
|
||||||
WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child)
|
WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child)
|
||||||
: WrapperBase(new WrapperBasePrivate(this, name), std::move(child))
|
: WrapperBase(new WrapperBasePrivate(this, name), std::move(child))
|
||||||
{}
|
{}
|
||||||
|
|
||||||
WrapperBase::WrapperBase(WrapperBasePrivate *impl, Stage::pointer &&child)
|
WrapperBase::WrapperBase(WrapperBasePrivate *impl, Stage::pointer &&child)
|
||||||
: ContainerBase(impl)
|
: ParallelContainerBase(impl)
|
||||||
{
|
{
|
||||||
if (child) insert(std::move(child));
|
if (child) insert(std::move(child));
|
||||||
}
|
}
|
||||||
@ -543,38 +626,7 @@ bool WrapperBase::insert(Stage::pointer &&stage, int before)
|
|||||||
// restrict num of children to one
|
// restrict num of children to one
|
||||||
if (numChildren() > 0)
|
if (numChildren() > 0)
|
||||||
return false;
|
return false;
|
||||||
return ContainerBase::insert(std::move(stage), before);
|
return ParallelContainerBase::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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Stage* WrapperBase::wrapped()
|
Stage* WrapperBase::wrapped()
|
||||||
@ -582,88 +634,65 @@ Stage* WrapperBase::wrapped()
|
|||||||
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get();
|
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get();
|
||||||
}
|
}
|
||||||
|
|
||||||
void WrapperBase::onNewSolution(const SolutionBase &s)
|
bool WrapperBase::canCompute() const
|
||||||
{
|
|
||||||
// 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
|
|
||||||
{
|
{
|
||||||
return wrapped()->pimpl()->canCompute();
|
return wrapped()->pimpl()->canCompute();
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Wrapper::compute()
|
bool WrapperBase::compute()
|
||||||
{
|
{
|
||||||
size_t num_before = numSolutions();
|
size_t num_before = numSolutions();
|
||||||
wrapped()->pimpl()->compute();
|
wrapped()->pimpl()->compute();
|
||||||
return numSolutions() > num_before;
|
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_)
|
active_child_ = nullptr;
|
||||||
if (!processor(*s))
|
ParallelContainerBase::reset();
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Wrapper::processFailures(const Stage::SolutionProcessor &processor) const
|
void Fallbacks::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
{
|
{
|
||||||
for(const auto& s : pimpl()->failures_)
|
ParallelContainerBase::init(scene);
|
||||||
if (!processor(*s))
|
active_child_ = pimpl()->children().front().get();
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Wrapper::spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s)
|
bool Fallbacks::canCompute() const
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
while (active_child_) {
|
||||||
s->setCreator(impl);
|
StagePrivate* child = active_child_->pimpl();
|
||||||
SolutionBase* solution = s.get();
|
if (child->canCompute()) return true;
|
||||||
if (s->isFailure()) {
|
|
||||||
impl->failure_states_.emplace_back(std::move(state));
|
// active child failed, continue with next
|
||||||
s->setStartState(impl->failure_states_.back());
|
auto next = child->it(); ++next;
|
||||||
s->setEndState(impl->failure_states_.back());
|
active_child_ = next->get();
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
impl->newSolution(*solution);
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Fallbacks::compute()
|
||||||
|
{
|
||||||
|
if (!active_child_) return false;
|
||||||
|
return active_child_->pimpl()->compute();
|
||||||
}
|
}
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
114
core/src/solvers/cartesian_path.cpp
Normal file
114
core/src/solvers/cartesian_path.cpp
Normal 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
137
core/src/solvers/pipeline_planner.cpp
Normal file
137
core/src/solvers/pipeline_planner.cpp
Normal 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
51
core/src/solvers/planner_interface.cpp
Normal file
51
core/src/solvers/planner_interface.cpp
Normal 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();
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -216,8 +216,8 @@ SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) {
|
|||||||
return *solutions_.insert(std::move(trajectory));
|
return *solutions_.insert(std::move(trajectory));
|
||||||
} else if (me()->storeFailures()) {
|
} else if (me()->storeFailures()) {
|
||||||
// only store failures when introspection is enabled
|
// only store failures when introspection is enabled
|
||||||
failures_.emplace_back(std::move(trajectory));
|
auto it = failures_.insert(failures_.end(), std::move(trajectory));
|
||||||
return failures_.back();
|
return *it;
|
||||||
} else
|
} else
|
||||||
return trajectory;
|
return trajectory;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,16 +1,28 @@
|
|||||||
add_library(${PROJECT_NAME}_stages
|
add_library(${PROJECT_NAME}_stages
|
||||||
${PROJECT_INCLUDE}/stages/cartesian_position_motion.h
|
|
||||||
${PROJECT_INCLUDE}/stages/compute_ik.h
|
${PROJECT_INCLUDE}/stages/compute_ik.h
|
||||||
${PROJECT_INCLUDE}/stages/current_state.h
|
${PROJECT_INCLUDE}/stages/current_state.h
|
||||||
${PROJECT_INCLUDE}/stages/generate_grasp_pose.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/gripper.h
|
||||||
|
${PROJECT_INCLUDE}/stages/modify_planning_scene.h
|
||||||
|
${PROJECT_INCLUDE}/stages/fix_collision_objects.h
|
||||||
${PROJECT_INCLUDE}/stages/move.h
|
${PROJECT_INCLUDE}/stages/move.h
|
||||||
|
|
||||||
cartesian_position_motion.cpp
|
|
||||||
current_state.cpp
|
current_state.cpp
|
||||||
compute_ik.cpp
|
compute_ik.cpp
|
||||||
generate_grasp_pose.cpp
|
generate_grasp_pose.cpp
|
||||||
|
connect.cpp
|
||||||
|
move_to.cpp
|
||||||
|
move_relative.cpp
|
||||||
|
|
||||||
|
cartesian_position_motion.cpp
|
||||||
gripper.cpp
|
gripper.cpp
|
||||||
|
modify_planning_scene.cpp
|
||||||
|
fix_collision_objects.cpp
|
||||||
move.cpp
|
move.cpp
|
||||||
)
|
)
|
||||||
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
|
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||||
|
|||||||
@ -20,7 +20,7 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
|
ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
|
||||||
: Wrapper(name, std::move(child))
|
: WrapperBase(name, std::move(child))
|
||||||
{
|
{
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<double>("timeout", 1.0);
|
p.declare<double>("timeout", 1.0);
|
||||||
@ -218,17 +218,17 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
start_time = now;
|
start_time = now;
|
||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr scene = s.start()->scene();
|
planning_scene::PlanningSceneConstPtr scene = s.start()->scene();
|
||||||
std::unique_ptr<SolutionBase> solution(new SubTrajectory());
|
SubTrajectory solution;
|
||||||
|
|
||||||
// include markers from original 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
|
// frame at target pose
|
||||||
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
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) {
|
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
|
// create a new scene for each solution as they will have different robot states
|
||||||
planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff();
|
planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff();
|
||||||
robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst();
|
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) {
|
auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) {
|
||||||
marker.ns = "ik solution";
|
marker.ns = "ik solution";
|
||||||
marker.color.a *= 0.5;
|
marker.color.a *= 0.5;
|
||||||
solution->markers().push_back(marker);
|
solution.markers().push_back(marker);
|
||||||
};
|
};
|
||||||
generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames());
|
generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames());
|
||||||
} else {
|
} else {
|
||||||
solution->setCost(std::numeric_limits<double>::infinity());
|
solution.setCost(std::numeric_limits<double>::infinity());
|
||||||
}
|
}
|
||||||
|
|
||||||
spawn(InterfaceState(scene), std::move(solution));
|
spawn(InterfaceState(scene), std::move(solution));
|
||||||
|
|||||||
73
core/src/stages/connect.cpp
Normal file
73
core/src/stages/connect.cpp
Normal 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
140
core/src/stages/fix_collision_objects.cpp
Normal file
140
core/src/stages/fix_collision_objects.cpp
Normal 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
130
core/src/stages/modify_planning_scene.cpp
Normal file
130
core/src/stages/modify_planning_scene.cpp
Normal 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
274
core/src/stages/move_relative.cpp
Normal file
274
core/src/stages/move_relative.cpp
Normal 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
193
core/src/stages/move_to.cpp
Normal 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -69,6 +69,10 @@ void Task::initModel () {
|
|||||||
throw Exception("Task failed to construct RobotModel");
|
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) {
|
planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) {
|
||||||
assert(rml_);
|
assert(rml_);
|
||||||
|
|
||||||
@ -182,10 +186,19 @@ void Task::reset()
|
|||||||
|
|
||||||
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
|
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
|
// provide introspection instance to all stages
|
||||||
pimpl()->setIntrospection(introspection_.get());
|
impl->setIntrospection(introspection_.get());
|
||||||
pimpl()->traverseStages([this](Stage& stage, int) {
|
impl->traverseStages([this](Stage& stage, int) {
|
||||||
stage.pimpl()->setIntrospection(introspection_.get());
|
stage.pimpl()->setIntrospection(introspection_.get());
|
||||||
return true;
|
return true;
|
||||||
}, 1, UINT_MAX);
|
}, 1, UINT_MAX);
|
||||||
@ -224,9 +237,7 @@ bool Task::plan()
|
|||||||
|
|
||||||
size_t Task::numSolutions() const
|
size_t Task::numSolutions() const
|
||||||
{
|
{
|
||||||
auto w = stages();
|
return stages()->numSolutions();
|
||||||
// during initial insert() we call numSolutions(), but wrapped() is not yet defined
|
|
||||||
return w ? w->numSolutions() : 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) const
|
void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) const
|
||||||
@ -242,7 +253,7 @@ void Task::publishAllSolutions(bool wait)
|
|||||||
|
|
||||||
void Task::onNewSolution(const SolutionBase &s)
|
void Task::onNewSolution(const SolutionBase &s)
|
||||||
{
|
{
|
||||||
WrapperBase::onNewSolution(s);
|
// no need to call WrapperBase::onNewSolution!
|
||||||
if (introspection_)
|
if (introspection_)
|
||||||
introspection_->publishSolution(s);
|
introspection_->publishSolution(s);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -55,7 +55,7 @@ TEST_F(BaseTest, serialContainer) {
|
|||||||
SerialContainer c("serial");
|
SerialContainer c("serial");
|
||||||
SerialContainerPrivate *cp = c.pimpl();
|
SerialContainerPrivate *cp = c.pimpl();
|
||||||
// pretend, that the container is connected
|
// pretend, that the container is connected
|
||||||
InterfacePtr dummy(new Interface(Interface::NotifyFunction()));
|
InterfacePtr dummy(new Interface);
|
||||||
cp->setNextStarts(dummy);
|
cp->setNextStarts(dummy);
|
||||||
cp->setPrevEnds(dummy);
|
cp->setPrevEnds(dummy);
|
||||||
planning_scene::PlanningScenePtr scene;
|
planning_scene::PlanningScenePtr scene;
|
||||||
|
|||||||
@ -18,8 +18,8 @@ public:
|
|||||||
robot_model_loader::RobotModelLoader loader;
|
robot_model_loader::RobotModelLoader loader;
|
||||||
ps.reset(new planning_scene::PlanningScene(loader.getModel()));
|
ps.reset(new planning_scene::PlanningScene(loader.getModel()));
|
||||||
|
|
||||||
prev.reset(new Interface(Interface::NotifyFunction()));
|
prev.reset(new Interface);
|
||||||
next.reset(new Interface(Interface::NotifyFunction()));
|
next.reset(new Interface);
|
||||||
pimpl()->setPrevEnds(prev);
|
pimpl()->setPrevEnds(prev);
|
||||||
pimpl()->setNextStarts(next);
|
pimpl()->setNextStarts(next);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,3 +1,4 @@
|
|||||||
add_subdirectory(utils)
|
add_subdirectory(utils)
|
||||||
|
add_subdirectory(properties)
|
||||||
add_subdirectory(src)
|
add_subdirectory(src)
|
||||||
add_subdirectory(test)
|
add_subdirectory(test)
|
||||||
|
|||||||
@ -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})
|
||||||
@ -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;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
@ -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);
|
||||||
|
|
||||||
|
}
|
||||||
@ -1,6 +1,10 @@
|
|||||||
set(MOVEIT_LIB_NAME motion_planning_tasks_rviz_plugin)
|
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}
|
add_library(${MOVEIT_LIB_NAME}
|
||||||
factory_model.cpp
|
factory_model.cpp
|
||||||
@ -22,7 +26,7 @@ add_library(${MOVEIT_LIB_NAME}
|
|||||||
|
|
||||||
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
|
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
|
||||||
target_link_libraries(${MOVEIT_LIB_NAME}
|
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}
|
${catkin_LIBRARIES} ${QT_LIBRARIES}
|
||||||
)
|
)
|
||||||
target_include_directories(${MOVEIT_LIB_NAME}
|
target_include_directories(${MOVEIT_LIB_NAME}
|
||||||
|
|||||||
@ -36,6 +36,7 @@
|
|||||||
|
|
||||||
#include "local_task_model.h"
|
#include "local_task_model.h"
|
||||||
#include "factory_model.h"
|
#include "factory_model.h"
|
||||||
|
#include "properties/property_factory.h"
|
||||||
#include <moveit/task_constructor/container_p.h>
|
#include <moveit/task_constructor/container_p.h>
|
||||||
|
|
||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
@ -232,4 +233,14 @@ DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex &index)
|
|||||||
return DisplaySolutionPtr();
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -49,6 +49,7 @@ class LocalTaskModel
|
|||||||
typedef moveit::task_constructor::StagePrivate Node;
|
typedef moveit::task_constructor::StagePrivate Node;
|
||||||
Node *root_;
|
Node *root_;
|
||||||
StageFactoryPtr stage_factory_;
|
StageFactoryPtr stage_factory_;
|
||||||
|
std::map<Node*, rviz::PropertyTreeModel*> properties_;
|
||||||
|
|
||||||
inline Node* node(const QModelIndex &index) const;
|
inline Node* node(const QModelIndex &index) const;
|
||||||
QModelIndex index(Node *n) const;
|
QModelIndex index(Node *n) const;
|
||||||
@ -73,6 +74,8 @@ public:
|
|||||||
|
|
||||||
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
|
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
|
||||||
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
|
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
|
||||||
|
|
||||||
|
rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -40,6 +40,8 @@
|
|||||||
#include <moveit/task_constructor/container.h>
|
#include <moveit/task_constructor/container.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit_task_constructor_msgs/GetSolution.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/console.h>
|
||||||
#include <ros/service_client.h>
|
#include <ros/service_client.h>
|
||||||
|
|
||||||
@ -64,9 +66,11 @@ struct RemoteTaskModel::Node {
|
|||||||
InterfaceFlags interface_flags_;
|
InterfaceFlags interface_flags_;
|
||||||
NodeFlags node_flags_;
|
NodeFlags node_flags_;
|
||||||
std::unique_ptr<RemoteSolutionModel> solutions_;
|
std::unique_ptr<RemoteSolutionModel> solutions_;
|
||||||
|
std::unique_ptr<rviz::PropertyTreeModel> properties_;
|
||||||
|
|
||||||
inline Node(Node *parent) : parent_(parent) {
|
inline Node(Node *parent) : parent_(parent) {
|
||||||
solutions_.reset(new RemoteSolutionModel());
|
solutions_.reset(new RemoteSolutionModel());
|
||||||
|
properties_.reset(new rviz::PropertyTreeModel(new rviz::Property()));
|
||||||
}
|
}
|
||||||
|
|
||||||
bool setName(const QString& name) {
|
bool setName(const QString& name) {
|
||||||
@ -369,6 +373,13 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
|
|||||||
return it->second;
|
return it->second;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex &index)
|
||||||
|
{
|
||||||
|
Node *n = node(index);
|
||||||
|
if (!n) return nullptr;
|
||||||
|
return n->properties_.get();
|
||||||
|
}
|
||||||
|
|
||||||
namespace detail {
|
namespace detail {
|
||||||
// method used by sorted_ container (requiring an additional dereference to access id)
|
// method used by sorted_ container (requiring an additional dereference to access id)
|
||||||
template <class T>
|
template <class T>
|
||||||
|
|||||||
@ -87,6 +87,8 @@ public:
|
|||||||
|
|
||||||
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
|
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
|
||||||
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
|
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
|
||||||
|
|
||||||
|
rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -264,7 +264,7 @@ void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last
|
|||||||
TaskListModel* m = static_cast<TaskListModel*>(sender());
|
TaskListModel* m = static_cast<TaskListModel*>(sender());
|
||||||
for (; first <= last; ++first) {
|
for (; first <= last; ++first) {
|
||||||
QModelIndex idx = m->index(first, 0, parent);
|
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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -50,6 +50,7 @@
|
|||||||
#include <memory>
|
#include <memory>
|
||||||
|
|
||||||
namespace ros { class ServiceClient; }
|
namespace ros { class ServiceClient; }
|
||||||
|
namespace rviz { class PropertyTreeModel; }
|
||||||
|
|
||||||
namespace moveit_rviz_plugin {
|
namespace moveit_rviz_plugin {
|
||||||
|
|
||||||
@ -84,8 +85,13 @@ public:
|
|||||||
Qt::ItemFlags flags(const QModelIndex &index) const override;
|
Qt::ItemFlags flags(const QModelIndex &index) const override;
|
||||||
unsigned int taskFlags() const { return flags_; }
|
unsigned int taskFlags() const { return flags_; }
|
||||||
|
|
||||||
|
/// get solution model for given stage index
|
||||||
virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0;
|
virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0;
|
||||||
|
/// get solution for given solution index
|
||||||
virtual DisplaySolutionPtr getSolution(const QModelIndex &index) = 0;
|
virtual DisplaySolutionPtr getSolution(const QModelIndex &index) = 0;
|
||||||
|
|
||||||
|
/// get property model for given stage index
|
||||||
|
virtual rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -79,13 +79,15 @@ TaskPanel::TaskPanel(QWidget* parent)
|
|||||||
: rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this))
|
: rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this))
|
||||||
{
|
{
|
||||||
Q_D(TaskPanel);
|
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)),
|
d->tasks_widget = new TaskView(this);
|
||||||
this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex)));
|
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 still undefined, this becomes the global instance
|
||||||
if (TaskPanelPrivate::global_instance_.isNull()) {
|
if (TaskPanelPrivate::global_instance_.isNull()) {
|
||||||
@ -127,14 +129,41 @@ void TaskPanel::decUseCount()
|
|||||||
|
|
||||||
TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
|
TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
|
||||||
: q_ptr(q_ptr)
|
: q_ptr(q_ptr)
|
||||||
, settings(new rviz::PropertyTreeModel(new rviz::Property, q_ptr))
|
|
||||||
{
|
{
|
||||||
setupUi(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();
|
MetaTaskListModel *meta_model = &MetaTaskListModel::instance();
|
||||||
StageFactoryPtr factory = getStageFactory();
|
StageFactoryPtr factory = getStageFactory();
|
||||||
if (factory) meta_model->setMimeTypes( { factory->mimeType() } );
|
if (factory) meta_model->setMimeTypes( { factory->mimeType() } );
|
||||||
else button_show_stage_dock_widget->setEnabled(false);
|
|
||||||
tasks_view->setModel(meta_model);
|
tasks_view->setModel(meta_model);
|
||||||
|
|
||||||
tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection);
|
tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection);
|
||||||
@ -147,99 +176,86 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
|
|||||||
solutions_view->setStretchSection(2);
|
solutions_view->setStretchSection(2);
|
||||||
|
|
||||||
// init actions
|
// init actions
|
||||||
tasks_view->addActions({actionRemoveTaskTreeRows, actionAddLocalTask});
|
tasks_view->addActions({actionAddLocalTask, actionRemoveTaskTreeRows});
|
||||||
|
|
||||||
initSettings(settings->getRoot());
|
|
||||||
settings_view->setModel(settings);
|
|
||||||
}
|
|
||||||
|
|
||||||
void TaskPanelPrivate::initSettings(rviz::Property* root)
|
|
||||||
{
|
|
||||||
}
|
}
|
||||||
|
|
||||||
std::pair<TaskListModel*, TaskDisplay*>
|
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());
|
auto *meta_model = static_cast<MetaTaskListModel*>(tasks_view->model());
|
||||||
return meta_model->getTaskListModel(index);
|
return meta_model->getTaskListModel(index);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::pair<BaseTaskModel*, QModelIndex>
|
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());
|
auto *meta_model = static_cast<MetaTaskListModel*>(tasks_view->model());
|
||||||
return meta_model->getTaskModel(index);
|
return meta_model->getTaskModel(index);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskPanelPrivate::unlock(TaskDisplay* display)
|
void TaskViewPrivate::lock(TaskDisplay* display)
|
||||||
{
|
{
|
||||||
if (locked_display_ && locked_display_ != display) {
|
if (locked_display_ && locked_display_ != display) {
|
||||||
locked_display_->clearMarkers();
|
locked_display_->clearMarkers();
|
||||||
locked_display_->visualization()->unlock();
|
locked_display_->visualization()->unlock();
|
||||||
locked_display_ = display;
|
|
||||||
}
|
}
|
||||||
|
locked_display_ = display;
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskPanel::onInitialize()
|
TaskView::TaskView(QWidget *parent)
|
||||||
|
: QWidget(parent), d_ptr(new TaskViewPrivate(this))
|
||||||
{
|
{
|
||||||
d_ptr->window_manager_ = vis_manager_->getWindowManager();
|
Q_D(TaskView);
|
||||||
|
|
||||||
|
// connect signals
|
||||||
|
connect(d->actionRemoveTaskTreeRows, SIGNAL(triggered()), this, SLOT(removeSelectedStages()));
|
||||||
|
connect(d->actionAddLocalTask, SIGNAL(triggered()), this, SLOT(addTask()));
|
||||||
|
|
||||||
|
connect(d->tasks_view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)),
|
||||||
|
this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex)));
|
||||||
|
|
||||||
|
onCurrentStageChanged(d->tasks_view->currentIndex(), QModelIndex());
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskPanel::save(rviz::Config config) const
|
void TaskView::addTask()
|
||||||
{
|
{
|
||||||
rviz::Panel::save(config);
|
|
||||||
d_ptr->settings->getRoot()->save(config);
|
|
||||||
}
|
|
||||||
|
|
||||||
void TaskPanel::load(const rviz::Config& config)
|
|
||||||
{
|
|
||||||
rviz::Panel::load(config);
|
|
||||||
d_ptr->settings->getRoot()->load(config);
|
|
||||||
}
|
|
||||||
|
|
||||||
void TaskPanel::addTask()
|
|
||||||
{
|
|
||||||
TaskListModel* task_list_model = nullptr;
|
|
||||||
QModelIndex current = d_ptr->tasks_view->currentIndex();
|
QModelIndex current = d_ptr->tasks_view->currentIndex();
|
||||||
if (!current.isValid()) {
|
if (!current.isValid()) return;
|
||||||
// create new TaskDisplay
|
bool is_top_level = !current.parent().isValid();
|
||||||
TaskDisplay *display = new TaskDisplay();
|
|
||||||
display->setName("Motion Planning Task");
|
|
||||||
vis_manager_->getRootDisplayGroup()->addDisplay(display);
|
|
||||||
display->initialize(vis_manager_);
|
|
||||||
display->setEnabled(true);
|
|
||||||
|
|
||||||
task_list_model = &display->getTaskListModel();
|
TaskListModel* task_list_model = d_ptr->getTaskListModel(current).first;
|
||||||
} else
|
task_list_model->insertModel(new LocalTaskModel(task_list_model), is_top_level ? -1 : current.row());
|
||||||
task_list_model = d_ptr->getTaskListModel(current).first;
|
|
||||||
|
|
||||||
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()
|
void TaskView::removeSelectedStages()
|
||||||
{
|
|
||||||
rviz::PanelDockWidget *dock = getStageDockWidget(d_ptr->window_manager_);
|
|
||||||
if (dock) dock->show();
|
|
||||||
}
|
|
||||||
|
|
||||||
void TaskPanel::removeSelectedStages()
|
|
||||||
{
|
{
|
||||||
auto *m = d_ptr->tasks_view->model();
|
auto *m = d_ptr->tasks_view->model();
|
||||||
for (const auto &range : d_ptr->tasks_view->selectionModel()->selection())
|
for (const auto &range : d_ptr->tasks_view->selectionModel()->selection())
|
||||||
m->removeRows(range.top(), range.bottom()-range.top()+1, range.parent());
|
m->removeRows(range.top(), range.bottom()-range.top()+1, range.parent());
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous)
|
void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous)
|
||||||
{
|
{
|
||||||
|
// adding task is allowed on top-level items and sub-top-level items
|
||||||
|
d_ptr->actionAddLocalTask->setEnabled(current.isValid() &&
|
||||||
|
(!current.parent().isValid() || !current.parent().parent().isValid()));
|
||||||
|
// removing stuff is allowed if there is any selection / any curren item
|
||||||
|
d_ptr->actionRemoveTaskTreeRows->setEnabled(current.isValid());
|
||||||
|
|
||||||
BaseTaskModel *task;
|
BaseTaskModel *task;
|
||||||
QModelIndex task_index;
|
QModelIndex task_index;
|
||||||
std::tie(task, task_index) = d_ptr->getTaskModel(current);
|
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->lock(nullptr); // unlocks any locked_display_
|
||||||
d_ptr->unlock(nullptr);
|
|
||||||
|
|
||||||
auto *view = d_ptr->solutions_view;
|
// update the SolutionModel
|
||||||
|
QTreeView *view = d_ptr->solutions_view;
|
||||||
QItemSelectionModel *sm = view->selectionModel();
|
QItemSelectionModel *sm = view->selectionModel();
|
||||||
QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr;
|
QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr;
|
||||||
view->sortByColumn(-1);
|
view->sortByColumn(-1);
|
||||||
@ -253,12 +269,19 @@ void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIn
|
|||||||
connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)),
|
connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)),
|
||||||
this, SLOT(onSolutionSelectionChanged(QItemSelection, QItemSelection)));
|
this, SLOT(onSolutionSelectionChanged(QItemSelection, QItemSelection)));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// update the PropertyModel
|
||||||
|
view = d_ptr->property_view;
|
||||||
|
sm = view->selectionModel();
|
||||||
|
m = task ? task->getPropertyModel(task_index) : nullptr;
|
||||||
|
view->setModel(m);
|
||||||
|
delete sm; // we don't store the selection model
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous)
|
void TaskView::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous)
|
||||||
{
|
{
|
||||||
TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second;
|
TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second;
|
||||||
d_ptr->unlock(display);
|
d_ptr->lock(display);
|
||||||
|
|
||||||
if (!display || !current.isValid())
|
if (!display || !current.isValid())
|
||||||
return;
|
return;
|
||||||
@ -266,7 +289,6 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode
|
|||||||
BaseTaskModel *task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first;
|
BaseTaskModel *task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first;
|
||||||
Q_ASSERT(task);
|
Q_ASSERT(task);
|
||||||
|
|
||||||
d_ptr->locked_display_ = display;
|
|
||||||
TaskSolutionVisualization* vis = display->visualization();
|
TaskSolutionVisualization* vis = display->visualization();
|
||||||
const DisplaySolutionPtr& solution = task->getSolution(current);
|
const DisplaySolutionPtr& solution = task->getSolution(current);
|
||||||
display->setSolutionStatus(bool(solution));
|
display->setSolutionStatus(bool(solution));
|
||||||
@ -274,7 +296,7 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode
|
|||||||
vis->showTrajectory(solution, true);
|
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();
|
QItemSelectionModel *sm = d_ptr->solutions_view->selectionModel();
|
||||||
const QModelIndexList& selected_rows = sm->selectedRows();
|
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"
|
#include "moc_task_panel.cpp"
|
||||||
|
|||||||
@ -82,9 +82,23 @@ public:
|
|||||||
void load(const rviz::Config& config) override;
|
void load(const rviz::Config& config) override;
|
||||||
void save(rviz::Config config) const 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:
|
public Q_SLOTS:
|
||||||
void addTask();
|
void addTask();
|
||||||
void showStageDockWidget();
|
|
||||||
|
|
||||||
protected Q_SLOTS:
|
protected Q_SLOTS:
|
||||||
void removeSelectedStages();
|
void removeSelectedStages();
|
||||||
@ -93,4 +107,15 @@ protected Q_SLOTS:
|
|||||||
void onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected);
|
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);
|
||||||
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@ -14,9 +14,6 @@
|
|||||||
<string>Tasks Panel</string>
|
<string>Tasks Panel</string>
|
||||||
</property>
|
</property>
|
||||||
<layout class="QVBoxLayout" name="verticalLayout">
|
<layout class="QVBoxLayout" name="verticalLayout">
|
||||||
<property name="spacing">
|
|
||||||
<number>0</number>
|
|
||||||
</property>
|
|
||||||
<property name="leftMargin">
|
<property name="leftMargin">
|
||||||
<number>0</number>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
@ -30,56 +27,38 @@
|
|||||||
<number>0</number>
|
<number>0</number>
|
||||||
</property>
|
</property>
|
||||||
<item>
|
<item>
|
||||||
<widget class="QTabWidget" name="tabWidget">
|
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||||
<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>
|
|
||||||
<item>
|
<item>
|
||||||
<layout class="QHBoxLayout" name="tasks_view_label_layout">
|
<widget class="QToolButton" name="button_show_settings">
|
||||||
<property name="spacing">
|
|
||||||
<number>6</number>
|
|
||||||
</property>
|
|
||||||
<item>
|
|
||||||
<widget class="QLabel" name="tasks_view_label">
|
|
||||||
<property name="text">
|
<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>
|
</property>
|
||||||
</widget>
|
</widget>
|
||||||
</item>
|
</item>
|
||||||
<item>
|
<item>
|
||||||
<widget class="QToolButton" name="button_show_stage_dock_widget">
|
<spacer name="horizontalSpacer">
|
||||||
<property name="maximumSize">
|
<property name="orientation">
|
||||||
|
<enum>Qt::Horizontal</enum>
|
||||||
|
</property>
|
||||||
|
<property name="sizeHint" stdset="0">
|
||||||
<size>
|
<size>
|
||||||
<width>18</width>
|
<width>40</width>
|
||||||
<height>18</height>
|
<height>20</height>
|
||||||
</size>
|
</size>
|
||||||
</property>
|
</property>
|
||||||
|
</spacer>
|
||||||
|
</item>
|
||||||
|
<item>
|
||||||
|
<widget class="QToolButton" name="button_show_stage_dock_widget">
|
||||||
<property name="text">
|
<property name="text">
|
||||||
<string/>
|
<string>...</string>
|
||||||
</property>
|
</property>
|
||||||
<property name="icon">
|
<property name="icon">
|
||||||
<iconset resource="resources.qrc">
|
<iconset resource="resources.qrc">
|
||||||
@ -89,134 +68,8 @@
|
|||||||
</item>
|
</item>
|
||||||
</layout>
|
</layout>
|
||||||
</item>
|
</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>
|
</layout>
|
||||||
</widget>
|
</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>
|
<resources>
|
||||||
<include location="resources.qrc"/>
|
<include location="resources.qrc"/>
|
||||||
</resources>
|
</resources>
|
||||||
|
|||||||
@ -40,6 +40,8 @@
|
|||||||
|
|
||||||
#include "task_panel.h"
|
#include "task_panel.h"
|
||||||
#include "ui_task_panel.h"
|
#include "ui_task_panel.h"
|
||||||
|
#include "ui_task_view.h"
|
||||||
|
#include "ui_task_settings.h"
|
||||||
|
|
||||||
#include <rviz/panel.h>
|
#include <rviz/panel.h>
|
||||||
#include <rviz/properties/property_tree_model.h>
|
#include <rviz/properties/property_tree_model.h>
|
||||||
@ -55,7 +57,19 @@ class TaskPanelPrivate : public Ui_TaskPanel {
|
|||||||
public:
|
public:
|
||||||
TaskPanelPrivate(TaskPanel *q_ptr);
|
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
|
/// retrieve TaskListModel corresponding to given index
|
||||||
inline std::pair<TaskListModel*, TaskDisplay*>
|
inline std::pair<TaskListModel*, TaskDisplay*>
|
||||||
@ -66,15 +80,18 @@ public:
|
|||||||
getTaskModel(const QModelIndex& index) const;
|
getTaskModel(const QModelIndex& index) const;
|
||||||
|
|
||||||
/// unlock locked_display_ if given display is different
|
/// unlock locked_display_ if given display is different
|
||||||
void unlock(TaskDisplay *display);
|
void lock(TaskDisplay *display);
|
||||||
|
|
||||||
TaskPanel* q_ptr;
|
TaskView *q_ptr;
|
||||||
rviz::PropertyTreeModel* settings;
|
|
||||||
QPointer<TaskDisplay> locked_display_;
|
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;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
69
visualization/motion_planning_tasks/src/task_settings.ui
Normal file
69
visualization/motion_planning_tasks/src/task_settings.ui
Normal 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>
|
||||||
157
visualization/motion_planning_tasks/src/task_view.ui
Normal file
157
visualization/motion_planning_tasks/src/task_view.ui
Normal 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>
|
||||||
Loading…
Reference in New Issue
Block a user