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
|
||||
/// For each solution path, callback the given processor passing
|
||||
/// the full trace (from start to end, but not including start) and its accumulated costs
|
||||
template<TraverseDirection dir>
|
||||
template<Interface::Direction dir>
|
||||
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||
solution_container &trace, double trace_cost = 0);
|
||||
|
||||
@ -135,102 +135,101 @@ public:
|
||||
void reset() override;
|
||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||
|
||||
size_t numSolutions() const override;
|
||||
void processSolutions(const SolutionProcessor &processor) const override;
|
||||
size_t numFailures() const override;
|
||||
void processFailures(const SolutionProcessor &processor) const override;
|
||||
|
||||
protected:
|
||||
ParallelContainerBase(ParallelContainerBasePrivate* impl);
|
||||
|
||||
/// method called if any child found a new (internal) solution
|
||||
virtual void onNewSolution(const SolutionBase& s) override;
|
||||
|
||||
/// callback for new start states (received externally)
|
||||
virtual void onNewStartState(Interface::iterator external, bool updated) = 0;
|
||||
/// callback for new end states (received externally)
|
||||
virtual void onNewEndState(Interface::iterator external, bool updated) = 0;
|
||||
/// lift unmodified child solution (useful for simple filtering)
|
||||
void liftSolution(const SolutionBase* solution) {
|
||||
liftSolution(solution, solution->cost());
|
||||
}
|
||||
/// lift child solution, but allow for modifying costs
|
||||
void liftSolution(const SolutionBase* solution, double cost);
|
||||
|
||||
/// spawn a new solution with given state as start and end
|
||||
void spawn(InterfaceState &&state, SubTrajectory&& trajectory);
|
||||
/// convience method spawning an empty SubTrajectory with given cost
|
||||
void spawn(InterfaceState &&state, double cost) {
|
||||
SubTrajectory trajectory;
|
||||
trajectory.setCost(cost);
|
||||
spawn(std::move(state), std::move(trajectory));
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/** Wrap an existing solution - for use in containers.
|
||||
/** Plan for different alternatives in parallel.
|
||||
*
|
||||
* This essentially wraps a solution of a child and thus allows
|
||||
* for new new clones of start / end states, which in turn will
|
||||
* have separate incoming/outgoing trajectories */
|
||||
class WrappedSolution : public SolutionBase {
|
||||
* Solution of all children are reported - sorted by cost.
|
||||
*/
|
||||
class Alternatives : public ParallelContainerBase
|
||||
{
|
||||
public:
|
||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped)
|
||||
: SolutionBase(creator, wrapped->cost()), wrapped_(wrapped)
|
||||
{}
|
||||
void fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
||||
Introspection* introspection = nullptr) const override {
|
||||
wrapped_->fillMessage(solution, introspection);
|
||||
}
|
||||
Alternatives(const std::string &name) : ParallelContainerBase(name) {}
|
||||
|
||||
private:
|
||||
const SolutionBase* wrapped_;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
};
|
||||
|
||||
|
||||
/** Plan for different alternatives in sequence.
|
||||
*
|
||||
* Try to find feasible solutions using first child. Only if this fails,
|
||||
* proceed to the next child trying an alternative planning strategy.
|
||||
* All solutions of the last active child are reported.
|
||||
*/
|
||||
class Fallbacks : public ParallelContainerBase
|
||||
{
|
||||
mutable Stage* active_child_ = nullptr;
|
||||
|
||||
public:
|
||||
Fallbacks(const std::string &name) : ParallelContainerBase(name) {}
|
||||
|
||||
void reset() override;
|
||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
};
|
||||
|
||||
|
||||
class WrapperBasePrivate;
|
||||
/** Base class for Wrapper and Task
|
||||
/** A wrapper wraps a single child stage, which can be accessed via wrapped().
|
||||
*
|
||||
* WrapperBase ensures that only a single child is wrapped in a container.
|
||||
* This child can be accessed via wrapped().
|
||||
* Implementations of this interface need to implement onNewSolution(), which is
|
||||
* called when the child has generated a new solution.
|
||||
* The wrapper may reject the solution or create one or multiple derived solutions,
|
||||
* potentially adapting the cost, as well as its start and end states.
|
||||
*
|
||||
* Care needs to be taken to not modify pulled states, but only pushed ones!
|
||||
* liftFor each new solution, liftSolution() should be called, which comes in various
|
||||
* flavours to allow for handing in new states (or not).
|
||||
*/
|
||||
class WrapperBase : public ContainerBase
|
||||
class WrapperBase : public ParallelContainerBase
|
||||
{
|
||||
public:
|
||||
PRIVATE_CLASS(WrapperBase)
|
||||
WrapperBase(const std::string &name, pointer &&child = Stage::pointer());
|
||||
|
||||
void reset() override;
|
||||
void init(const planning_scene::PlanningSceneConstPtr &scene) override;
|
||||
|
||||
size_t numSolutions() const override;
|
||||
|
||||
/// insertion is only allowed if children() is empty
|
||||
bool insert(Stage::pointer&& stage, int before = -1) override;
|
||||
|
||||
/// access the single wrapped child
|
||||
/// access the single wrapped child, NULL if still empty
|
||||
Stage* wrapped();
|
||||
inline const Stage* wrapped() const {
|
||||
return const_cast<WrapperBase*>(this)->wrapped();
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void onNewSolution(const SolutionBase& s) = 0;
|
||||
|
||||
WrapperBase(WrapperBasePrivate *impl, pointer &&child = Stage::pointer());
|
||||
};
|
||||
|
||||
|
||||
class WrapperPrivate;
|
||||
/** A wrapper wraps a single generator-style stage (and acts itself as a generator).
|
||||
*
|
||||
* The wrapped stage must act as a generator, i.e. only spawn new states
|
||||
* in its external interfaces. It's intended, e.g. to filter or clone
|
||||
* a generated solution of its wrapped generator.
|
||||
*/
|
||||
class Wrapper : public WrapperBase
|
||||
{
|
||||
public:
|
||||
PRIVATE_CLASS(Wrapper)
|
||||
Wrapper(const std::string &name, pointer &&child = Stage::pointer());
|
||||
|
||||
void reset() override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
|
||||
size_t numSolutions() const override;
|
||||
size_t numFailures() const override;
|
||||
void processSolutions(const SolutionProcessor &processor) const override;
|
||||
void processFailures(const SolutionProcessor &processor) const override;
|
||||
|
||||
void spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s);
|
||||
void spawn(InterfaceState &&state, double cost) {
|
||||
std::unique_ptr<SolutionBase> s(new SubTrajectory());
|
||||
s->setCost(cost);
|
||||
spawn(std::move(state), std::move(s));
|
||||
}
|
||||
|
||||
protected:
|
||||
Wrapper(WrapperPrivate* impl, pointer &&child = Stage::pointer());
|
||||
WrapperBase(WrapperBasePrivate* impl, pointer &&child = Stage::pointer());
|
||||
|
||||
/// called by a (direct) child when a new solution becomes available
|
||||
void onNewSolution(const SolutionBase &s) override = 0;
|
||||
|
||||
@ -39,7 +39,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include "utils.h"
|
||||
#include "stage_p.h"
|
||||
|
||||
#include <map>
|
||||
@ -47,13 +46,16 @@
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
/* A container needs to decouple its interfaces from those of its children.
|
||||
* Both, the container and the children have their own starts_ and ends_.
|
||||
* The container needs to forward states received in its interfaces to
|
||||
* the interfaces of the children.
|
||||
* Solutions found by the children, then need to be connected to the
|
||||
* container's interface states. To this end, we remember the mapping
|
||||
* from internal to external states.
|
||||
/* A container needs to decouple its own (external) interfaces
|
||||
* from those (internal) of its children.
|
||||
* Both, the container and the children have their own pull interfaces: starts_ and ends_.
|
||||
* The container forwards states received in its pull interfaces to the
|
||||
* corresponding interfaces of the children.
|
||||
* States pushed by children are temporarily stored in pending_backward_ and pending_forward_.
|
||||
*
|
||||
* Solutions found by the children need to be lifted from the internal level to the
|
||||
* external level. To this end, we remember the mapping from internal to external states.
|
||||
*
|
||||
* Note, that there might be many solutions connecting a single start-end pair.
|
||||
* These solutions might origin from different children (ParallelContainer)
|
||||
* or from different solution paths in a SerialContainer.
|
||||
@ -92,19 +94,40 @@ public:
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
|
||||
InterfacePtr pendingBackward() const { return pending_backward_; }
|
||||
InterfacePtr pendingForward() const { return pending_forward_; }
|
||||
|
||||
protected:
|
||||
ContainerBasePrivate(ContainerBase *me, const std::string &name)
|
||||
: StagePrivate(me, name)
|
||||
{}
|
||||
ContainerBasePrivate(ContainerBase *me, const std::string &name);
|
||||
|
||||
// Get push interface to be used for children: If our own push interface is not set,
|
||||
// don't set children's interface either: pushing is not supported.
|
||||
// Otherwise return pending_* buffer.
|
||||
InterfacePtr getPushBackwardInterface() {
|
||||
return prevEnds() ? pending_backward_ : InterfacePtr();
|
||||
}
|
||||
InterfacePtr getPushForwardInterface() {
|
||||
return nextStarts() ? pending_forward_ : InterfacePtr();
|
||||
}
|
||||
|
||||
/// copy external_state to a child's interface and remember the link in internal_to map
|
||||
void copyState(Interface::iterator external, Stage &child, bool to_start, bool updated);
|
||||
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
|
||||
/// lift solution from internal to external level
|
||||
void liftSolution(SolutionBase& solution,
|
||||
const InterfaceState *internal_from, const InterfaceState *internal_to);
|
||||
|
||||
protected:
|
||||
container_type children_;
|
||||
|
||||
// map start/end states of children (internal) to corresponding states in our external interfaces
|
||||
std::map<const InterfaceState*, Interface::iterator> internal_to_external_;
|
||||
|
||||
/* TODO: these interfaces don't need to be priority-sorted.
|
||||
* Introduce base class UnsortedInterface (which is a plain list) for this use case. */
|
||||
// interface to receive children's sendBackward() states
|
||||
InterfacePtr pending_backward_;
|
||||
// interface to receive children's sendForward() states
|
||||
InterfacePtr pending_forward_;
|
||||
};
|
||||
PIMPL_FUNCTIONS(ContainerBase)
|
||||
|
||||
@ -134,69 +157,74 @@ private:
|
||||
* The solution of a single child stage is usually disconnected to the container's start or end.
|
||||
* Only if all the children in the chain have found a coherent solution from start to end,
|
||||
* this solution can be announced as a solution of the SerialContainer.
|
||||
*
|
||||
* Particularly, the first/last stage's sendBackward()/sendForward() call
|
||||
* cannot directly propagate their associated state to the previous/next stage of this container,
|
||||
* because we cannot provide a full solution yet. Hence, the first/last stage
|
||||
* propagate to the pending_backward_/pending_forward_ interface first.
|
||||
* If eventually a full solution is found, it is propagated to prevEnds()/nextStarts() -
|
||||
* together with the solution. */
|
||||
*/
|
||||
class SerialContainerPrivate : public ContainerBasePrivate {
|
||||
friend class SerialContainer;
|
||||
|
||||
public:
|
||||
SerialContainerPrivate(SerialContainer* me, const std::string &name);
|
||||
|
||||
void storeNewSolution(SerialSolution&& s);
|
||||
const ordered<SerialSolution>& solutions() const { return solutions_; }
|
||||
|
||||
private:
|
||||
void connect(StagePrivate *prev, StagePrivate *next);
|
||||
|
||||
// interface to buffer first child's sendBackward() states
|
||||
InterfacePtr pending_backward_;
|
||||
// interface to buffer last child's sendForward() states
|
||||
InterfacePtr pending_forward_;
|
||||
|
||||
// set of all solutions
|
||||
ordered<SerialSolution> solutions_;
|
||||
};
|
||||
PIMPL_FUNCTIONS(SerialContainer)
|
||||
|
||||
|
||||
/** Wrap an existing solution - for use in parallel containers and wrappers.
|
||||
*
|
||||
* This essentially wraps a solution of a child and thus allows
|
||||
* for new clones of start / end states, which in turn will
|
||||
* have separate incoming/outgoing trajectories */
|
||||
class WrappedSolution : public SolutionBase {
|
||||
public:
|
||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost)
|
||||
: SolutionBase(creator, cost), wrapped_(wrapped)
|
||||
{}
|
||||
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped)
|
||||
: WrappedSolution(creator, wrapped, wrapped->cost())
|
||||
{}
|
||||
void fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
||||
Introspection* introspection = nullptr) const override;
|
||||
|
||||
private:
|
||||
const SolutionBase* wrapped_;
|
||||
};
|
||||
|
||||
|
||||
class ParallelContainerBasePrivate : public ContainerBasePrivate {
|
||||
friend class ParallelContainerBase;
|
||||
|
||||
public:
|
||||
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name);
|
||||
|
||||
private:
|
||||
/// callback for new externally received states
|
||||
void onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated);
|
||||
|
||||
// buffer for wrapped solutions
|
||||
std::list<WrappedSolution> wrapped_solutions_;
|
||||
// buffer for newly created (not wrapped) solutions
|
||||
std::list<SubTrajectory> created_solutions_;
|
||||
// buffer of created states (for use in created solutions)
|
||||
std::list<InterfaceState> states_;
|
||||
|
||||
// cost-ordered set of solutions (pointers into wrapped or created)
|
||||
ordered<SolutionBase*> solutions_;
|
||||
// buffer for failures (pointers into wrapped or created)
|
||||
std::list<SolutionBase*> failures_;
|
||||
};
|
||||
PIMPL_FUNCTIONS(ParallelContainerBase)
|
||||
|
||||
|
||||
class WrapperBasePrivate : public ContainerBasePrivate {
|
||||
class WrapperBasePrivate : public ParallelContainerBasePrivate {
|
||||
friend class WrapperBase;
|
||||
|
||||
public:
|
||||
WrapperBasePrivate(WrapperBase* me, const std::string& name);
|
||||
|
||||
private:
|
||||
InterfacePtr dummy_starts_;
|
||||
InterfacePtr dummy_ends_;
|
||||
};
|
||||
PIMPL_FUNCTIONS(WrapperBase)
|
||||
|
||||
|
||||
class WrapperPrivate : public WrapperBasePrivate {
|
||||
friend class Wrapper;
|
||||
|
||||
public:
|
||||
WrapperPrivate(Wrapper* me, const std::string& name);
|
||||
|
||||
private:
|
||||
ordered<std::unique_ptr<SolutionBase>, pointerLessThan<std::unique_ptr<SolutionBase>>> solutions_;
|
||||
std::list<std::unique_ptr<SolutionBase>> failures_;
|
||||
std::list<InterfaceState> failure_states_;
|
||||
};
|
||||
PIMPL_FUNCTIONS(Wrapper)
|
||||
|
||||
} }
|
||||
|
||||
@ -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 nextStarts() const { return next_starts_.lock(); }
|
||||
|
||||
/// templated access to pull/push interfaces
|
||||
inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
|
||||
inline InterfacePtr pushInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); }
|
||||
inline InterfaceConstPtr pullInterface(Interface::Direction dir) const { return dir == Interface::FORWARD ? starts_ : ends_; }
|
||||
inline InterfaceConstPtr pushInterface(Interface::Direction dir) const { return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock(); }
|
||||
|
||||
/// the following methods should be called only by a container
|
||||
/// to setup the connection structure of their children
|
||||
inline void setHierarchy(ContainerBase* parent, container_type::iterator it) {
|
||||
|
||||
@ -15,7 +15,7 @@ namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class ComputeIK : public Wrapper {
|
||||
class ComputeIK : public WrapperBase {
|
||||
public:
|
||||
ComputeIK(const std::string &name, pointer &&child = Stage::pointer());
|
||||
|
||||
|
||||
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. */
|
||||
class Interface : public ordered<InterfaceState> {
|
||||
public:
|
||||
enum Direction { FORWARD, BACKWARD, START=FORWARD, END=BACKWARD };
|
||||
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
|
||||
Interface(const NotifyFunction ¬ify = NotifyFunction());
|
||||
|
||||
@ -170,6 +171,9 @@ public:
|
||||
inline const InterfaceState* start() const { return start_; }
|
||||
inline const InterfaceState* end() const { return end_; }
|
||||
|
||||
template <Interface::Direction dir>
|
||||
inline const InterfaceState::Solutions& trajectories() const;
|
||||
|
||||
inline void setStartState(const InterfaceState& state){
|
||||
assert(start_ == NULL); // only allow setting once (by Stage)
|
||||
start_ = &state;
|
||||
@ -234,27 +238,34 @@ public:
|
||||
{}
|
||||
|
||||
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
||||
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
|
||||
|
||||
void fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||
Introspection* introspection = nullptr) const override;
|
||||
|
||||
private:
|
||||
// actual trajectory, might be empty
|
||||
const robot_trajectory::RobotTrajectoryPtr trajectory_;
|
||||
robot_trajectory::RobotTrajectoryPtr trajectory_;
|
||||
};
|
||||
|
||||
|
||||
enum TraverseDirection { FORWARD, BACKWARD };
|
||||
template <TraverseDirection dir>
|
||||
const InterfaceState::Solutions& trajectories(const SolutionBase &start);
|
||||
|
||||
template <> inline
|
||||
const InterfaceState::Solutions& trajectories<FORWARD>(const SolutionBase &solution) {
|
||||
return solution.end()->outgoingTrajectories();
|
||||
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
|
||||
return end_->outgoingTrajectories();
|
||||
}
|
||||
template <> inline
|
||||
const InterfaceState::Solutions& trajectories<BACKWARD>(const SolutionBase &solution) {
|
||||
return solution.start()->incomingTrajectories();
|
||||
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::BACKWARD>() const {
|
||||
return start_->incomingTrajectories();
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
namespace std {
|
||||
// comparison for pointers to SolutionBase: compare based on value
|
||||
template<> struct less<moveit::task_constructor::SolutionBase*> {
|
||||
bool operator()(const moveit::task_constructor::SolutionBase* x,
|
||||
const moveit::task_constructor::SolutionBase* y) {
|
||||
return *x < *y;
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
@ -62,9 +62,9 @@ MOVEIT_CLASS_FORWARD(Task)
|
||||
|
||||
/** A Task is the root of a tree of stages.
|
||||
*
|
||||
* Actually a tasks wraps a single container, which serves as the root of all stages.
|
||||
* The wrapped container spawns its solutions into the prevEnds(), nextStarts() interfaces,
|
||||
* which are provided by the wrappers end_ and start_ and interfaces respectively. */
|
||||
* Actually a tasks wraps a single container (by default a SerialContainer),
|
||||
* which serves as the root of all stages.
|
||||
*/
|
||||
class Task : protected WrapperBase {
|
||||
public:
|
||||
Task(const std::string& id = "",
|
||||
@ -76,6 +76,7 @@ public:
|
||||
~Task();
|
||||
|
||||
std::string id() const;
|
||||
const moveit::core::RobotModelPtr getRobotModel() const;
|
||||
|
||||
void add(Stage::pointer &&stage);
|
||||
void clear() override;
|
||||
|
||||
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;
|
||||
|
||||
|
||||
// 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}/utils.h
|
||||
|
||||
${PROJECT_INCLUDE}/solvers/planner_interface.h
|
||||
${PROJECT_INCLUDE}/solvers/cartesian_path.h
|
||||
${PROJECT_INCLUDE}/solvers/pipeline_planner.h
|
||||
|
||||
container.cpp
|
||||
introspection.cpp
|
||||
marker_tools.cpp
|
||||
@ -18,6 +22,10 @@ add_library(${PROJECT_NAME}
|
||||
stage.cpp
|
||||
storage.cpp
|
||||
task.cpp
|
||||
|
||||
solvers/planner_interface.cpp
|
||||
solvers/cartesian_path.cpp
|
||||
solvers/pipeline_planner.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
|
||||
@ -49,6 +49,13 @@ using namespace std::placeholders;
|
||||
|
||||
namespace moveit { namespace task_constructor {
|
||||
|
||||
ContainerBasePrivate::ContainerBasePrivate(ContainerBase *me, const std::string &name)
|
||||
: StagePrivate(me, name)
|
||||
{
|
||||
pending_backward_.reset(new Interface);
|
||||
pending_forward_.reset(new Interface);
|
||||
}
|
||||
|
||||
ContainerBasePrivate::const_iterator ContainerBasePrivate::position(int index) const {
|
||||
const_iterator position = children_.begin();
|
||||
if (index > 0) {
|
||||
@ -90,17 +97,42 @@ bool ContainerBasePrivate::compute()
|
||||
return static_cast<ContainerBase*>(me_)->compute();
|
||||
}
|
||||
|
||||
void ContainerBasePrivate::copyState(Interface::iterator external,
|
||||
Stage &child, bool to_start, bool updated) {
|
||||
if (to_start) {
|
||||
InterfaceState& internal = *child.pimpl()->starts()->clone(*external);
|
||||
internal_to_external_.insert(std::make_pair(&internal, external));
|
||||
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
|
||||
// TODO need to update existing mapping?
|
||||
|
||||
// create a clone of external state within target interface (child's starts() or ends())
|
||||
InterfaceState& internal = *target->clone(*external);
|
||||
// and remember the mapping between them
|
||||
internal_to_external_.insert(std::make_pair(&internal, external));
|
||||
}
|
||||
|
||||
void ContainerBasePrivate::liftSolution(SolutionBase& solution,
|
||||
const InterfaceState *internal_from, const InterfaceState *internal_to)
|
||||
{
|
||||
// add solution to existing or new start state
|
||||
auto it = internal_to_external_.find(internal_from);
|
||||
if (it != internal_to_external_.end()) {
|
||||
// connect solution to existing start state
|
||||
solution.setStartState(*it->second);
|
||||
} else {
|
||||
InterfaceState& internal = *child.pimpl()->ends()->clone(*external);
|
||||
internal_to_external_.insert(std::make_pair(&internal, external));
|
||||
// spawn a new state in previous stage
|
||||
Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution);
|
||||
internal_to_external_.insert(std::make_pair(internal_from, external));
|
||||
}
|
||||
|
||||
// add solution to existing or new end state
|
||||
it = internal_to_external_.find(internal_to);
|
||||
if (it != internal_to_external_.end()) {
|
||||
// connect solution to existing start state
|
||||
solution.setEndState(*it->second);
|
||||
} else {
|
||||
// spawn a new state in next stage
|
||||
Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
|
||||
internal_to_external_.insert(std::make_pair(internal_to, external));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
ContainerBase::ContainerBase(ContainerBasePrivate *impl)
|
||||
: Stage(impl)
|
||||
{
|
||||
@ -139,6 +171,7 @@ bool ContainerBase::insert(Stage::pointer &&stage, int before)
|
||||
bool ContainerBase::remove(int pos)
|
||||
{
|
||||
ContainerBasePrivate::const_iterator it = pimpl()->position(pos);
|
||||
(*it)->pimpl()->setHierarchy(nullptr, ContainerBasePrivate::iterator());
|
||||
pimpl()->children_.erase(it);
|
||||
return true;
|
||||
}
|
||||
@ -156,7 +189,10 @@ void ContainerBase::reset()
|
||||
for (auto& child: impl->children())
|
||||
child->reset();
|
||||
|
||||
// clear mapping
|
||||
// clear buffer interfaces
|
||||
impl->pending_backward_->clear();
|
||||
impl->pending_forward_->clear();
|
||||
// ... and state mapping
|
||||
impl->internal_to_external_.clear();
|
||||
|
||||
Stage::reset();
|
||||
@ -190,15 +226,6 @@ void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
}
|
||||
|
||||
|
||||
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
|
||||
: ContainerBasePrivate(me, name)
|
||||
{
|
||||
// these lists don't need a notify function, connections are handled by onNewSolution()
|
||||
pending_backward_.reset(new Interface(Interface::NotifyFunction()));
|
||||
pending_forward_.reset(new Interface(Interface::NotifyFunction()));
|
||||
}
|
||||
|
||||
|
||||
struct SolutionCollector {
|
||||
SolutionCollector(size_t max_depth) : max_depth(max_depth) {}
|
||||
|
||||
@ -232,11 +259,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||
|
||||
// find all incoming solution pathes ending at current solution
|
||||
SolutionCollector incoming(num_before);
|
||||
traverse<BACKWARD>(current, std::ref(incoming), trace);
|
||||
traverse<Interface::BACKWARD>(current, std::ref(incoming), trace);
|
||||
|
||||
// find all outgoing solution pathes starting at current solution
|
||||
SolutionCollector outgoing(num_after);
|
||||
traverse<FORWARD>(current, std::ref(outgoing), trace);
|
||||
traverse<Interface::FORWARD>(current, std::ref(outgoing), trace);
|
||||
|
||||
// collect (and sort) all solutions spanning from start to end of this container
|
||||
ordered<SerialSolution> sorted;
|
||||
@ -268,42 +295,11 @@ void SerialContainer::onNewSolution(const SolutionBase ¤t)
|
||||
}
|
||||
|
||||
// store new solutions (in sorted)
|
||||
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it)
|
||||
impl->storeNewSolution(std::move(*it));
|
||||
}
|
||||
|
||||
void SerialContainerPrivate::storeNewSolution(SerialSolution &&s)
|
||||
{
|
||||
const InterfaceState *internal_from = s.internalStart();
|
||||
const InterfaceState *internal_to = s.internalEnd();
|
||||
|
||||
// create new SerialSolution and get a reference to it
|
||||
SerialSolution& solution = *solutions_.insert(std::move(s));
|
||||
|
||||
// add solution to existing or new start state
|
||||
auto it = internal_to_external_.find(internal_from);
|
||||
if (it != internal_to_external_.end()) {
|
||||
// connect solution to existing start state
|
||||
solution.setStartState(*it->second);
|
||||
} else {
|
||||
// spawn a new state in previous stage
|
||||
Interface::iterator external = prevEnds()->add(InterfaceState(*internal_from), NULL, &solution);
|
||||
internal_to_external_.insert(std::make_pair(internal_from, external));
|
||||
for (auto it = sorted.begin(), end = sorted.end(); it != end; ++it) {
|
||||
auto inserted = impl->solutions_.insert(std::move(*it));
|
||||
impl->liftSolution(*inserted, inserted->internalStart(), inserted->internalEnd());
|
||||
impl->newSolution(*inserted);
|
||||
}
|
||||
|
||||
// add solution to existing or new end state
|
||||
it = internal_to_external_.find(internal_to);
|
||||
if (it != internal_to_external_.end()) {
|
||||
// connect solution to existing start state
|
||||
solution.setEndState(*it->second);
|
||||
} else {
|
||||
// spawn a new state in next stage
|
||||
Interface::iterator external = nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
|
||||
internal_to_external_.insert(std::make_pair(internal_to, external));
|
||||
}
|
||||
|
||||
// perform default stage action on new solution
|
||||
newSolution(solution);
|
||||
}
|
||||
|
||||
|
||||
@ -320,13 +316,15 @@ void SerialContainer::reset()
|
||||
|
||||
// clear queues
|
||||
impl->solutions_.clear();
|
||||
impl->pending_backward_->clear();
|
||||
impl->pending_forward_->clear();
|
||||
|
||||
// recursively reset children
|
||||
ContainerBase::reset();
|
||||
}
|
||||
|
||||
SerialContainerPrivate::SerialContainerPrivate(SerialContainer *me, const std::string &name)
|
||||
: ContainerBasePrivate(me, name)
|
||||
{}
|
||||
|
||||
void SerialContainerPrivate::connect(StagePrivate* prev, StagePrivate* next) {
|
||||
prev->setNextStarts(next->starts());
|
||||
next->setPrevEnds(prev->ends());
|
||||
@ -342,11 +340,11 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
/*** connect children ***/
|
||||
// first stage sends backward to pending_backward_
|
||||
auto start = impl->children().begin();
|
||||
(*start)->pimpl()->setPrevEnds(impl->pending_backward_);
|
||||
(*start)->pimpl()->setPrevEnds(impl->getPushBackwardInterface());
|
||||
|
||||
// last stage sends forward to pending_forward_
|
||||
auto last = --impl->children().end();
|
||||
(*last)->pimpl()->setNextStarts(impl->pending_forward_);
|
||||
(*last)->pimpl()->setNextStarts(impl->getPushForwardInterface());
|
||||
|
||||
auto cur = start;
|
||||
auto prev = cur; ++cur; // prev points to 1st, cur points to 2nd stage
|
||||
@ -365,19 +363,10 @@ void SerialContainer::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
ContainerBase::init(scene);
|
||||
|
||||
// initialize starts_ and ends_ interfaces
|
||||
Stage* child = start->get();
|
||||
if (child->pimpl()->starts())
|
||||
impl->starts_.reset(new Interface([impl, child](Interface::iterator external, bool updated){
|
||||
// new external state in our starts_ interface is copied to first child
|
||||
impl->copyState(external, *child, true, updated);
|
||||
}));
|
||||
|
||||
child = last->get();
|
||||
if (child->pimpl()->ends())
|
||||
impl->ends_.reset(new Interface([impl, child](Interface::iterator external, bool updated){
|
||||
// new external state in our ends_ interface is copied to last child
|
||||
impl->copyState(external, *child, false, updated);
|
||||
}));
|
||||
if (const InterfacePtr& target = (*start)->pimpl()->starts())
|
||||
impl->starts_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
|
||||
if (const InterfacePtr& target = (*last)->pimpl()->ends())
|
||||
impl->ends_.reset(new Interface(std::bind(&SerialContainerPrivate::copyState, impl, _1, std::cref(target), _2)));
|
||||
|
||||
// validate connectivity of this
|
||||
if (!impl->nextStarts())
|
||||
@ -421,16 +410,16 @@ size_t SerialContainer::numSolutions() const
|
||||
|
||||
void SerialContainer::processSolutions(const ContainerBase::SolutionProcessor &processor) const
|
||||
{
|
||||
for(const SolutionBase& s : pimpl()->solutions())
|
||||
for(const SolutionBase& s : pimpl()->solutions_)
|
||||
if (!processor(s))
|
||||
break;
|
||||
}
|
||||
|
||||
template <TraverseDirection dir>
|
||||
template <Interface::Direction dir>
|
||||
void SerialContainer::traverse(const SolutionBase &start, const SolutionProcessor &cb,
|
||||
solution_container &trace, double trace_cost)
|
||||
{
|
||||
const InterfaceState::Solutions& solutions = trajectories<dir>(start);
|
||||
const InterfaceState::Solutions& solutions = start.trajectories<dir>();
|
||||
if (solutions.empty()) // if we reached the end, call the callback
|
||||
cb(trace, trace_cost);
|
||||
else for (SolutionBase* successor : solutions) {
|
||||
@ -445,7 +434,7 @@ void SerialContainer::traverse(const SolutionBase &start, const SolutionProcesso
|
||||
}
|
||||
|
||||
void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||
Introspection* introspection = nullptr) const
|
||||
Introspection* introspection) const
|
||||
{
|
||||
moveit_task_constructor_msgs::SubSolution sub_msg;
|
||||
sub_msg.id = introspection ? introspection->solutionId(*this) : 0;
|
||||
@ -467,11 +456,20 @@ void SerialSolution::fillMessage(moveit_task_constructor_msgs::Solution &msg,
|
||||
}
|
||||
|
||||
|
||||
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution &solution,
|
||||
Introspection *introspection) const
|
||||
{
|
||||
wrapped_->fillMessage(solution, introspection);
|
||||
}
|
||||
|
||||
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase *me, const std::string &name)
|
||||
: ContainerBasePrivate(me, name)
|
||||
{
|
||||
starts_.reset(new Interface(std::bind(&ParallelContainerBase::onNewStartState, me, _1, _2)));
|
||||
ends_.reset(new Interface(std::bind(&ParallelContainerBase::onNewEndState, me, _1, _2)));
|
||||
}
|
||||
|
||||
void ParallelContainerBasePrivate::onNewExternalState(Interface::Direction dir, Interface::iterator external, bool updated) {
|
||||
for (const Stage::pointer& stage : children())
|
||||
copyState(external, stage->pimpl()->pullInterface(dir), updated);
|
||||
}
|
||||
|
||||
|
||||
@ -486,54 +484,139 @@ void ParallelContainerBase::reset()
|
||||
{
|
||||
// recursively reset children
|
||||
ContainerBase::reset();
|
||||
// clear buffers
|
||||
auto impl = pimpl();
|
||||
impl->solutions_.clear();
|
||||
impl->failures_.clear();
|
||||
impl->wrapped_solutions_.clear();
|
||||
impl->created_solutions_.clear();
|
||||
impl->states_.clear();
|
||||
}
|
||||
|
||||
/* States received by the container need to be copied to all children's pull interfaces.
|
||||
* States generated by children can be directly forwarded into the container's push interfaces.
|
||||
*/
|
||||
void ParallelContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
{
|
||||
InitStageException errors;
|
||||
auto impl = pimpl();
|
||||
|
||||
// connect children such that they directly send to this' prevEnds() / nextStarts()
|
||||
// initialize push connections of children
|
||||
InterfacePtr push_prev = impl->getPushBackwardInterface();
|
||||
InterfacePtr push_next = impl->getPushForwardInterface();
|
||||
for (const Stage::pointer& stage : impl->children()) {
|
||||
StagePrivate *child = stage->pimpl();
|
||||
child->setPrevEnds(impl->prevEnds());
|
||||
child->setNextStarts(impl->nextStarts());
|
||||
child->setPrevEnds(push_prev);
|
||||
child->setNextStarts(push_next);
|
||||
}
|
||||
// recursively init + validate all children
|
||||
// this needs to be done *after* initializing push connections
|
||||
ContainerBase::init(scene);
|
||||
|
||||
bool pulls[2];
|
||||
for (const Stage::pointer& stage : impl->children()) {
|
||||
StagePrivate *child = stage->pimpl();
|
||||
// is there any child reading from starts() or ends() ?
|
||||
pulls[Interface::START] |= bool(child->pullInterface(Interface::START));
|
||||
pulls[Interface::END] |= bool(child->pullInterface(Interface::END));
|
||||
}
|
||||
|
||||
// recursively init + validate all children
|
||||
// this needs to be done *after* initializing the connections
|
||||
ContainerBase::init(scene);
|
||||
// initialize this' pull connections
|
||||
for (Interface::Direction dir : { Interface::START, Interface::END }) {
|
||||
if (pulls[dir])
|
||||
impl->pullInterface(dir).reset(new Interface(std::bind(&ParallelContainerBasePrivate::onNewExternalState, impl, dir, _1, _2)));
|
||||
else
|
||||
impl->pullInterface(dir).reset();
|
||||
}
|
||||
|
||||
if (impl->children().empty())
|
||||
errors.push_back(*this, "no children");
|
||||
|
||||
if (errors)
|
||||
throw errors;
|
||||
}
|
||||
|
||||
void ParallelContainerBase::onNewSolution(const SolutionBase &s)
|
||||
size_t ParallelContainerBase::numSolutions() 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);
|
||||
return pimpl()->solutions_.size();
|
||||
}
|
||||
|
||||
pimpl()->newSolution(s);
|
||||
void ParallelContainerBase::processSolutions(const Stage::SolutionProcessor &processor) const
|
||||
{
|
||||
for(const SolutionBase* s : pimpl()->solutions_)
|
||||
if (!processor(*s))
|
||||
break;
|
||||
}
|
||||
|
||||
size_t ParallelContainerBase::numFailures() const
|
||||
{
|
||||
return pimpl()->failures_.size();
|
||||
}
|
||||
|
||||
void ParallelContainerBase::processFailures(const Stage::SolutionProcessor &processor) const
|
||||
{
|
||||
for(const SolutionBase* f : pimpl()->failures_)
|
||||
if (!processor(*f))
|
||||
break;
|
||||
}
|
||||
|
||||
void ParallelContainerBase::onNewSolution(const SolutionBase& s)
|
||||
{
|
||||
liftSolution(&s);
|
||||
}
|
||||
|
||||
void ParallelContainerBase::liftSolution(const SolutionBase* solution, double cost)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
// create new WrappedSolution instance
|
||||
auto wit = impl->wrapped_solutions_.insert(impl->wrapped_solutions_.end(), WrappedSolution(impl, solution, cost));
|
||||
|
||||
if (wit->isFailure()) {
|
||||
wit->setStartState(*solution->start());
|
||||
wit->setEndState(*solution->end());
|
||||
impl->failures_.push_back(&*wit);
|
||||
} else {
|
||||
impl->solutions_.insert(&*wit);
|
||||
impl->liftSolution(*wit, solution->start(), solution->end());
|
||||
}
|
||||
impl->newSolution(*wit);
|
||||
}
|
||||
|
||||
void ParallelContainerBase::spawn(InterfaceState &&state, SubTrajectory&& t)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
assert(impl->prevEnds() && impl->nextStarts());
|
||||
|
||||
t.setCreator(impl);
|
||||
// store newly created solution (otherwise it's lost)
|
||||
auto it = impl->created_solutions_.insert(impl->created_solutions_.end(), std::move(t));
|
||||
|
||||
if (it->isFailure()) {
|
||||
auto state_it = impl->states_.insert(impl->states_.end(), std::move(state));
|
||||
it->setStartState(*state_it);
|
||||
it->setEndState(*state_it);
|
||||
impl->failures_.push_back(&*it);
|
||||
} else {
|
||||
// directly spawn states in push interfaces
|
||||
impl->prevEnds()->add(InterfaceState(state), NULL, &*it);
|
||||
impl->nextStarts()->add(std::move(state), &*it, NULL);
|
||||
impl->solutions_.insert(&*it);
|
||||
}
|
||||
impl->newSolution(*it);
|
||||
}
|
||||
|
||||
|
||||
WrapperBasePrivate::WrapperBasePrivate(WrapperBase *me, const std::string &name)
|
||||
: ContainerBasePrivate(me, name)
|
||||
{
|
||||
dummy_starts_.reset(new Interface(Interface::NotifyFunction()));
|
||||
dummy_ends_.reset(new Interface(Interface::NotifyFunction()));
|
||||
}
|
||||
: ParallelContainerBasePrivate(me, name)
|
||||
{}
|
||||
|
||||
|
||||
WrapperBase::WrapperBase(const std::string &name, Stage::pointer &&child)
|
||||
: WrapperBase(new WrapperBasePrivate(this, name), std::move(child))
|
||||
{}
|
||||
|
||||
WrapperBase::WrapperBase(WrapperBasePrivate *impl, Stage::pointer &&child)
|
||||
: ContainerBase(impl)
|
||||
: ParallelContainerBase(impl)
|
||||
{
|
||||
if (child) insert(std::move(child));
|
||||
}
|
||||
@ -543,38 +626,7 @@ bool WrapperBase::insert(Stage::pointer &&stage, int before)
|
||||
// restrict num of children to one
|
||||
if (numChildren() > 0)
|
||||
return false;
|
||||
return ContainerBase::insert(std::move(stage), before);
|
||||
}
|
||||
|
||||
void WrapperBase::reset()
|
||||
{
|
||||
pimpl()->dummy_starts_->clear();
|
||||
pimpl()->dummy_ends_->clear();
|
||||
}
|
||||
|
||||
void WrapperBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
|
||||
if (numChildren() != 1)
|
||||
throw InitStageException(*this, "no wrapped child");
|
||||
|
||||
// as a generator-like stage, we don't accept inputs
|
||||
assert(!impl->starts());
|
||||
assert(!impl->ends());
|
||||
|
||||
// provide a dummy interface to receive interface states of wrapped child
|
||||
wrapped()->pimpl()->setPrevEnds(impl->dummy_ends_);
|
||||
wrapped()->pimpl()->setNextStarts(impl->dummy_starts_);
|
||||
|
||||
// init + validate children
|
||||
ContainerBase::init(scene);
|
||||
}
|
||||
|
||||
size_t WrapperBase::numSolutions() const
|
||||
{
|
||||
// dummy implementation needed to allow insert() in constructor
|
||||
return 0;
|
||||
return ParallelContainerBase::insert(std::move(stage), before);
|
||||
}
|
||||
|
||||
Stage* WrapperBase::wrapped()
|
||||
@ -582,88 +634,65 @@ Stage* WrapperBase::wrapped()
|
||||
return pimpl()->children().empty() ? nullptr : pimpl()->children().front().get();
|
||||
}
|
||||
|
||||
void WrapperBase::onNewSolution(const SolutionBase &s)
|
||||
{
|
||||
// update state priorities
|
||||
InterfaceState::Priority prio(1, s.cost());
|
||||
InterfaceState* start = const_cast<InterfaceState*>(s.start());
|
||||
start->owner()->updatePriority(*start, prio);
|
||||
InterfaceState* end = const_cast<InterfaceState*>(s.end());
|
||||
end->owner()->updatePriority(*end, prio);
|
||||
|
||||
pimpl()->newSolution(s);
|
||||
}
|
||||
|
||||
|
||||
WrapperPrivate::WrapperPrivate(Wrapper *me, const std::string &name)
|
||||
: WrapperBasePrivate(me, name)
|
||||
{}
|
||||
|
||||
Wrapper::Wrapper(WrapperPrivate *impl, Stage::pointer &&child)
|
||||
: WrapperBase(impl, std::move(child))
|
||||
{}
|
||||
Wrapper::Wrapper(const std::string &name, Stage::pointer &&child)
|
||||
: Wrapper(new WrapperPrivate(this, name), std::move(child))
|
||||
{}
|
||||
|
||||
void Wrapper::reset()
|
||||
{
|
||||
WrapperBase::reset();
|
||||
pimpl()->solutions_.clear();
|
||||
}
|
||||
|
||||
bool Wrapper::canCompute() const
|
||||
bool WrapperBase::canCompute() const
|
||||
{
|
||||
return wrapped()->pimpl()->canCompute();
|
||||
}
|
||||
|
||||
bool Wrapper::compute()
|
||||
bool WrapperBase::compute()
|
||||
{
|
||||
size_t num_before = numSolutions();
|
||||
wrapped()->pimpl()->compute();
|
||||
return numSolutions() > num_before;
|
||||
}
|
||||
|
||||
size_t Wrapper::numSolutions() const
|
||||
|
||||
bool Alternatives::canCompute() const
|
||||
{
|
||||
return pimpl()->solutions_.size();
|
||||
for (const auto& stage : pimpl()->children())
|
||||
if (stage->pimpl()->canCompute())
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
size_t Wrapper::numFailures() const
|
||||
bool Alternatives::compute()
|
||||
{
|
||||
return pimpl()->failures_.size();
|
||||
bool success = false;
|
||||
for (const auto& stage : pimpl()->children())
|
||||
success |= stage->pimpl()->compute();
|
||||
return success;
|
||||
}
|
||||
|
||||
void Wrapper::processSolutions(const Stage::SolutionProcessor &processor) const
|
||||
|
||||
void Fallbacks::reset()
|
||||
{
|
||||
for(const auto& s : pimpl()->solutions_)
|
||||
if (!processor(*s))
|
||||
break;
|
||||
active_child_ = nullptr;
|
||||
ParallelContainerBase::reset();
|
||||
}
|
||||
|
||||
void Wrapper::processFailures(const Stage::SolutionProcessor &processor) const
|
||||
void Fallbacks::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
{
|
||||
for(const auto& s : pimpl()->failures_)
|
||||
if (!processor(*s))
|
||||
break;
|
||||
ParallelContainerBase::init(scene);
|
||||
active_child_ = pimpl()->children().front().get();
|
||||
}
|
||||
|
||||
void Wrapper::spawn(InterfaceState &&state, std::unique_ptr<SolutionBase>&& s)
|
||||
bool Fallbacks::canCompute() const
|
||||
{
|
||||
auto impl = pimpl();
|
||||
s->setCreator(impl);
|
||||
SolutionBase* solution = s.get();
|
||||
if (s->isFailure()) {
|
||||
impl->failure_states_.emplace_back(std::move(state));
|
||||
s->setStartState(impl->failure_states_.back());
|
||||
s->setEndState(impl->failure_states_.back());
|
||||
impl->failures_.emplace_back(std::move(s));
|
||||
} else {
|
||||
impl->solutions_.insert(std::move(s));
|
||||
impl->prevEnds()->add(InterfaceState(state), NULL, solution);
|
||||
impl->nextStarts()->add(std::move(state), solution, NULL);
|
||||
while (active_child_) {
|
||||
StagePrivate* child = active_child_->pimpl();
|
||||
if (child->canCompute()) return true;
|
||||
|
||||
// active child failed, continue with next
|
||||
auto next = child->it(); ++next;
|
||||
active_child_ = next->get();
|
||||
}
|
||||
impl->newSolution(*solution);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Fallbacks::compute()
|
||||
{
|
||||
if (!active_child_) return false;
|
||||
return active_child_->pimpl()->compute();
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
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));
|
||||
} else if (me()->storeFailures()) {
|
||||
// only store failures when introspection is enabled
|
||||
failures_.emplace_back(std::move(trajectory));
|
||||
return failures_.back();
|
||||
auto it = failures_.insert(failures_.end(), std::move(trajectory));
|
||||
return *it;
|
||||
} else
|
||||
return trajectory;
|
||||
}
|
||||
|
||||
@ -1,16 +1,28 @@
|
||||
add_library(${PROJECT_NAME}_stages
|
||||
${PROJECT_INCLUDE}/stages/cartesian_position_motion.h
|
||||
${PROJECT_INCLUDE}/stages/compute_ik.h
|
||||
${PROJECT_INCLUDE}/stages/current_state.h
|
||||
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
|
||||
${PROJECT_INCLUDE}/stages/connect.h
|
||||
${PROJECT_INCLUDE}/stages/move_to.h
|
||||
${PROJECT_INCLUDE}/stages/move_relative.h
|
||||
|
||||
${PROJECT_INCLUDE}/stages/cartesian_position_motion.h
|
||||
${PROJECT_INCLUDE}/stages/gripper.h
|
||||
${PROJECT_INCLUDE}/stages/modify_planning_scene.h
|
||||
${PROJECT_INCLUDE}/stages/fix_collision_objects.h
|
||||
${PROJECT_INCLUDE}/stages/move.h
|
||||
|
||||
cartesian_position_motion.cpp
|
||||
current_state.cpp
|
||||
compute_ik.cpp
|
||||
generate_grasp_pose.cpp
|
||||
connect.cpp
|
||||
move_to.cpp
|
||||
move_relative.cpp
|
||||
|
||||
cartesian_position_motion.cpp
|
||||
gripper.cpp
|
||||
modify_planning_scene.cpp
|
||||
fix_collision_objects.cpp
|
||||
move.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||
|
||||
@ -20,7 +20,7 @@
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
|
||||
: Wrapper(name, std::move(child))
|
||||
: WrapperBase(name, std::move(child))
|
||||
{
|
||||
auto& p = properties();
|
||||
p.declare<double>("timeout", 1.0);
|
||||
@ -218,17 +218,17 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
start_time = now;
|
||||
|
||||
planning_scene::PlanningSceneConstPtr scene = s.start()->scene();
|
||||
std::unique_ptr<SolutionBase> solution(new SubTrajectory());
|
||||
SubTrajectory solution;
|
||||
|
||||
// include markers from original solution
|
||||
std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution->markers()));
|
||||
std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution.markers()));
|
||||
|
||||
// frame at target pose
|
||||
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
||||
rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame");
|
||||
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
|
||||
|
||||
if (succeeded) {
|
||||
solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
|
||||
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
|
||||
// create a new scene for each solution as they will have different robot states
|
||||
planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff();
|
||||
robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst();
|
||||
@ -240,11 +240,11 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) {
|
||||
marker.ns = "ik solution";
|
||||
marker.color.a *= 0.5;
|
||||
solution->markers().push_back(marker);
|
||||
solution.markers().push_back(marker);
|
||||
};
|
||||
generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames());
|
||||
} else {
|
||||
solution->setCost(std::numeric_limits<double>::infinity());
|
||||
solution.setCost(std::numeric_limits<double>::infinity());
|
||||
}
|
||||
|
||||
spawn(InterfaceState(scene), std::move(solution));
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
const moveit::core::RobotModelPtr Task::getRobotModel() const {
|
||||
return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr();
|
||||
}
|
||||
|
||||
planning_scene::PlanningScenePtr Task::initScene(ros::Duration timeout) {
|
||||
assert(rml_);
|
||||
|
||||
@ -182,10 +186,19 @@ void Task::reset()
|
||||
|
||||
void Task::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||
{
|
||||
WrapperBase::init(scene);
|
||||
auto impl = pimpl();
|
||||
// initialize push connections of wrapped child
|
||||
StagePrivate *child = wrapped()->pimpl();
|
||||
child->setPrevEnds(impl->pendingBackward());
|
||||
child->setNextStarts(impl->pendingForward());
|
||||
|
||||
// explicitly call ContainerBase::init(), not WrapperBase::init()
|
||||
// to keep the just set push interface for the wrapped child
|
||||
ContainerBase::init(scene);
|
||||
|
||||
// provide introspection instance to all stages
|
||||
pimpl()->setIntrospection(introspection_.get());
|
||||
pimpl()->traverseStages([this](Stage& stage, int) {
|
||||
impl->setIntrospection(introspection_.get());
|
||||
impl->traverseStages([this](Stage& stage, int) {
|
||||
stage.pimpl()->setIntrospection(introspection_.get());
|
||||
return true;
|
||||
}, 1, UINT_MAX);
|
||||
@ -224,9 +237,7 @@ bool Task::plan()
|
||||
|
||||
size_t Task::numSolutions() const
|
||||
{
|
||||
auto w = stages();
|
||||
// during initial insert() we call numSolutions(), but wrapped() is not yet defined
|
||||
return w ? w->numSolutions() : 0;
|
||||
return stages()->numSolutions();
|
||||
}
|
||||
|
||||
void Task::processSolutions(const ContainerBase::SolutionProcessor &processor) const
|
||||
@ -242,7 +253,7 @@ void Task::publishAllSolutions(bool wait)
|
||||
|
||||
void Task::onNewSolution(const SolutionBase &s)
|
||||
{
|
||||
WrapperBase::onNewSolution(s);
|
||||
// no need to call WrapperBase::onNewSolution!
|
||||
if (introspection_)
|
||||
introspection_->publishSolution(s);
|
||||
}
|
||||
|
||||
@ -55,7 +55,7 @@ TEST_F(BaseTest, serialContainer) {
|
||||
SerialContainer c("serial");
|
||||
SerialContainerPrivate *cp = c.pimpl();
|
||||
// pretend, that the container is connected
|
||||
InterfacePtr dummy(new Interface(Interface::NotifyFunction()));
|
||||
InterfacePtr dummy(new Interface);
|
||||
cp->setNextStarts(dummy);
|
||||
cp->setPrevEnds(dummy);
|
||||
planning_scene::PlanningScenePtr scene;
|
||||
|
||||
@ -18,8 +18,8 @@ public:
|
||||
robot_model_loader::RobotModelLoader loader;
|
||||
ps.reset(new planning_scene::PlanningScene(loader.getModel()));
|
||||
|
||||
prev.reset(new Interface(Interface::NotifyFunction()));
|
||||
next.reset(new Interface(Interface::NotifyFunction()));
|
||||
prev.reset(new Interface);
|
||||
next.reset(new Interface);
|
||||
pimpl()->setPrevEnds(prev);
|
||||
pimpl()->setNextStarts(next);
|
||||
}
|
||||
|
||||
@ -1,3 +1,4 @@
|
||||
add_subdirectory(utils)
|
||||
add_subdirectory(properties)
|
||||
add_subdirectory(src)
|
||||
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)
|
||||
|
||||
qt_wrap_ui(UIC_FILES task_panel.ui)
|
||||
qt_wrap_ui(UIC_FILES
|
||||
task_panel.ui
|
||||
task_view.ui
|
||||
task_settings.ui
|
||||
)
|
||||
|
||||
add_library(${MOVEIT_LIB_NAME}
|
||||
factory_model.cpp
|
||||
@ -22,7 +26,7 @@ add_library(${MOVEIT_LIB_NAME}
|
||||
|
||||
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
|
||||
target_link_libraries(${MOVEIT_LIB_NAME}
|
||||
motion_planning_tasks_utils moveit_task_visualization_tools
|
||||
motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools
|
||||
${catkin_LIBRARIES} ${QT_LIBRARIES}
|
||||
)
|
||||
target_include_directories(${MOVEIT_LIB_NAME}
|
||||
|
||||
@ -36,6 +36,7 @@
|
||||
|
||||
#include "local_task_model.h"
|
||||
#include "factory_model.h"
|
||||
#include "properties/property_factory.h"
|
||||
#include <moveit/task_constructor/container_p.h>
|
||||
|
||||
#include <ros/console.h>
|
||||
@ -232,4 +233,14 @@ DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex &index)
|
||||
return DisplaySolutionPtr();
|
||||
}
|
||||
|
||||
rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex &index)
|
||||
{
|
||||
Node *n = node(index);
|
||||
if (!n) return nullptr;
|
||||
auto it_inserted = properties_.insert(std::make_pair(n, nullptr));
|
||||
if (it_inserted.second) // newly inserted, create new model
|
||||
it_inserted.first->second = createPropertyTreeModel(n->me()->properties(), this);
|
||||
return it_inserted.first->second;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -49,6 +49,7 @@ class LocalTaskModel
|
||||
typedef moveit::task_constructor::StagePrivate Node;
|
||||
Node *root_;
|
||||
StageFactoryPtr stage_factory_;
|
||||
std::map<Node*, rviz::PropertyTreeModel*> properties_;
|
||||
|
||||
inline Node* node(const QModelIndex &index) const;
|
||||
QModelIndex index(Node *n) const;
|
||||
@ -73,6 +74,8 @@ public:
|
||||
|
||||
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
|
||||
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
|
||||
|
||||
rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -40,6 +40,8 @@
|
||||
#include <moveit/task_constructor/container.h>
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit_task_constructor_msgs/GetSolution.h>
|
||||
#include <rviz/properties/property_tree_model.h>
|
||||
#include <rviz/properties/property.h>
|
||||
#include <ros/console.h>
|
||||
#include <ros/service_client.h>
|
||||
|
||||
@ -64,9 +66,11 @@ struct RemoteTaskModel::Node {
|
||||
InterfaceFlags interface_flags_;
|
||||
NodeFlags node_flags_;
|
||||
std::unique_ptr<RemoteSolutionModel> solutions_;
|
||||
std::unique_ptr<rviz::PropertyTreeModel> properties_;
|
||||
|
||||
inline Node(Node *parent) : parent_(parent) {
|
||||
solutions_.reset(new RemoteSolutionModel());
|
||||
properties_.reset(new rviz::PropertyTreeModel(new rviz::Property()));
|
||||
}
|
||||
|
||||
bool setName(const QString& name) {
|
||||
@ -369,6 +373,13 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
|
||||
return it->second;
|
||||
}
|
||||
|
||||
rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex &index)
|
||||
{
|
||||
Node *n = node(index);
|
||||
if (!n) return nullptr;
|
||||
return n->properties_.get();
|
||||
}
|
||||
|
||||
namespace detail {
|
||||
// method used by sorted_ container (requiring an additional dereference to access id)
|
||||
template <class T>
|
||||
|
||||
@ -87,6 +87,8 @@ public:
|
||||
|
||||
QAbstractItemModel* getSolutionModel(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());
|
||||
for (; first <= last; ++first) {
|
||||
QModelIndex idx = m->index(first, 0, parent);
|
||||
tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data()));
|
||||
tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data()), first);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -50,6 +50,7 @@
|
||||
#include <memory>
|
||||
|
||||
namespace ros { class ServiceClient; }
|
||||
namespace rviz { class PropertyTreeModel; }
|
||||
|
||||
namespace moveit_rviz_plugin {
|
||||
|
||||
@ -84,8 +85,13 @@ public:
|
||||
Qt::ItemFlags flags(const QModelIndex &index) const override;
|
||||
unsigned int taskFlags() const { return flags_; }
|
||||
|
||||
/// get solution model for given stage index
|
||||
virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0;
|
||||
/// get solution for given solution index
|
||||
virtual DisplaySolutionPtr getSolution(const QModelIndex &index) = 0;
|
||||
|
||||
/// get property model for given stage index
|
||||
virtual rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -79,13 +79,15 @@ TaskPanel::TaskPanel(QWidget* parent)
|
||||
: rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this))
|
||||
{
|
||||
Q_D(TaskPanel);
|
||||
// connect signals
|
||||
connect(d->actionRemoveTaskTreeRows, SIGNAL(triggered()), this, SLOT(removeSelectedStages()));
|
||||
connect(d->actionAddLocalTask, SIGNAL(triggered()), this, SLOT(addTask()));
|
||||
connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget()));
|
||||
|
||||
connect(d->tasks_view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)),
|
||||
this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex)));
|
||||
d->tasks_widget = new TaskView(this);
|
||||
d->settings_widget = new TaskSettings(this);
|
||||
layout()->addWidget(d->tasks_widget);
|
||||
layout()->addWidget(d->settings_widget);
|
||||
|
||||
connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget()));
|
||||
connect(d->button_show_settings, SIGNAL(toggled(bool)), d->settings_widget, SLOT(setVisible(bool)));
|
||||
d->settings_widget->setVisible(d->button_show_settings->isChecked());
|
||||
|
||||
// if still undefined, this becomes the global instance
|
||||
if (TaskPanelPrivate::global_instance_.isNull()) {
|
||||
@ -127,14 +129,41 @@ void TaskPanel::decUseCount()
|
||||
|
||||
TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
|
||||
: q_ptr(q_ptr)
|
||||
, settings(new rviz::PropertyTreeModel(new rviz::Property, q_ptr))
|
||||
{
|
||||
setupUi(q_ptr);
|
||||
// init tasks view
|
||||
button_show_stage_dock_widget->setEnabled(bool(getStageFactory()));
|
||||
}
|
||||
|
||||
void TaskPanel::onInitialize()
|
||||
{
|
||||
d_ptr->window_manager_ = vis_manager_->getWindowManager();
|
||||
}
|
||||
|
||||
void TaskPanel::save(rviz::Config config) const
|
||||
{
|
||||
rviz::Panel::save(config);
|
||||
}
|
||||
|
||||
void TaskPanel::load(const rviz::Config& config)
|
||||
{
|
||||
rviz::Panel::load(config);
|
||||
}
|
||||
|
||||
void TaskPanel::showStageDockWidget()
|
||||
{
|
||||
rviz::PanelDockWidget *dock = getStageDockWidget(d_ptr->window_manager_);
|
||||
if (dock) dock->show();
|
||||
}
|
||||
|
||||
|
||||
TaskViewPrivate::TaskViewPrivate(TaskView *q_ptr)
|
||||
: q_ptr(q_ptr)
|
||||
{
|
||||
setupUi(q_ptr);
|
||||
|
||||
MetaTaskListModel *meta_model = &MetaTaskListModel::instance();
|
||||
StageFactoryPtr factory = getStageFactory();
|
||||
if (factory) meta_model->setMimeTypes( { factory->mimeType() } );
|
||||
else button_show_stage_dock_widget->setEnabled(false);
|
||||
tasks_view->setModel(meta_model);
|
||||
|
||||
tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection);
|
||||
@ -147,99 +176,86 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
|
||||
solutions_view->setStretchSection(2);
|
||||
|
||||
// init actions
|
||||
tasks_view->addActions({actionRemoveTaskTreeRows, actionAddLocalTask});
|
||||
|
||||
initSettings(settings->getRoot());
|
||||
settings_view->setModel(settings);
|
||||
}
|
||||
|
||||
void TaskPanelPrivate::initSettings(rviz::Property* root)
|
||||
{
|
||||
tasks_view->addActions({actionAddLocalTask, actionRemoveTaskTreeRows});
|
||||
}
|
||||
|
||||
std::pair<TaskListModel*, TaskDisplay*>
|
||||
TaskPanelPrivate::getTaskListModel(const QModelIndex &index) const
|
||||
TaskViewPrivate::getTaskListModel(const QModelIndex &index) const
|
||||
{
|
||||
auto *meta_model = static_cast<MetaTaskListModel*>(tasks_view->model());
|
||||
return meta_model->getTaskListModel(index);
|
||||
}
|
||||
|
||||
std::pair<BaseTaskModel*, QModelIndex>
|
||||
TaskPanelPrivate::getTaskModel(const QModelIndex &index) const
|
||||
TaskViewPrivate::getTaskModel(const QModelIndex &index) const
|
||||
{
|
||||
auto *meta_model = static_cast<MetaTaskListModel*>(tasks_view->model());
|
||||
return meta_model->getTaskModel(index);
|
||||
}
|
||||
|
||||
void TaskPanelPrivate::unlock(TaskDisplay* display)
|
||||
void TaskViewPrivate::lock(TaskDisplay* display)
|
||||
{
|
||||
if (locked_display_ && locked_display_ != display) {
|
||||
locked_display_->clearMarkers();
|
||||
locked_display_->visualization()->unlock();
|
||||
locked_display_ = display;
|
||||
}
|
||||
locked_display_ = display;
|
||||
}
|
||||
|
||||
void TaskPanel::onInitialize()
|
||||
TaskView::TaskView(QWidget *parent)
|
||||
: QWidget(parent), d_ptr(new TaskViewPrivate(this))
|
||||
{
|
||||
d_ptr->window_manager_ = vis_manager_->getWindowManager();
|
||||
Q_D(TaskView);
|
||||
|
||||
// connect signals
|
||||
connect(d->actionRemoveTaskTreeRows, SIGNAL(triggered()), this, SLOT(removeSelectedStages()));
|
||||
connect(d->actionAddLocalTask, SIGNAL(triggered()), this, SLOT(addTask()));
|
||||
|
||||
connect(d->tasks_view->selectionModel(), SIGNAL(currentChanged(QModelIndex, QModelIndex)),
|
||||
this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex)));
|
||||
|
||||
onCurrentStageChanged(d->tasks_view->currentIndex(), QModelIndex());
|
||||
}
|
||||
|
||||
void TaskPanel::save(rviz::Config config) const
|
||||
void TaskView::addTask()
|
||||
{
|
||||
rviz::Panel::save(config);
|
||||
d_ptr->settings->getRoot()->save(config);
|
||||
}
|
||||
|
||||
void TaskPanel::load(const rviz::Config& config)
|
||||
{
|
||||
rviz::Panel::load(config);
|
||||
d_ptr->settings->getRoot()->load(config);
|
||||
}
|
||||
|
||||
void TaskPanel::addTask()
|
||||
{
|
||||
TaskListModel* task_list_model = nullptr;
|
||||
QModelIndex current = d_ptr->tasks_view->currentIndex();
|
||||
if (!current.isValid()) {
|
||||
// create new TaskDisplay
|
||||
TaskDisplay *display = new TaskDisplay();
|
||||
display->setName("Motion Planning Task");
|
||||
vis_manager_->getRootDisplayGroup()->addDisplay(display);
|
||||
display->initialize(vis_manager_);
|
||||
display->setEnabled(true);
|
||||
if (!current.isValid()) return;
|
||||
bool is_top_level = !current.parent().isValid();
|
||||
|
||||
task_list_model = &display->getTaskListModel();
|
||||
} else
|
||||
task_list_model = d_ptr->getTaskListModel(current).first;
|
||||
TaskListModel* task_list_model = d_ptr->getTaskListModel(current).first;
|
||||
task_list_model->insertModel(new LocalTaskModel(task_list_model), is_top_level ? -1 : current.row());
|
||||
|
||||
task_list_model->insertModel(new LocalTaskModel(task_list_model), current.row());
|
||||
// select and edit newly inserted model
|
||||
if (is_top_level) current = current.model()->index(task_list_model->rowCount()-1, 0, current);
|
||||
d_ptr->tasks_view->scrollTo(current);
|
||||
d_ptr->tasks_view->setCurrentIndex(current);
|
||||
d_ptr->tasks_view->edit(current);
|
||||
}
|
||||
|
||||
void TaskPanel::showStageDockWidget()
|
||||
{
|
||||
rviz::PanelDockWidget *dock = getStageDockWidget(d_ptr->window_manager_);
|
||||
if (dock) dock->show();
|
||||
}
|
||||
|
||||
void TaskPanel::removeSelectedStages()
|
||||
void TaskView::removeSelectedStages()
|
||||
{
|
||||
auto *m = d_ptr->tasks_view->model();
|
||||
for (const auto &range : d_ptr->tasks_view->selectionModel()->selection())
|
||||
m->removeRows(range.top(), range.bottom()-range.top()+1, range.parent());
|
||||
}
|
||||
|
||||
void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous)
|
||||
void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelIndex &previous)
|
||||
{
|
||||
// adding task is allowed on top-level items and sub-top-level items
|
||||
d_ptr->actionAddLocalTask->setEnabled(current.isValid() &&
|
||||
(!current.parent().isValid() || !current.parent().parent().isValid()));
|
||||
// removing stuff is allowed if there is any selection / any curren item
|
||||
d_ptr->actionRemoveTaskTreeRows->setEnabled(current.isValid());
|
||||
|
||||
BaseTaskModel *task;
|
||||
QModelIndex task_index;
|
||||
std::tie(task, task_index) = d_ptr->getTaskModel(current);
|
||||
d_ptr->actionRemoveTaskTreeRows->setEnabled(task != nullptr);
|
||||
|
||||
// TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second;
|
||||
d_ptr->unlock(nullptr);
|
||||
d_ptr->lock(nullptr); // unlocks any locked_display_
|
||||
|
||||
auto *view = d_ptr->solutions_view;
|
||||
// update the SolutionModel
|
||||
QTreeView *view = d_ptr->solutions_view;
|
||||
QItemSelectionModel *sm = view->selectionModel();
|
||||
QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr;
|
||||
view->sortByColumn(-1);
|
||||
@ -253,12 +269,19 @@ void TaskPanel::onCurrentStageChanged(const QModelIndex ¤t, const QModelIn
|
||||
connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)),
|
||||
this, SLOT(onSolutionSelectionChanged(QItemSelection, QItemSelection)));
|
||||
}
|
||||
|
||||
// update the PropertyModel
|
||||
view = d_ptr->property_view;
|
||||
sm = view->selectionModel();
|
||||
m = task ? task->getPropertyModel(task_index) : nullptr;
|
||||
view->setModel(m);
|
||||
delete sm; // we don't store the selection model
|
||||
}
|
||||
|
||||
void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous)
|
||||
void TaskView::onCurrentSolutionChanged(const QModelIndex ¤t, const QModelIndex &previous)
|
||||
{
|
||||
TaskDisplay *display = d_ptr->getTaskListModel(d_ptr->tasks_view->currentIndex()).second;
|
||||
d_ptr->unlock(display);
|
||||
d_ptr->lock(display);
|
||||
|
||||
if (!display || !current.isValid())
|
||||
return;
|
||||
@ -266,7 +289,6 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode
|
||||
BaseTaskModel *task = d_ptr->getTaskModel(d_ptr->tasks_view->currentIndex()).first;
|
||||
Q_ASSERT(task);
|
||||
|
||||
d_ptr->locked_display_ = display;
|
||||
TaskSolutionVisualization* vis = display->visualization();
|
||||
const DisplaySolutionPtr& solution = task->getSolution(current);
|
||||
display->setSolutionStatus(bool(solution));
|
||||
@ -274,7 +296,7 @@ void TaskPanel::onCurrentSolutionChanged(const QModelIndex ¤t, const QMode
|
||||
vis->showTrajectory(solution, true);
|
||||
}
|
||||
|
||||
void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected)
|
||||
void TaskView::onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected)
|
||||
{
|
||||
QItemSelectionModel *sm = d_ptr->solutions_view->selectionModel();
|
||||
const QModelIndexList& selected_rows = sm->selectedRows();
|
||||
@ -292,6 +314,19 @@ void TaskPanel::onSolutionSelectionChanged(const QItemSelection &selected, const
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TaskSettingsPrivate::TaskSettingsPrivate(TaskSettings *q_ptr)
|
||||
: q_ptr(q_ptr)
|
||||
{
|
||||
setupUi(q_ptr);
|
||||
}
|
||||
|
||||
TaskSettings::TaskSettings(QWidget *parent)
|
||||
: QWidget(parent), d_ptr(new TaskSettingsPrivate(this))
|
||||
{
|
||||
Q_D(TaskSettings);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include "moc_task_panel.cpp"
|
||||
|
||||
@ -82,9 +82,23 @@ public:
|
||||
void load(const rviz::Config& config) override;
|
||||
void save(rviz::Config config) const override;
|
||||
|
||||
protected Q_SLOTS:
|
||||
void showStageDockWidget();
|
||||
};
|
||||
|
||||
|
||||
class MetaTaskListModel;
|
||||
class TaskViewPrivate;
|
||||
class TaskView : public QWidget {
|
||||
Q_OBJECT
|
||||
Q_DECLARE_PRIVATE(TaskView)
|
||||
TaskViewPrivate *d_ptr;
|
||||
|
||||
public:
|
||||
TaskView(QWidget* parent = 0);
|
||||
|
||||
public Q_SLOTS:
|
||||
void addTask();
|
||||
void showStageDockWidget();
|
||||
|
||||
protected Q_SLOTS:
|
||||
void removeSelectedStages();
|
||||
@ -93,4 +107,15 @@ protected Q_SLOTS:
|
||||
void onSolutionSelectionChanged(const QItemSelection &selected, const QItemSelection &deselected);
|
||||
};
|
||||
|
||||
|
||||
class TaskSettingsPrivate;
|
||||
class TaskSettings : public QWidget {
|
||||
Q_OBJECT
|
||||
Q_DECLARE_PRIVATE(TaskSettings)
|
||||
TaskSettingsPrivate *d_ptr;
|
||||
|
||||
public:
|
||||
TaskSettings(QWidget* parent = 0);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -14,9 +14,6 @@
|
||||
<string>Tasks Panel</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
@ -30,193 +27,49 @@
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QTabWidget" name="tabWidget">
|
||||
<property name="currentIndex">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="tab_tasks">
|
||||
<attribute name="icon">
|
||||
<iconset resource="resources.qrc">
|
||||
<normaloff>:/icons/tasks.png</normaloff>:/icons/tasks.png</iconset>
|
||||
</attribute>
|
||||
<attribute name="title">
|
||||
<string/>
|
||||
</attribute>
|
||||
<layout class="QVBoxLayout" name="tab_tasks_layout">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||
<item>
|
||||
<widget class="QToolButton" name="button_show_settings">
|
||||
<property name="text">
|
||||
<string>...</string>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
<property name="icon">
|
||||
<iconset resource="resources.qrc">
|
||||
<normaloff>:/icons/settings.png</normaloff>:/icons/settings.png</iconset>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
<property name="checkable">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="horizontalSpacer">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<number>0</number>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
<item>
|
||||
<layout class="QHBoxLayout" name="tasks_view_label_layout">
|
||||
<property name="spacing">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QLabel" name="tasks_view_label">
|
||||
<property name="text">
|
||||
<string>Task Tree</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QToolButton" name="button_show_stage_dock_widget">
|
||||
<property name="maximumSize">
|
||||
<size>
|
||||
<width>18</width>
|
||||
<height>18</height>
|
||||
</size>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
<property name="icon">
|
||||
<iconset resource="resources.qrc">
|
||||
<normaloff>:/icons/new-stage.png</normaloff>:/icons/new-stage.png</iconset>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QSplitter" name="splitter">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Expanding" vsizetype="Expanding">
|
||||
<horstretch>0</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<widget class="moveit_rviz_plugin::TaskListView" name="tasks_view">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Expanding">
|
||||
<horstretch>2</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="contextMenuPolicy">
|
||||
<enum>Qt::ActionsContextMenu</enum>
|
||||
</property>
|
||||
<attribute name="headerStretchLastSection">
|
||||
<bool>false</bool>
|
||||
</attribute>
|
||||
</widget>
|
||||
<widget class="moveit_rviz_plugin::AutoAdjustingTreeView" name="solutions_view">
|
||||
<property name="sizePolicy">
|
||||
<sizepolicy hsizetype="Preferred" vsizetype="Expanding">
|
||||
<horstretch>1</horstretch>
|
||||
<verstretch>0</verstretch>
|
||||
</sizepolicy>
|
||||
</property>
|
||||
<property name="selectionMode">
|
||||
<enum>QAbstractItemView::ExtendedSelection</enum>
|
||||
</property>
|
||||
<property name="rootIsDecorated">
|
||||
<bool>false</bool>
|
||||
</property>
|
||||
<property name="uniformRowHeights">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="sortingEnabled">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<attribute name="headerStretchLastSection">
|
||||
<bool>false</bool>
|
||||
</attribute>
|
||||
</widget>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
<widget class="QWidget" name="tab_settings">
|
||||
<attribute name="icon">
|
||||
<iconset resource="resources.qrc">
|
||||
<normaloff>:/icons/settings.png</normaloff>:/icons/settings.png</iconset>
|
||||
</attribute>
|
||||
<attribute name="title">
|
||||
<string/>
|
||||
</attribute>
|
||||
<layout class="QVBoxLayout" name="tab_settings_layout">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QToolButton" name="button_show_stage_dock_widget">
|
||||
<property name="text">
|
||||
<string>...</string>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
<property name="icon">
|
||||
<iconset resource="resources.qrc">
|
||||
<normaloff>:/icons/new-stage.png</normaloff>:/icons/new-stage.png</iconset>
|
||||
</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>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
<action name="actionRemoveTaskTreeRows">
|
||||
<property name="text">
|
||||
<string>Remove</string>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Remove selected rows</string>
|
||||
</property>
|
||||
<property name="shortcut">
|
||||
<string>Del</string>
|
||||
</property>
|
||||
<property name="shortcutContext">
|
||||
<enum>Qt::WidgetShortcut</enum>
|
||||
</property>
|
||||
</action>
|
||||
<action name="actionAddLocalTask">
|
||||
<property name="text">
|
||||
<string>Add task</string>
|
||||
</property>
|
||||
</action>
|
||||
</widget>
|
||||
<customwidgets>
|
||||
<customwidget>
|
||||
<class>rviz::PropertyTreeWidget</class>
|
||||
<extends>QTreeView</extends>
|
||||
<header location="global">rviz/properties/property_tree_widget.h</header>
|
||||
</customwidget>
|
||||
<customwidget>
|
||||
<class>moveit_rviz_plugin::TaskListView</class>
|
||||
<extends>QTreeView</extends>
|
||||
<header>task_list_model.h</header>
|
||||
</customwidget>
|
||||
<customwidget>
|
||||
<class>moveit_rviz_plugin::AutoAdjustingTreeView</class>
|
||||
<extends>QTreeView</extends>
|
||||
<header>task_list_model.h</header>
|
||||
</customwidget>
|
||||
</customwidgets>
|
||||
<resources>
|
||||
<include location="resources.qrc"/>
|
||||
</resources>
|
||||
|
||||
@ -40,6 +40,8 @@
|
||||
|
||||
#include "task_panel.h"
|
||||
#include "ui_task_panel.h"
|
||||
#include "ui_task_view.h"
|
||||
#include "ui_task_settings.h"
|
||||
|
||||
#include <rviz/panel.h>
|
||||
#include <rviz/properties/property_tree_model.h>
|
||||
@ -55,7 +57,19 @@ class TaskPanelPrivate : public Ui_TaskPanel {
|
||||
public:
|
||||
TaskPanelPrivate(TaskPanel *q_ptr);
|
||||
|
||||
void initSettings(rviz::Property *root);
|
||||
TaskPanel* q_ptr;
|
||||
TaskView* tasks_widget;
|
||||
TaskSettings* settings_widget;
|
||||
|
||||
rviz::WindowManagerInterface* window_manager_;
|
||||
static QPointer<TaskPanel> global_instance_;
|
||||
static uint global_use_count_;
|
||||
};
|
||||
|
||||
|
||||
class TaskViewPrivate : public Ui_TaskView {
|
||||
public:
|
||||
TaskViewPrivate(TaskView *q_ptr);
|
||||
|
||||
/// retrieve TaskListModel corresponding to given index
|
||||
inline std::pair<TaskListModel*, TaskDisplay*>
|
||||
@ -66,15 +80,18 @@ public:
|
||||
getTaskModel(const QModelIndex& index) const;
|
||||
|
||||
/// unlock locked_display_ if given display is different
|
||||
void unlock(TaskDisplay *display);
|
||||
void lock(TaskDisplay *display);
|
||||
|
||||
TaskPanel* q_ptr;
|
||||
rviz::PropertyTreeModel* settings;
|
||||
TaskView *q_ptr;
|
||||
QPointer<TaskDisplay> locked_display_;
|
||||
rviz::WindowManagerInterface* window_manager_;
|
||||
};
|
||||
|
||||
static QPointer<TaskPanel> global_instance_;
|
||||
static uint global_use_count_;
|
||||
|
||||
class TaskSettingsPrivate : public Ui_TaskSettings {
|
||||
public:
|
||||
TaskSettingsPrivate(TaskSettings *q_ptr);
|
||||
|
||||
TaskSettings *q_ptr;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
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