Introduce clang-format (#102)

* fix catkin_lint issues
* introduce clang-format config and apply it
This commit is contained in:
Michael Görner 2019-07-18 11:04:35 +02:00 committed by Robert Haschke
parent 7ca49bc29b
commit f7b259259e
124 changed files with 4929 additions and 5343 deletions

64
.clang-format Normal file
View File

@ -0,0 +1,64 @@
---
Language: Cpp
BasedOnStyle: Google
AccessModifierOffset: -3
AlignAfterOpenBracket: Align
AlignEscapedNewlinesLeft: true
AlignTrailingComments: false
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortFunctionsOnASingleLine: Inline
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AlwaysBreakBeforeMultilineStrings: false
AlwaysBreakTemplateDeclarations: true
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterClass: true
AfterControlStatement: false
AfterEnum: true
AfterFunction: false
AfterNamespace: false
AfterStruct: true
AfterUnion: true
BeforeCatch: false
BeforeElse: false
IndentBraces: false
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Custom
BreakBeforeTernaryOperators: false
BreakConstructorInitializersBeforeComma: true
ColumnLimit: 120
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 2
ContinuationIndentWidth: 4
Cpp11BracedListStyle: false
DerivePointerBinding: false
ExperimentalAutoDetectBinPacking: false
IndentCaseLabels: true
IndentFunctionDeclarationAfterType: false
IndentWidth: 3
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 60
PenaltyBreakFirstLessLess: 1000
PenaltyBreakString: 100
PenaltyExcessCharacter: 1000
PenaltyReturnTypeOnItsOwnLine: 70
PointerBindsToType: true
SortIncludes: false
SpaceAfterCStyleCast: false
SpaceAfterControlStatementKeyword: false
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: ControlStatements
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
Standard: Auto
TabWidth: 3
UseTab: ForIndentation
...

View File

@ -17,6 +17,7 @@ env:
- ROS_DISTRO=melodic
- UPSTREAM_WORKSPACE=.rosinstall
matrix:
- TEST=clang-format
- ROS_REPO=ros-shadow-fixed
- DOCKER_IMAGE=moveit/moveit:master-source

View File

@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
LIBRARIES $PROJECT_NAME}
CATKIN_DEPENDS
moveit_task_constructor_msgs
std_msgs
)

View File

@ -1,5 +1,4 @@
<?xml version="1.0"?>
<package>
<package format="2">
<name>moveit_task_constructor_capabilities</name>
<version>0.0.0</version>
<description>
@ -13,16 +12,12 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>moveit_core</build_depend>
<build_depend>moveit_ros_move_group</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>moveit_task_constructor_msgs</build_depend>
<run_depend>moveit_core</run_depend>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>std_msgs</run_depend>
<depend>moveit_core</depend>
<depend>moveit_ros_move_group</depend>
<depend>actionlib</depend>
<depend>pluginlib</depend>
<depend>std_msgs</depend>
<depend>moveit_task_constructor_msgs</depend>
<export>
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>

View File

@ -45,23 +45,23 @@
namespace {
// TODO: move to moveit::core::RobotModel
const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::RobotModel& model, const std::vector<std::string>& joints) {
const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::RobotModel& model,
const std::vector<std::string>& joints) {
std::set<std::string> joint_set(joints.begin(), joints.end());
const std::vector<const moveit::core::JointModelGroup*>& jmgs = model.getJointModelGroups();
for(const moveit::core::JointModelGroup* jmg : jmgs){
for (const moveit::core::JointModelGroup* jmg : jmgs) {
const std::vector<std::string>& jmg_joints = jmg->getJointModelNames();
std::set<std::string> jmg_joint_set(jmg_joints.begin(), jmg_joints.end());
// return group if sets agree on all active joints
if(std::includes(jmg_joint_set.begin(), jmg_joint_set.end(), joint_set.begin(), joint_set.end())) {
if (std::includes(jmg_joint_set.begin(), jmg_joint_set.end(), joint_set.begin(), joint_set.end())) {
std::set<std::string> difference;
std::set_difference(jmg_joint_set.begin(), jmg_joint_set.end(),
joint_set.begin(), joint_set.end(),
std::set_difference(jmg_joint_set.begin(), jmg_joint_set.end(), joint_set.begin(), joint_set.end(),
std::inserter(difference, difference.begin()));
unsigned int acceptable = 0;
for(const std::string& diff_joint : difference){
for (const std::string& diff_joint : difference) {
const moveit::core::JointModel* diff_jm = model.getJointModel(diff_joint);
if (diff_jm->isPassive() || diff_jm->getMimic() || diff_jm->getType() == moveit::core::JointModel::FIXED)
++acceptable;
@ -73,28 +73,23 @@ const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::Rob
return nullptr;
}
}
namespace move_group {
namespace move_group
{
ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability()
: MoveGroupCapability("ExecuteTaskSolution")
{}
ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability() : MoveGroupCapability("ExecuteTaskSolution") {}
void ExecuteTaskSolutionCapability::initialize() {
// configure the action server
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>(
root_node_handle_, "execute_task_solution",
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false));
as_->registerPreemptCallback(
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this));
root_node_handle_, "execute_task_solution",
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false));
as_->registerPreemptCallback(std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this));
as_->start();
}
void ExecuteTaskSolutionCapability::goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
void ExecuteTaskSolutionCapability::goalCallback(
const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
moveit_task_constructor_msgs::ExecuteTaskSolutionResult result;
if (!context_->plan_execution_) {
@ -105,7 +100,7 @@ void ExecuteTaskSolutionCapability::goalCallback(const moveit_task_constructor_m
}
plan_execution::ExecutableMotionPlan plan;
if(!constructMotionPlan(goal->solution, plan))
if (!constructMotionPlan(goal->solution, plan))
result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
else {
ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution");
@ -123,7 +118,7 @@ void ExecuteTaskSolutionCapability::goalCallback(const moveit_task_constructor_m
}
void ExecuteTaskSolutionCapability::preemptCallback() {
if(context_->plan_execution_)
if (context_->plan_execution_)
context_->plan_execution_->stop();
}
@ -134,54 +129,55 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
robot_state::RobotState state(model);
{
planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
state= scene->getCurrentState();
state = scene->getCurrentState();
}
plan.plan_components_.reserve(solution.sub_trajectory.size());
for(size_t i= 0; i < solution.sub_trajectory.size(); ++i) {
for (size_t i = 0; i < solution.sub_trajectory.size(); ++i) {
const moveit_task_constructor_msgs::SubTrajectory& sub_traj = solution.sub_trajectory[i];
plan.plan_components_.emplace_back();
plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
// define individual variable for use in closure below
const std::string description = std::to_string(i+1) + "/"
+ std::to_string(solution.sub_trajectory.size())
+ " - subsolution " + std::to_string(sub_traj.info.id)
+ " of stage " + std::to_string(sub_traj.info.stage_id);
const std::string description = std::to_string(i + 1) + "/" + std::to_string(solution.sub_trajectory.size()) +
" - subsolution " + std::to_string(sub_traj.info.id) + " of stage " +
std::to_string(sub_traj.info.stage_id);
exec_traj.description_ = description;
const moveit::core::JointModelGroup* group = nullptr;
{
std::vector<std::string> joint_names(sub_traj.trajectory.joint_trajectory.joint_names);
joint_names.insert(joint_names.end(),
sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.begin(),
joint_names.insert(joint_names.end(), sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.begin(),
sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.end());
if (joint_names.size()) {
group = findJointModelGroup(*model, joint_names);
if(!group){
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" << boost::algorithm::join(joint_names, ", ") << "}" );
if (!group) {
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {"
<< boost::algorithm::join(joint_names, ", ") << "}");
return false;
}
ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution", group->getName().c_str());
ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution",
group->getName().c_str());
}
}
exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory);
/* TODO add action feedback and markers */
exec_traj.effect_on_success_ = [this,sub_traj,description](const plan_execution::ExecutableMotionPlan*){
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)){
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description );
exec_traj.effect_on_success_ = [this, sub_traj, description](const plan_execution::ExecutableMotionPlan*) {
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
}
return true;
};
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)){
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "invalid intermediate robot state in scene diff of SubTrajectory " << description);
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) {
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution",
"invalid intermediate robot state in scene diff of SubTrajectory " << description);
return false;
}
}

View File

@ -48,8 +48,7 @@
#include <memory>
namespace move_group
{
namespace move_group {
class ExecuteTaskSolutionCapability : public MoveGroupCapability
{
@ -59,7 +58,8 @@ public:
virtual void initialize();
private:
bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, plan_execution::ExecutableMotionPlan& plan);
bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
plan_execution::ExecutableMotionPlan& plan);
void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
void preemptCallback();
@ -67,4 +67,4 @@ private:
std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>> as_;
};
} // namespace move_group
} // namespace move_group

View File

@ -40,7 +40,8 @@
#include "stage.h"
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
class ContainerBasePrivate;
/** Base class for all container stages, i.e. ones that have one or more children */
@ -55,9 +56,9 @@ public:
typedef std::function<bool(const Stage&, int depth)> StageCallback;
/// traverse direct children of this container, calling the callback for each of them
bool traverseChildren(const StageCallback &processor) const;
bool traverseChildren(const StageCallback& processor) const;
/// traverse all children of this container recursively
bool traverseRecursively(const StageCallback &processor) const;
bool traverseRecursively(const StageCallback& processor) const;
virtual bool insert(Stage::pointer&& stage, int before = -1);
virtual bool remove(int pos);
@ -78,7 +79,6 @@ protected:
};
std::ostream& operator<<(std::ostream& os, const ContainerBase& stage);
class SerialContainerPrivate;
/** SerialContainer allows to sequentially chain a set of child stages */
class SerialContainer : public ContainerBase
@ -94,24 +94,23 @@ public:
protected:
/// called by a (direct) child when a new solution becomes available
void onNewSolution(const SolutionBase &s) override;
void onNewSolution(const SolutionBase& s) override;
typedef std::function<void(const SolutionSequence::container_type &trace,
double trace_accumulated_cost)> SolutionProcessor;
typedef std::function<void(const SolutionSequence::container_type& trace, double trace_accumulated_cost)>
SolutionProcessor;
/// Traverse all solution pathes starting at start and going in given direction dir
/// 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<Interface::Direction dir>
void traverse(const SolutionBase &start, const SolutionProcessor &cb,
SolutionSequence::container_type &trace, double trace_cost = 0);
template <Interface::Direction dir>
void traverse(const SolutionBase& start, const SolutionProcessor& cb, SolutionSequence::container_type& trace,
double trace_cost = 0);
protected:
SerialContainer(SerialContainerPrivate* impl);
};
class ParallelContainerBasePrivate;
class ParallelContainerBase;
/** Parallel containers allow for alternative planning stages
@ -125,7 +124,7 @@ class ParallelContainerBase : public ContainerBase
{
public:
PRIVATE_CLASS(ParallelContainerBase)
ParallelContainerBase(const std::string &name = "parallel container");
ParallelContainerBase(const std::string& name = "parallel container");
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
@ -133,26 +132,21 @@ protected:
ParallelContainerBase(ParallelContainerBasePrivate* impl);
/// lift unmodified child solution (useful for simple filtering)
inline void liftSolution(const SolutionBase& solution) {
liftSolution(solution, solution.cost());
}
inline void liftSolution(const SolutionBase& solution) { liftSolution(solution, solution.cost()); }
/// lift child solution to external interface, adapting the costs
void liftSolution(const SolutionBase& solution, double cost) {
liftSolution(solution, cost, solution.comment());
}
void liftSolution(const SolutionBase& solution, double cost) { liftSolution(solution, cost, solution.comment()); }
/// lift child solution to external interface, adapting the costs and comment
void liftSolution(const SolutionBase& solution, double cost, std::string comment);
/// spawn a new solution with given state as start and end
void spawn(InterfaceState &&state, SubTrajectory&& trajectory);
void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
/// propagate a solution forwards
void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
/// propagate a solution backwards
void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory);
};
/** Plan for different alternatives in parallel.
*
* Solution of all children are reported - sorted by cost.
@ -160,7 +154,7 @@ protected:
class Alternatives : public ParallelContainerBase
{
public:
Alternatives(const std::string &name = "alternatives") : ParallelContainerBase(name) {}
Alternatives(const std::string& name = "alternatives") : ParallelContainerBase(name) {}
bool canCompute() const override;
void compute() override;
@ -168,7 +162,6 @@ public:
void onNewSolution(const SolutionBase& s) override;
};
/** Plan for different alternatives in sequence.
*
* Try to find feasible solutions using first child. Only if this fails,
@ -180,7 +173,7 @@ class Fallbacks : public ParallelContainerBase
mutable Stage* active_child_ = nullptr;
public:
Fallbacks(const std::string &name = "fallbacks") : ParallelContainerBase(name) {}
Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {}
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
@ -190,17 +183,16 @@ public:
void onNewSolution(const SolutionBase& s) override;
};
class MergerPrivate;
/** Plan for different sub tasks in parallel and finally merge all sub solutions into a single trajectory */
class Merger : public ParallelContainerBase
{
public:
PRIVATE_CLASS(Merger)
Merger(const std::string &name = "merger");
Merger(const std::string& name = "merger");
void reset() override;
void init(const core::RobotModelConstPtr &robot_model) override;
void init(const core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
@ -209,7 +201,6 @@ protected:
void onNewSolution(const SolutionBase& s) override;
};
class WrapperBasePrivate;
/** A wrapper wraps a single child stage, which can be accessed via wrapped().
*
@ -222,22 +213,20 @@ class WrapperBase : public ParallelContainerBase
{
public:
PRIVATE_CLASS(WrapperBase)
WrapperBase(const std::string &name = "wrapper", Stage::pointer &&child = Stage::pointer());
WrapperBase(const std::string& name = "wrapper", Stage::pointer&& child = Stage::pointer());
/// insertion is only allowed if children() is empty
bool insert(Stage::pointer&& stage, int before = -1) override;
/// access the single wrapped child, NULL if still empty
Stage* wrapped();
inline const Stage* wrapped() const {
return const_cast<WrapperBase*>(this)->wrapped();
}
inline const Stage* wrapped() const { return const_cast<WrapperBase*>(this)->wrapped(); }
bool canCompute() const override;
void compute() override;
protected:
WrapperBase(WrapperBasePrivate* impl, Stage::pointer &&child = Stage::pointer());
WrapperBase(WrapperBasePrivate* impl, Stage::pointer&& child = Stage::pointer());
};
} }
}
}

View File

@ -45,12 +45,15 @@
#include <map>
#include <climits>
namespace moveit { namespace core {
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(JointModelGroup)
MOVEIT_CLASS_FORWARD(RobotState)
} }
}
}
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
/* A container needs to decouple its own (external) interfaces
* from those (internal) of its children.
@ -89,12 +92,11 @@ public:
bool remove(ContainerBasePrivate::const_iterator pos);
/// traversing all stages up to max_depth
bool traverseStages(const ContainerBase::StageCallback &processor,
unsigned int cur_depth, unsigned int max_depth) const;
bool traverseStages(const ContainerBase::StageCallback& processor, unsigned int cur_depth,
unsigned int max_depth) const;
/// non-const version
bool traverseStages(const NonConstStageCallback &processor,
unsigned int cur_depth, unsigned int max_depth) {
bool traverseStages(const NonConstStageCallback& processor, unsigned int cur_depth, unsigned int max_depth) {
const auto& const_processor = [&processor](const Stage& stage, unsigned int depth) {
return processor(const_cast<Stage&>(stage), depth);
};
@ -111,7 +113,7 @@ public:
InterfacePtr pendingForward() const { return pending_forward_; }
protected:
ContainerBasePrivate(ContainerBase *me, const std::string &name);
ContainerBasePrivate(ContainerBase* me, const std::string& name);
// Set child's push interfaces: allow pushing if child requires it or
// if the interface is unknown: in this case greedily assume a push interface.
@ -127,14 +129,12 @@ protected:
child.pimpl()->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
}
// report error about mismatching interface (start or end as determined by mask)
void mismatchingInterface(InitStageException& errors, const StagePrivate& child,
const InterfaceFlags mask) const;
void mismatchingInterface(InitStageException& errors, const StagePrivate& child, const InterfaceFlags mask) const;
/// copy external_state to a child's interface and remember the link in internal_to map
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
/// lift solution from internal to external level
void liftSolution(SolutionBasePtr solution,
const InterfaceState *internal_from, const InterfaceState *internal_to);
void liftSolution(SolutionBasePtr solution, const InterfaceState* internal_from, const InterfaceState* internal_to);
auto& internalToExternalMap() { return internal_to_external_; }
const auto& internalToExternalMap() const { return internal_to_external_; }
@ -154,17 +154,17 @@ private:
};
PIMPL_FUNCTIONS(ContainerBase)
/* A solution of a SerialContainer needs to connect start to end via a full path.
* 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.
*/
class SerialContainerPrivate : public ContainerBasePrivate {
class SerialContainerPrivate : public ContainerBasePrivate
{
friend class SerialContainer;
public:
SerialContainerPrivate(SerialContainer* me, const std::string &name);
SerialContainerPrivate(SerialContainer* me, const std::string& name);
// containers derive their required interface from their children
InterfaceFlags requiredInterface() const override;
@ -179,51 +179,45 @@ private:
bool connect(container_type::const_iterator cur);
// called by init() to prune interfaces for children in range [first, last)
void pruneInterfaces(container_type::const_iterator first,
container_type::const_iterator end);
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end);
// prune interface for children in range [first, last) to given direction
void pruneInterfaces(container_type::const_iterator first,
container_type::const_iterator end,
void pruneInterfaces(container_type::const_iterator first, container_type::const_iterator end,
InterfaceFlags accepted);
// store first/last child's interface as required for this container
void storeRequiredInterface(container_type::const_iterator first,
container_type::const_iterator end);
void storeRequiredInterface(container_type::const_iterator first, container_type::const_iterator end);
private:
InterfaceFlags required_interface_;
};
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 {
class WrappedSolution : public SolutionBase
{
public:
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost, std::string comment)
: SolutionBase(creator, cost, std::move(comment)), wrapped_(wrapped)
{}
: SolutionBase(creator, cost, std::move(comment)), wrapped_(wrapped) {}
explicit WrappedSolution(StagePrivate* creator, const SolutionBase* wrapped, double cost)
: SolutionBase(creator, cost), wrapped_(wrapped)
{}
: 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,
: WrappedSolution(creator, wrapped, wrapped->cost()) {}
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const override;
private:
const SolutionBase* wrapped_;
};
class ParallelContainerBasePrivate : public ContainerBasePrivate {
class ParallelContainerBasePrivate : public ContainerBasePrivate
{
friend class ParallelContainerBase;
public:
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string &name);
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
// containers derive their required interface from their children
InterfaceFlags requiredInterface() const override;
@ -239,8 +233,8 @@ private:
};
PIMPL_FUNCTIONS(ParallelContainerBase)
class WrapperBasePrivate : public ParallelContainerBasePrivate {
class WrapperBasePrivate : public ParallelContainerBasePrivate
{
friend class WrapperBase;
public:
@ -248,8 +242,8 @@ public:
};
PIMPL_FUNCTIONS(WrapperBase)
class MergerPrivate : public ParallelContainerBasePrivate {
class MergerPrivate : public ParallelContainerBasePrivate
{
friend class Merger;
moveit::core::JointModelGroupPtr jmg_merged_;
@ -268,12 +262,12 @@ public:
void onNewGeneratorSolution(const SolutionBase& s);
void mergeAnyCombination(const ChildSolutionMap& all_solutions, const SolutionBase& current,
const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner);
void merge(const ChildSolutionList& sub_solutions,
const planning_scene::PlanningSceneConstPtr& start_scene, const Spawner& spawner);
void merge(const ChildSolutionList& sub_solutions, const planning_scene::PlanningSceneConstPtr& start_scene,
const Spawner& spawner);
void sendForward(SubTrajectory&& t, const InterfaceState* from);
void sendBackward(SubTrajectory&& t, const InterfaceState* to);
};
PIMPL_FUNCTIONS(Merger)
} }
}
}

View File

@ -8,14 +8,14 @@
/// ValueOrPointeeLess provides correct comparison for plain and pointer-like types
template <typename T, typename = bool>
struct ValueOrPointeeLess : public std::less<T> {};
struct ValueOrPointeeLess : public std::less<T>
{};
/// The following template-specialization is for pointer-like types
template <typename T>
struct ValueOrPointeeLess<T, decltype(*std::declval<T>() < *std::declval<T>())> {
bool operator()(const T& x, const T& y) const {
return *x < *y;
}
struct ValueOrPointeeLess<T, decltype(*std::declval<T>() < *std::declval<T>())>
{
bool operator()(const T& x, const T& y) const { return *x < *y; }
};
/**
@ -25,8 +25,7 @@ struct ValueOrPointeeLess<T, decltype(*std::declval<T>() < *std::declval<T>())>
* This ensures, that existing iterators remain valid upon insertion and deletion.
* Sorted insertion has logarithmic complexity.
*/
template <typename T,
typename Compare = ValueOrPointeeLess<T>>
template <typename T, typename Compare = ValueOrPointeeLess<T>>
class ordered
{
public:
@ -62,7 +61,11 @@ public:
reference top() { return c.front(); }
const_reference top() const { return c.front(); }
value_type pop() { value_type result(top()); c.pop_front(); return result; }
value_type pop() {
value_type result(top());
c.pop_front();
return result;
}
reference front() { return c.front(); }
const_reference front() const { return c.front(); }
@ -99,7 +102,7 @@ public:
iterator erase(const_iterator pos) { return c.erase(pos); }
/// update sort position of a single item after changes
iterator update(iterator &it) {
iterator update(iterator& it) {
container_type temp;
temp.splice(temp.end(), c, it); // move it from c to temp
iterator at = std::upper_bound(c.begin(), c.end(), *it, comp);
@ -119,20 +122,21 @@ public:
return pos;
}
template<typename Predicate>
void remove_if(Predicate p) { c.remove_if(p); }
template <typename Predicate>
void remove_if(Predicate p) {
c.remove_if(p);
}
};
namespace detail {
template <typename ValueType, typename CostType>
struct ItemCostPair : std::pair<ValueType, CostType> {
struct ItemCostPair : std::pair<ValueType, CostType>
{
typedef CostType cost_type;
ItemCostPair(const std::pair<ValueType, CostType>& other)
: std::pair<ValueType, CostType>(other) {}
ItemCostPair(std::pair<ValueType, CostType>&& other)
: std::pair<ValueType, CostType>(std::move(other)) {}
ItemCostPair(const std::pair<ValueType, CostType>& other) : std::pair<ValueType, CostType>(other) {}
ItemCostPair(std::pair<ValueType, CostType>&& other) : std::pair<ValueType, CostType>(std::move(other)) {}
inline ValueType& value() { return this->first; }
inline const ValueType& value() const { return this->first; }
@ -140,9 +144,7 @@ struct ItemCostPair : std::pair<ValueType, CostType> {
inline CostType cost() const { return this->second; }
// comparison only considers cost
constexpr bool operator<(const ItemCostPair& other) const {
return this->cost() < other.cost();
}
constexpr bool operator<(const ItemCostPair& other) const { return this->cost() < other.cost(); }
};
} // namespace detail
@ -152,10 +154,9 @@ template <typename ValueType, typename CostType = double,
class cost_ordered : public ordered<detail::ItemCostPair<ValueType, CostType>, Compare>
{
typedef ordered<detail::ItemCostPair<ValueType, CostType>, Compare> base_type;
public:
auto insert(const ValueType& value, const CostType cost) {
return base_type::insert(std::make_pair(value, cost));
}
auto insert(const ValueType& value, const CostType cost) { return base_type::insert(std::make_pair(value, cost)); }
auto insert(ValueType&& value, const CostType cost) {
return base_type::insert(std::make_pair(std::move(value), cost));
}

View File

@ -45,11 +45,12 @@
#include <moveit_task_constructor_msgs/GetSolution.h>
#define DESCRIPTION_TOPIC "description"
#define STATISTICS_TOPIC "statistics"
#define SOLUTION_TOPIC "solution"
#define STATISTICS_TOPIC "statistics"
#define SOLUTION_TOPIC "solution"
#define GET_SOLUTION_SERVICE "get_solution"
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(SolutionBase)
@ -61,16 +62,18 @@ class IntrospectionPrivate;
*
* It is interlinked to its task.
*/
class Introspection {
IntrospectionPrivate *impl;
class Introspection
{
IntrospectionPrivate* impl;
public:
Introspection(const TaskPrivate *task);
Introspection(const Introspection &other) = delete;
Introspection(const TaskPrivate* task);
Introspection(const Introspection& other) = delete;
~Introspection();
/// fill task description message for publishing the task configuration
moveit_task_constructor_msgs::TaskDescription& fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg);
moveit_task_constructor_msgs::TaskDescription&
fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg);
/// publish detailed task description
void publishTaskDescription();
@ -83,31 +86,31 @@ public:
void reset();
/// register the given solution, assigning a unique ID
void registerSolution(const SolutionBase &s);
void registerSolution(const SolutionBase& s);
/// publish the given solution
void publishSolution(const SolutionBase &s);
void publishSolution(const SolutionBase& s);
/// publish all top-level solutions of task
void publishAllSolutions(bool wait = true);
/// get solution
bool getSolution(moveit_task_constructor_msgs::GetSolution::Request &req,
moveit_task_constructor_msgs::GetSolution::Response &res);
bool getSolution(moveit_task_constructor_msgs::GetSolution::Request& req,
moveit_task_constructor_msgs::GetSolution::Response& res);
/// retrieve id of given stage
uint32_t stageId(const moveit::task_constructor::Stage * const s) const;
uint32_t stageId(const moveit::task_constructor::Stage* const s) const;
/// retrieve or set id of given solution
uint32_t solutionId(const moveit::task_constructor::SolutionBase &s);
uint32_t solutionId(const moveit::task_constructor::SolutionBase& s);
private:
void fillStageStatistics(const Stage &stage, moveit_task_constructor_msgs::StageStatistics &s);
void fillSolution(moveit_task_constructor_msgs::Solution &msg, const SolutionBase &s);
void fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s);
void fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s);
/// retrieve or set id of given stage
uint32_t stageId(const moveit::task_constructor::Stage * const s);
uint32_t stageId(const moveit::task_constructor::Stage* const s);
/// retrieve solution with given id
const SolutionBase* solutionFromId(uint id) const;
};
} }
}
}

View File

@ -8,43 +8,40 @@
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene)
}
namespace moveit { namespace core {
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotState)
MOVEIT_CLASS_FORWARD(LinkModel)
} }
}
}
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
/** signature of callback function, passing the generated marker and the name of the robot link / scene object */
typedef std::function<void(visualization_msgs::Marker&, const std::string&)> MarkerCallback;
/** generate marker msgs to visualize the planning scene, calling the given callback for each of them
* object_names: set of links to include (or all if empty) */
void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr &scene,
const MarkerCallback& callback,
const std::vector<std::string> &object_names = {});
void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr& scene, const MarkerCallback& callback,
const std::vector<std::string>& object_names = {});
/** generate marker msgs to visualize robot's collision geometry, calling the given callback for each of them
* link_names: set of links to include (or all if empty) */
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<std::string> &link_names = {});
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<const moveit::core::LinkModel*> &link_models);
void generateCollisionMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<std::string>& link_names = {});
void generateCollisionMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<const moveit::core::LinkModel*>& link_models);
/** generate marker msgs to visualize robot's visual geometry, calling the given callback for each of them
* link_names: set of links to include (or all if empty) */
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<std::string> &link_names = {});
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<const moveit::core::LinkModel*> &link_models);
void generateVisualMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<std::string>& link_names = {});
void generateVisualMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<const moveit::core::LinkModel*>& link_models);
/** generate marker msgs to visualize the planning scene, calling the given callback for each of them
* calls generateMarkersForRobot() and generateMarkersForObjects() */
void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr &scene,
const MarkerCallback& callback);
} }
void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr& scene, const MarkerCallback& callback);
}
}

View File

@ -40,8 +40,8 @@
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
/// create a new JointModelGroup comprising all joints of the given groups
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups);
@ -53,8 +53,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
* The list of duplicate joints is returned in \e duplicates and in \e names (as a comma-separated list) */
bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
std::vector<const moveit::core::JointModel*> joints,
std::vector<const moveit::core::JointModel*>& duplicates,
std::string& names);
std::vector<const moveit::core::JointModel*>& duplicates, std::string& names);
/** merge all sub trajectories into a single RobotTrajectory for parallel execution
*
@ -63,7 +62,8 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
* or created on the fly. This JMG needs to stay alive during the lifetime of the trajectory.
* For now, only the trajectory path is considered. Timings, velocities, etc. are ignored.
*/
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
} }
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
}
}

View File

@ -56,7 +56,6 @@ class PropertyMap;
/// initializer function, using given name from the passed property map
boost::any fromName(const PropertyMap& other, const std::string& other_name);
/** Property is a wrapper for a boost::any value, also providing a default value and a description.
*
* Properties can be configured to be initialized from another PropertyMap - if still undefined.
@ -67,13 +66,13 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name);
* Using reset() the default value can be restored.
* setCurrentValue() and setDefaultValue() only set the specific value.
*/
class Property {
class Property
{
friend class PropertyMap;
/// typed constructor is only accessible via PropertyMap
Property(const boost::typeindex::type_info& type_info,
const std::string &description,
const boost::any &default_value);
Property(const boost::typeindex::type_info& type_info, const std::string& description,
const boost::any& default_value);
public:
typedef boost::typeindex::type_info type_info;
@ -127,9 +126,9 @@ public:
/// return true, if property initialized from given SourceId
bool initsFrom(SourceFlags source) const;
/// configure initialization from source using an arbitrary function
Property &configureInitFrom(SourceFlags source, const InitializerFunction& f);
Property& configureInitFrom(SourceFlags source, const InitializerFunction& f);
/// configure initialization from source using given other property name
Property &configureInitFrom(SourceFlags source, const std::string& name);
Property& configureInitFrom(SourceFlags source, const std::string& name);
private:
std::string description_;
@ -143,46 +142,54 @@ private:
InitializerFunction initializer_;
};
class Property::error : public std::runtime_error {
class Property::error : public std::runtime_error
{
protected:
std::string property_name_;
std::string msg_;
public:
explicit error(const std::string& msg);
const std::string& name() const { return property_name_; }
void setName(const std::string& name);
const char* what() const noexcept override { return msg_.c_str(); }
};
class Property::undeclared : public Property::error {
class Property::undeclared : public Property::error
{
public:
explicit undeclared(const std::string& name, const std::string& msg = "undeclared");
};
class Property::undefined : public Property::error {
class Property::undefined : public Property::error
{
public:
explicit undefined(const std::string& name, const std::string& msg = "undefined");
};
class Property::type_error : public Property::error {
class Property::type_error : public Property::error
{
public:
explicit type_error(const std::string& current_type, const std::string& declared_type);
};
// hasSerialize<T>::value provides a true/false constexpr depending on whether operator<< is supported.
// This uses SFINAE, extracted from https://jguegant.github.io/blogs/tech/sfinae-introduction.html
template <typename T, typename = std::ostream&>
struct hasSerialize : std::false_type {};
struct hasSerialize : std::false_type
{};
template <typename T>
struct hasSerialize<T, decltype(std::declval<std::ostream&>() << std::declval<T>())> : std::true_type {};
struct hasSerialize<T, decltype(std::declval<std::ostream&>() << std::declval<T>())> : std::true_type
{};
template <typename T, typename = std::istream&>
struct hasDeserialize : std::false_type {};
struct hasDeserialize : std::false_type
{};
template <typename T>
struct hasDeserialize<T, decltype(std::declval<std::istream&>() >> std::declval<T&>())> : std::true_type {};
struct hasDeserialize<T, decltype(std::declval<std::istream&>() >> std::declval<T&>())> : std::true_type
{};
class PropertySerializerBase {
class PropertySerializerBase
{
public:
typedef std::string (*SerializeFunction)(const boost::any&);
typedef boost::any (*DeserializeFunction)(const std::string&);
@ -191,33 +198,31 @@ public:
static boost::any dummyDeserialize(const std::string&) { return boost::any(); }
protected:
static bool insert(const std::type_index& type_index, const std::string& type_name,
SerializeFunction serialize, DeserializeFunction deserialize);
static bool insert(const std::type_index& type_index, const std::string& type_name, SerializeFunction serialize,
DeserializeFunction deserialize);
};
/// utility class to register serializer/deserializer functions for a property of type T
template <typename T>
class PropertySerializer : protected PropertySerializerBase {
class PropertySerializer : protected PropertySerializerBase
{
public:
PropertySerializer() {
insert(typeid(T), typeName<T>(), &serialize, &deserialize);
}
PropertySerializer() { insert(typeid(T), typeName<T>(), &serialize, &deserialize); }
template <class Q = T>
static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type
typeName() {
static typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
return ros::message_traits::DataType<T>::value();
}
template <class Q = T>
static typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type
typeName() { return typeid(T).name(); }
static typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type typeName() {
return typeid(T).name();
}
private:
/** Serialization based on std::[io]stringstream */
template <class Q = T>
static typename std::enable_if<hasSerialize<Q>::value, std::string>::type
serialize(const boost::any& value) {
static typename std::enable_if<hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
std::ostringstream oss;
oss << boost::any_cast<T>(value);
return oss.str();
@ -233,14 +238,16 @@ private:
/** No serialization available */
template <class Q = T>
static typename std::enable_if<!hasSerialize<Q>::value, std::string>::type
serialize(const boost::any& value) { return dummySerialize(value); }
static typename std::enable_if<!hasSerialize<Q>::value, std::string>::type serialize(const boost::any& value) {
return dummySerialize(value);
}
template <class Q = T>
static typename std::enable_if<!hasSerialize<Q>::value || !hasDeserialize<Q>::value, boost::any>::type
deserialize(const std::string& wire) { return dummyDeserialize(wire); }
deserialize(const std::string& wire) {
return dummyDeserialize(wire);
}
};
/** PropertyMap is map of (name, Property) pairs.
*
* Conveniency methods are provided to setup property initialization for several
@ -251,19 +258,19 @@ class PropertyMap
std::map<std::string, Property> props_;
/// implementation of declare methods
Property& declare(const std::string& name, const Property::type_info& type_info,
const std::string& description, const boost::any& default_value);
Property& declare(const std::string& name, const Property::type_info& type_info, const std::string& description,
const boost::any& default_value);
public:
/// declare a property for future use
template<typename T>
template <typename T>
Property& declare(const std::string& name, const std::string& description = "") {
PropertySerializer<T>(); // register serializer/deserializer
return declare(name, typeid(T), description, boost::any());
}
/// declare a property with default value
template<typename T>
Property& declare(const std::string& name, const T& default_value,
const std::string& description = "") {
template <typename T>
Property& declare(const std::string& name, const T& default_value, const std::string& description = "") {
PropertySerializer<T>(); // register serializer/deserializer
return declare(name, typeid(T), description, default_value);
}
@ -275,13 +282,11 @@ public:
void exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const;
/// check whether given property is declared
bool hasProperty(const std::string &name) const;
bool hasProperty(const std::string& name) const;
/// get the property with given name, throws Property::undeclared for unknown name
Property& property(const std::string &name);
const Property& property(const std::string &name) const {
return const_cast<PropertyMap*>(this)->property(name);
}
Property& property(const std::string& name);
const Property& property(const std::string& name) const { return const_cast<PropertyMap*>(this)->property(name); }
typedef std::map<std::string, Property>::iterator iterator;
typedef std::map<std::string, Property>::const_iterator const_iterator;
@ -292,7 +297,7 @@ public:
const_iterator end() const { return props_.end(); }
/// allow initialization from given source for listed properties - always using the same name
void configureInitFrom(Property::SourceFlags source, const std::set<std::string> &properties = {});
void configureInitFrom(Property::SourceFlags source, const std::set<std::string>& properties = {});
/// set (and, if neccessary, declare) the value of a property
template <typename T>
@ -305,9 +310,7 @@ public:
}
/// overloading: const char* is stored as std::string
inline void set(const std::string& name, const char* value){
set<std::string>(name, value);
}
inline void set(const std::string& name, const char* value) { set<std::string>(name, value); }
/// temporarily set the value of a property
void setCurrent(const std::string& name, const boost::any& value);
@ -316,7 +319,7 @@ public:
const boost::any& get(const std::string& name) const;
/// Get typed value of property. Throws undeclared, undefined, or bad_any_cast.
template<typename T>
template <typename T>
const T& get(const std::string& name) const {
const boost::any& value = get(name);
if (value.empty())
@ -324,7 +327,7 @@ public:
return boost::any_cast<const T&>(value);
}
/// get typed value of property, using fallback if undefined. Throws bad_any_cast on type mismatch.
template<typename T>
template <typename T>
const T& get(const std::string& name, const T& fallback) const {
const boost::any& value = get(name);
return (value.empty()) ? fallback : boost::any_cast<const T&>(value);
@ -344,5 +347,5 @@ public:
template <>
void PropertyMap::set<boost::any>(const std::string& name, const boost::any& value);
} // namespace task_constructor
} // namespace moveit
} // namespace task_constructor
} // namespace moveit

View File

@ -40,12 +40,15 @@
#include <moveit/task_constructor/solvers/planner_interface.h>
namespace moveit { namespace task_constructor { namespace solvers {
namespace moveit {
namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(CartesianPath)
/** Use RobotState's computeCartesianPath() to generate a straigh-line path between to scenes */
class CartesianPath : public PlannerInterface {
class CartesianPath : public PlannerInterface
{
public:
CartesianPath();
@ -58,20 +61,15 @@ public:
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,
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup *jmg,
double timeout,
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} } }
}
}
}

View File

@ -41,31 +41,29 @@
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/macros/class_forward.h>
namespace moveit { namespace task_constructor { namespace solvers {
namespace moveit {
namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(JointInterpolationPlanner)
/** Use MoveIt's PlanningPipeline to plan a trajectory between to scenes */
class JointInterpolationPlanner : public PlannerInterface {
class JointInterpolationPlanner : public PlannerInterface
{
public:
JointInterpolationPlanner();
JointInterpolationPlanner();
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,
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Isometry3d& target,
const core::JointModelGroup *jmg,
double timeout,
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} } }
}
}
}

View File

@ -45,36 +45,34 @@ namespace planning_pipeline {
MOVEIT_CLASS_FORWARD(PlanningPipeline)
}
namespace moveit { namespace task_constructor { namespace solvers {
namespace moveit {
namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(PipelinePlanner)
/** Use MoveIt's PlanningPipeline to plan a trajectory between to scenes */
class PipelinePlanner : public PlannerInterface {
class PipelinePlanner : public PlannerInterface
{
public:
PipelinePlanner();
void setPlannerId(const std::string &planner) { setProperty("planner", planner); }
void setPlannerId(const std::string& planner) { setProperty("planner", planner); }
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,
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Isometry3d& target,
const core::JointModelGroup *jmg,
double timeout,
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
protected:
planning_pipeline::PlanningPipelinePtr planner_;
};
} } }
}
}
}

View File

@ -49,16 +49,21 @@ MOVEIT_CLASS_FORWARD(PlanningScene)
namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory)
}
namespace moveit { namespace core {
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(LinkModel)
MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(JointModelGroup)
} }
}
}
namespace moveit { namespace task_constructor { namespace solvers {
namespace moveit {
namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(PlannerInterface)
class PlannerInterface {
class PlannerInterface
{
// these properties take precedence over stage properties
PropertyMap properties_;
@ -69,28 +74,22 @@ public:
PropertyMap& properties() { return properties_; }
const PropertyMap& properties() const { return properties_; }
void setProperty(const std::string& name, const boost::any& value) {
properties_.set(name, value);
}
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,
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 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::Isometry3d& target,
const moveit::core::JointModelGroup *jmg,
double timeout,
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
};
} } }
}
}
}

View File

@ -45,14 +45,16 @@
#include <vector>
#include <list>
#define PRIVATE_CLASS(Class) \
friend class Class##Private; \
#define PRIVATE_CLASS(Class) \
friend class Class##Private; \
inline const Class##Private* pimpl() const; \
inline Class##Private* pimpl();
namespace moveit { namespace core {
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
} }
}
}
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene)
@ -66,62 +68,63 @@ namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory)
}
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
enum InterfaceFlag {
READS_START = 0x01,
READS_END = 0x02,
WRITES_NEXT_START = 0x04,
WRITES_PREV_END = 0x08,
enum InterfaceFlag
{
READS_START = 0x01,
READS_END = 0x02,
WRITES_NEXT_START = 0x04,
WRITES_PREV_END = 0x08,
CONNECT = READS_START | READS_END,
CONNECT = READS_START | READS_END,
PROPAGATE_FORWARDS = READS_START | WRITES_NEXT_START,
PROPAGATE_BACKWARDS = READS_END | WRITES_PREV_END,
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
};
typedef Flags<InterfaceFlag> InterfaceFlags;
// some useful constants
constexpr InterfaceFlags UNKNOWN;
constexpr InterfaceFlags START_IF_MASK({READS_START, WRITES_PREV_END});
constexpr InterfaceFlags END_IF_MASK({READS_END, WRITES_NEXT_START});
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS});
constexpr InterfaceFlags START_IF_MASK({ READS_START, WRITES_PREV_END });
constexpr InterfaceFlags END_IF_MASK({ READS_END, WRITES_NEXT_START });
constexpr InterfaceFlags PROPAGATE_BOTHWAYS({ PROPAGATE_FORWARDS, PROPAGATE_BACKWARDS });
MOVEIT_CLASS_FORWARD(Interface)
MOVEIT_CLASS_FORWARD(Stage)
class InterfaceState;
typedef std::pair<const InterfaceState&, const InterfaceState&> InterfaceStatePair;
/// exception thrown by Stage::init()
/// It collects individual errors in stages throughout the pipeline to allow overall error reporting
class InitStageException : public std::exception {
class InitStageException : public std::exception
{
friend std::ostream& operator<<(std::ostream& os, const InitStageException& e);
public:
explicit InitStageException() {}
explicit InitStageException(const Stage& stage, const std::string& msg) {
push_back(stage, msg);
}
explicit InitStageException(const Stage& stage, const std::string& msg) { push_back(stage, msg); }
/// push_back a single new error in stage
void push_back(const Stage& stage, const std::string& msg);
/// append all the errors from other (which is emptied)
void append(InitStageException &other);
void append(InitStageException& other);
/// check of existing errors
operator bool() const { return !errors_.empty(); }
virtual const char* what() const noexcept override;
private:
std::list<std::pair<const Stage*, const std::string>> errors_;
};
std::ostream& operator<<(std::ostream &os, const InitStageException& e);
std::ostream& operator<<(std::ostream& os, const InitStageException& e);
class ContainerBase;
class StagePrivate;
class Stage {
class Stage
{
public:
PRIVATE_CLASS(Stage)
typedef std::unique_ptr<Stage> pointer;
@ -132,10 +135,11 @@ public:
*
* INTERFACE takes precedence over PARENT.
*/
enum PropertyInitializerSource { // TODO: move to property.cpp
DEFAULT = 0,
MANUAL = 1,
PARENT = 2,
enum PropertyInitializerSource
{ // TODO: move to property.cpp
DEFAULT = 0,
MANUAL = 1,
PARENT = 2,
INTERFACE = 4,
};
virtual ~Stage();
@ -177,15 +181,15 @@ public:
/// forwarding of properties between interface states
void forwardProperties(const InterfaceState& source, InterfaceState& dest);
std::set<std::string> forwardedProperties() const
{ return properties().get<std::set<std::string>>("forwarded_properties"); }
void setForwardedProperties(const std::set<std::string>& names)
{ setProperty("forwarded_properties", names); }
std::set<std::string> forwardedProperties() const {
return properties().get<std::set<std::string>>("forwarded_properties");
}
void setForwardedProperties(const std::set<std::string>& names) { setProperty("forwarded_properties", names); }
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
typedef std::function<void(const SolutionBase& s)> SolutionCallback;
typedef std::list<SolutionCallback> SolutionCallbackList;
/// add function to be called for every newly found solution or failure
SolutionCallbackList::const_iterator addSolutionCallback(SolutionCallback &&cb);
SolutionCallbackList::const_iterator addSolutionCallback(SolutionCallback&& cb);
/// remove function callback
void removeSolutionCallback(SolutionCallbackList::const_iterator which);
@ -199,21 +203,17 @@ public:
/// get the stage's property map
PropertyMap& properties();
const PropertyMap& properties() const {
return const_cast<Stage*>(this)->properties();
}
const PropertyMap& properties() const { return const_cast<Stage*>(this)->properties(); }
/// set a previously declared property to a new value
void setProperty(const std::string& name, const boost::any& value);
/// overload: const char* values are stored as std::string
inline void setProperty(const std::string& name, const char* value) {
setProperty(name, std::string(value));
}
inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
/// analyze source of error and report accordingly
void reportPropertyError(const Property::error &e);
void reportPropertyError(const Property::error& e);
protected:
/// Stage can only be instantiated through derived classes
Stage(StagePrivate *impl);
Stage(StagePrivate* impl);
/// Stage cannot be copied
Stage(const Stage&) = delete;
@ -222,9 +222,9 @@ protected:
};
std::ostream& operator<<(std::ostream& os, const Stage& stage);
class ComputeBasePrivate;
class ComputeBase : public Stage {
class ComputeBase : public Stage
{
public:
PRIVATE_CLASS(ComputeBase)
@ -233,7 +233,6 @@ protected:
ComputeBase(ComputeBasePrivate* impl);
};
class PropagatingEitherWayPrivate;
/** Base class for stages that can propagate InterfaceStates
*
@ -242,16 +241,18 @@ class PropagatingEitherWayPrivate;
* By default, the class auto-derives its actual propagation direction from the context.
* In order to enforce forward, backward, or bothway propagation, one can use restrictDirection().
*/
class PropagatingEitherWay : public ComputeBase {
class PropagatingEitherWay : public ComputeBase
{
public:
PRIVATE_CLASS(PropagatingEitherWay)
PropagatingEitherWay(const std::string& name = "propagating either way");
enum Direction {
AUTO = 0x00, // auto-derive direction from context
enum Direction
{
AUTO = 0x00, // auto-derive direction from context
FORWARD = 0x01, // propagate forward only
BACKWARD = 0x02, // propagate backward only
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
BACKWARD = 0x02, // propagate backward only
BOTHWAY = FORWARD | BACKWARD, // propagate both ways
};
void restrictDirection(Direction dir);
@ -260,55 +261,51 @@ public:
virtual void computeForward(const InterfaceState& from) = 0;
virtual void computeBackward(const InterfaceState& to) = 0;
void sendForward(const InterfaceState& from,
InterfaceState&& to,
SubTrajectory&& trajectory);
void sendBackward(InterfaceState&& from,
const InterfaceState& to,
SubTrajectory&& trajectory);
void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory);
protected:
// constructor for use in derived classes
PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
};
class PropagatingForwardPrivate;
class PropagatingForward : public PropagatingEitherWay {
class PropagatingForward : public PropagatingEitherWay
{
public:
PRIVATE_CLASS(PropagatingForward)
PropagatingForward(const std::string& name = "propagating forward");
private:
// restrict access to backward method to provide compile-time check
void computeBackward(const InterfaceState &to) override;
void computeBackward(const InterfaceState& to) override;
using PropagatingEitherWay::sendBackward;
};
class PropagatingBackwardPrivate;
class PropagatingBackward : public PropagatingEitherWay {
class PropagatingBackward : public PropagatingEitherWay
{
public:
PRIVATE_CLASS(PropagatingBackward)
PropagatingBackward(const std::string& name = "propagating backward");
private:
// restrict access to forward method to provide compile-time check
void computeForward(const InterfaceState &from) override;
void computeForward(const InterfaceState& from) override;
using PropagatingEitherWay::sendForward;
};
class GeneratorPrivate;
class Generator : public ComputeBase {
class Generator : public ComputeBase
{
public:
PRIVATE_CLASS(Generator)
Generator(const std::string& name = "generator");
virtual bool canCompute() const = 0;
virtual void compute() = 0;
void spawn(InterfaceState &&state, SubTrajectory &&trajectory);
void spawn(InterfaceState &&state, double cost) {
void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
void spawn(InterfaceState&& state, double cost) {
SubTrajectory trajectory;
trajectory.setCost(cost);
spawn(std::move(state), std::move(trajectory));
@ -318,7 +315,6 @@ protected:
Generator(GeneratorPrivate* impl);
};
class MonitoringGeneratorPrivate;
/** Generator that monitors solutions of another stage to make reuse of them
*
@ -331,7 +327,7 @@ class MonitoringGenerator : public Generator
{
public:
PRIVATE_CLASS(MonitoringGenerator)
MonitoringGenerator(const std::string &name = "monitoring generator", Stage* monitored = nullptr);
MonitoringGenerator(const std::string& name = "monitoring generator", Stage* monitored = nullptr);
void setMonitoredStage(Stage* monitored);
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
@ -340,12 +336,12 @@ protected:
MonitoringGenerator(MonitoringGeneratorPrivate* impl);
/// called by monitored stage when a new solution was generated
virtual void onNewSolution(const SolutionBase &s) = 0;
virtual void onNewSolution(const SolutionBase& s) = 0;
};
class ConnectingPrivate;
class Connecting : public ComputeBase {
class Connecting : public ComputeBase
{
protected:
virtual bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const;
@ -372,5 +368,5 @@ protected:
};
const char* flowSymbol(moveit::task_constructor::InterfaceFlags f);
} }
}
}

View File

@ -44,14 +44,16 @@
#include <ostream>
// define pimpl() functions accessing correctly casted pimpl_ pointer
#define PIMPL_FUNCTIONS(Class) \
#define PIMPL_FUNCTIONS(Class) \
const Class##Private* Class::pimpl() const { return static_cast<const Class##Private*>(pimpl_); } \
Class##Private* Class::pimpl() { return static_cast<Class##Private*>(pimpl_); } \
Class##Private* Class::pimpl() { return static_cast<Class##Private*>(pimpl_); }
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
class ContainerBase;
class StagePrivate {
class StagePrivate
{
friend class Stage;
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
@ -99,9 +101,15 @@ public:
/// 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(); }
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
@ -125,7 +133,7 @@ public:
bool storeFailures() const { return introspection_ != nullptr; }
protected:
Stage* me_; // associated/owning Stage instance
Stage* me_; // associated/owning Stage instance
std::string name_;
PropertyMap properties_;
@ -142,10 +150,10 @@ protected:
private:
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
ContainerBase* parent_; // owning parent
container_type::iterator it_; // iterator into parent's children_ list referring to this
ContainerBase* parent_; // owning parent
container_type::iterator it_; // iterator into parent's children_ list referring to this
InterfaceWeakPtr prev_ends_; // interface to be used for sendBackward()
InterfaceWeakPtr prev_ends_; // interface to be used for sendBackward()
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
Introspection* introspection_; // task's introspection instance
@ -153,30 +161,29 @@ private:
PIMPL_FUNCTIONS(Stage)
std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
// ComputeBasePrivate is the base class for all computing stages, i.e. non-containers.
// It adds the trajectories_ variable.
class ComputeBasePrivate : public StagePrivate {
class ComputeBasePrivate : public StagePrivate
{
friend class ComputeBase;
public:
ComputeBasePrivate(Stage* me, const std::string& name)
: StagePrivate(me, name)
{}
ComputeBasePrivate(Stage* me, const std::string& name) : StagePrivate(me, name) {}
private:
};
PIMPL_FUNCTIONS(ComputeBase)
class PropagatingEitherWayPrivate : public ComputeBasePrivate {
class PropagatingEitherWayPrivate : public ComputeBasePrivate
{
friend class PropagatingEitherWay;
public:
PropagatingEitherWay::Direction required_interface_dirs_;
inline PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction required_interface_dirs_,
const std::string &name);
inline PropagatingEitherWayPrivate(PropagatingEitherWay* me,
PropagatingEitherWay::Direction required_interface_dirs_,
const std::string& name);
InterfaceFlags requiredInterface() const override;
// initialize pull interfaces for given propagation directions
@ -190,10 +197,10 @@ public:
void compute() override;
bool hasStartState() const;
const InterfaceState &fetchStartState();
const InterfaceState& fetchStartState();
bool hasEndState() const;
const InterfaceState &fetchEndState();
const InterfaceState& fetchEndState();
protected:
// drop states corresponding to failed (infinite-cost) trajectories
@ -202,24 +209,24 @@ protected:
};
PIMPL_FUNCTIONS(PropagatingEitherWay)
class PropagatingForwardPrivate : public PropagatingEitherWayPrivate {
class PropagatingForwardPrivate : public PropagatingEitherWayPrivate
{
public:
inline PropagatingForwardPrivate(PropagatingForward *me, const std::string &name);
inline PropagatingForwardPrivate(PropagatingForward* me, const std::string& name);
};
PIMPL_FUNCTIONS(PropagatingForward)
class PropagatingBackwardPrivate : public PropagatingEitherWayPrivate {
class PropagatingBackwardPrivate : public PropagatingEitherWayPrivate
{
public:
inline PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name);
inline PropagatingBackwardPrivate(PropagatingBackward* me, const std::string& name);
};
PIMPL_FUNCTIONS(PropagatingBackward)
class GeneratorPrivate : public ComputeBasePrivate {
class GeneratorPrivate : public ComputeBasePrivate
{
public:
inline GeneratorPrivate(Generator *me, const std::string &name);
inline GeneratorPrivate(Generator* me, const std::string& name);
InterfaceFlags requiredInterface() const override;
bool canCompute() const override;
@ -227,37 +234,39 @@ public:
};
PIMPL_FUNCTIONS(Generator)
class MonitoringGeneratorPrivate : public GeneratorPrivate {
class MonitoringGeneratorPrivate : public GeneratorPrivate
{
friend class MonitoringGenerator;
public:
Stage* monitored_;
Stage::SolutionCallbackList::const_iterator cb_;
bool registered_;
inline MonitoringGeneratorPrivate(MonitoringGenerator *me, const std::string &name);
inline MonitoringGeneratorPrivate(MonitoringGenerator* me, const std::string& name);
private:
void solutionCB(const SolutionBase &s);
void solutionCB(const SolutionBase& s);
};
PIMPL_FUNCTIONS(MonitoringGenerator)
class ConnectingPrivate : public ComputeBasePrivate {
class ConnectingPrivate : public ComputeBasePrivate
{
friend class Connecting;
typedef std::pair<Interface::const_iterator, Interface::const_iterator> StatePair;
struct StatePairLess {
struct StatePairLess
{
bool operator()(const StatePair& x, const StatePair& y) const {
return x.first->priority() + x.second->priority() < y.first->priority() + y.second->priority();
}
};
template<Interface::Direction other>
template <Interface::Direction other>
inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
public:
inline ConnectingPrivate(Connecting *me, const std::string &name);
inline ConnectingPrivate(Connecting* me, const std::string& name);
InterfaceFlags requiredInterface() const override;
bool canCompute() const override;
@ -265,12 +274,12 @@ public:
private:
// get informed when new start or end state becomes available
template<Interface::Direction other>
template <Interface::Direction other>
void newState(Interface::iterator it, bool updated);
// ordered list of pending state pairs
ordered<StatePair, StatePairLess> pending;
};
PIMPL_FUNCTIONS(Connecting)
} }
}
}

View File

@ -48,7 +48,9 @@ MOVEIT_CLASS_FORWARD(JointModelGroup)
}
}
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
/** Wrapper for any pose generator stage to compute IK poses for a Cartesian pose.
*
@ -63,59 +65,49 @@ namespace moveit { namespace task_constructor { namespace stages {
* Properties of the internally received InterfaceState can be forwarded to the
* newly generated, externally exposed InterfaceState.
*/
class ComputeIK : public WrapperBase {
class ComputeIK : public WrapperBase
{
public:
ComputeIK(const std::string &name="IK", Stage::pointer &&child = Stage::pointer());
ComputeIK(const std::string& name = "IK", Stage::pointer&& child = Stage::pointer());
void reset() override;
void init(const core::RobotModelConstPtr &robot_model) override;
void onNewSolution(const SolutionBase &s) override;
void init(const core::RobotModelConstPtr& robot_model) override;
void onNewSolution(const SolutionBase& s) override;
bool canCompute() const override;
void compute() override;
void setEndEffector(const std::string& eef) {
setProperty("eef", eef);
}
void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
/// setters for IK frame
void setIKFrame(const geometry_msgs::PoseStamped &pose) {
setProperty("ik_frame", pose);
}
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Isometry3d pose; pose = p;
Eigen::Isometry3d pose;
pose = p;
setIKFrame(pose, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Isometry3d::Identity(), link);
}
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
/// setters for target pose
void setTargetPose(const geometry_msgs::PoseStamped &pose) {
setProperty("target_pose", pose);
}
void setTargetPose(const geometry_msgs::PoseStamped& pose) { setProperty("target_pose", pose); }
void setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame = "");
template <typename T>
void setTargetPose(const T& p, const std::string& frame = "") {
Eigen::Isometry3d pose; pose = p;
Eigen::Isometry3d pose;
pose = p;
setTargetPose(pose, frame);
}
void setMaxIKSolutions(uint32_t n) {
setProperty("max_ik_solutions", n);
}
void setIgnoreCollisions(bool flag) {
setProperty("ignore_collisions", flag);
}
void setMinSolutionDistance(double distance) {
setProperty("min_solution_distance", distance);
}
void setMaxIKSolutions(uint32_t n) { setProperty("max_ik_solutions", n); }
void setIgnoreCollisions(bool flag) { setProperty("ignore_collisions", flag); }
void setMinSolutionDistance(double distance) { setProperty("min_solution_distance", distance); }
protected:
ordered<const SolutionBase*> upstream_solutions_;
};
} } }
}
}
}

View File

@ -43,11 +43,15 @@
#include <moveit_msgs/Constraints.h>
namespace moveit { namespace core {
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotState)
} }
}
}
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
/** Connect arbitrary InterfaceStates by motion planning
*
@ -57,12 +61,17 @@ namespace moveit { namespace task_constructor { namespace stages {
* Finally, an attempt is made to merge the sub trajectories of individual planning results.
* If this fails, the sequential planning result is returned.
*/
class Connect : public Connecting {
class Connect : public Connecting
{
protected:
bool compatible(const InterfaceState &from_state, const InterfaceState &to_state) const override;
bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const override;
public:
enum MergeMode { SEQUENTIAL = 0, WAYPOINTS = 1 };
enum MergeMode
{
SEQUENTIAL = 0,
WAYPOINTS = 1
};
typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector;
Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {});
@ -73,7 +82,7 @@ public:
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
void compute(const InterfaceState &from, const InterfaceState &to) override;
void compute(const InterfaceState& from, const InterfaceState& to) override;
protected:
SolutionSequencePtr makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
@ -89,5 +98,6 @@ protected:
std::list<SubTrajectory> subsolutions_;
std::list<InterfaceState> states_;
};
} } }
}
}
}

View File

@ -41,11 +41,13 @@
#include <moveit/task_constructor/stage.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
/** Fetch the current PlanningScene state via get_planning_scene service */
class CurrentState : public Generator {
class CurrentState : public Generator
{
public:
CurrentState(const std::string& name = "current state");
@ -57,5 +59,6 @@ protected:
moveit::core::RobotModelConstPtr robot_model_;
planning_scene::PlanningScenePtr scene_;
};
} } }
}
}
}

View File

@ -43,9 +43,12 @@
#include <geometry_msgs/Vector3.h>
#include <moveit/collision_detection/collision_common.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
class FixCollisionObjects : public PropagatingEitherWay {
class FixCollisionObjects : public PropagatingEitherWay
{
public:
FixCollisionObjects(const std::string& name = "fix collisions of objects");
@ -56,7 +59,8 @@ public:
void setMaxPenetration(double penetration) { setProperty("max_penetration", penetration); }
private:
SubTrajectory fixCollisions(planning_scene::PlanningScene &scene) const;
SubTrajectory fixCollisions(planning_scene::PlanningScene& scene) const;
};
} } }
}
}
}

View File

@ -42,9 +42,12 @@
#include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
class FixedCartesianPoses : public MonitoringGenerator {
class FixedCartesianPoses : public MonitoringGenerator
{
public:
FixedCartesianPoses(const std::string& name = "generate poses");
@ -58,5 +61,6 @@ protected:
void onNewSolution(const SolutionBase& s) override;
ordered<const SolutionBase*> upstream_solutions_;
};
} } }
}
}
}

View File

@ -40,11 +40,13 @@
#include <moveit/task_constructor/stage.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
/** Spawn a pre-defined PlanningScene state */
class FixedState : public Generator {
class FixedState : public Generator
{
public:
FixedState(const std::string& name = "initial state");
void setState(const planning_scene::PlanningScenePtr& scene);
@ -57,5 +59,6 @@ protected:
planning_scene::PlanningScenePtr scene_;
bool ran_ = false;
};
} } }
}
}
}

View File

@ -40,17 +40,20 @@
#include <moveit/task_constructor/stages/generate_pose.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
class GenerateGraspPose : public GeneratePose {
class GenerateGraspPose : public GeneratePose
{
public:
GenerateGraspPose(const std::string& name = "generate grasp pose");
void init(const core::RobotModelConstPtr &robot_model) override;
void init(const core::RobotModelConstPtr& robot_model) override;
void compute() override;
void setEndEffector(const std::string &eef) { setProperty("eef", eef); }
void setObject(const std::string &object) { setProperty("object", object); }
void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
void setObject(const std::string& object) { setProperty("object", object); }
void setAngleDelta(double delta) { setProperty("angle_delta", delta); }
void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
@ -61,5 +64,6 @@ public:
protected:
void onNewSolution(const SolutionBase& s) override;
};
} } }
}
}
}

View File

@ -40,22 +40,26 @@
#include <moveit/task_constructor/stages/generate_pose.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
/** Simple IK pose generator to place an attached object in a specific pose
*
* The "pose" property, inherited from GeneratePose specifies the target pose
* of the grasped object. This stage transforms this pose into a target pose for the ik_frame */
class GeneratePlacePose : public GeneratePose {
class GeneratePlacePose : public GeneratePose
{
public:
GeneratePlacePose(const std::string& name = "place pose");
void compute() override;
void setObject(const std::string &object) { setProperty("object", object); }
void setObject(const std::string& object) { setProperty("object", object); }
protected:
void onNewSolution(const SolutionBase& s) override;
};
} } }
}
}
}

View File

@ -42,9 +42,12 @@
#include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
class GeneratePose : public MonitoringGenerator {
class GeneratePose : public MonitoringGenerator
{
public:
GeneratePose(const std::string& name = "generate pose");
@ -52,13 +55,12 @@ public:
bool canCompute() const override;
void compute() override;
void setPose(const geometry_msgs::PoseStamped pose){
setProperty("pose", std::move(pose));
}
void setPose(const geometry_msgs::PoseStamped pose) { setProperty("pose", std::move(pose)); }
protected:
void onNewSolution(const SolutionBase& s) override;
ordered<const SolutionBase*> upstream_solutions_;
};
} } }
}
}
}

View File

@ -43,11 +43,15 @@
#include <moveit/task_constructor/type_traits.h>
#include <map>
namespace moveit { namespace core {
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(JointModelGroup)
} }
}
}
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
/** Allow modification of planning scene
*
@ -57,10 +61,12 @@ namespace moveit { namespace task_constructor { namespace stages {
* - Attach or detach objects to robot links.
* - Spawn or remove objects.
*/
class ModifyPlanningScene : public PropagatingEitherWay {
class ModifyPlanningScene : public PropagatingEitherWay
{
public:
typedef std::vector<std::string> Names;
typedef std::function<void(const planning_scene::PlanningScenePtr& scene, const PropertyMap& properties)> ApplyCallback;
typedef std::function<void(const planning_scene::PlanningScenePtr& scene, const PropertyMap& properties)>
ApplyCallback;
ModifyPlanningScene(const std::string& name = "modify planning scene");
void computeForward(const InterfaceState& from) override;
@ -77,11 +83,13 @@ public:
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>>
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>>
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);
}
@ -90,39 +98,42 @@ public:
void allowCollisions(const Names& first, const Names& second, bool allow);
/// allow / forbid collisions for pair (first, second)
void allowCollisions(const std::string& first, const std::string& second, bool allow) {
allowCollisions(Names{first}, Names{second}, allow);
allowCollisions(Names{ first }, Names{ second }, allow);
}
/// allow / forbid all collisions for given object
void allowCollisions(const std::string& object, bool allow) {
allowCollisions(Names({object}), Names(), allow);
}
void allowCollisions(const std::string& object, bool allow) { allowCollisions(Names({ object }), Names(), allow); }
/// conveniency method accepting arbitrary container types
template <typename T1, typename T2,
typename E1 = typename std::enable_if_t<is_container<T1>::value && std::is_convertible<typename T1::value_type, std::string>::value>,
typename E2 = typename std::enable_if_t<is_container<T2>::value && std::is_convertible<typename T1::value_type, std::string>::value>>
typename E1 = typename std::enable_if_t<is_container<T1>::value &&
std::is_convertible<typename T1::value_type, std::string>::value>,
typename E2 = typename std::enable_if_t<is_container<T2>::value &&
std::is_convertible<typename T1::value_type, std::string>::value>>
inline void allowCollisions(const T1& first, const T2& second, bool enable_collision) {
allowCollisions(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_convertible<typename T::value_type, std::string>::value>>
template <typename T, typename E = typename std::enable_if_t<
is_container<T>::value && std::is_convertible<typename T::value_type, std::string>::value>>
inline void allowCollisions(const std::string& first, const T& second, bool enable_collision) {
allowCollisions(Names({first}), Names(second.cbegin(), second.cend()), enable_collision);
allowCollisions(Names({ first }), Names(second.cbegin(), second.cend()), enable_collision);
}
/// conveniency method accepting const char* and an arbitrary container of names
template <typename T, typename E = typename std::enable_if_t<is_container<T>::value && std::is_convertible<typename T::value_type, std::string>::value>>
template <typename T, typename E = typename std::enable_if_t<
is_container<T>::value && std::is_convertible<typename T::value_type, std::string>::value>>
inline void allowCollisions(const char* first, const T& second, bool enable_collision) {
allowCollisions(Names({first}), Names(second.cbegin(), second.cend()), enable_collision);
allowCollisions(Names({ first }), Names(second.cbegin(), second.cend()), enable_collision);
}
/// conveniency method accepting std::string and JointModelGroup
void allowCollisions(const std::string& first, const moveit::core::JointModelGroup& jmg, bool allow);
protected:
// list of objects to attach (true) / detach (false) to a given link
std::map<std::string, std::pair<Names, bool> > attach_objects_;
std::map<std::string, std::pair<Names, bool>> attach_objects_;
// list of objects to mutually
struct CollisionMatrixPairs {
struct CollisionMatrixPairs
{
Names first;
Names second;
bool allow;
@ -132,18 +143,19 @@ protected:
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 allowCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert);
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 allowCollisions(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::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);
inline void ModifyPlanningScene::detachObject(const std::string& object, const std::string& link) {
attachObjects(Names({ object }), link, false);
}
}
}
}
} } }

View File

@ -44,13 +44,18 @@
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
namespace moveit { namespace core {
namespace moveit {
namespace core {
class RobotState;
} }
namespace moveit { namespace task_constructor { namespace stages {
}
}
namespace moveit {
namespace task_constructor {
namespace stages {
/** Perform a Cartesian motion relative to some link */
class MoveRelative : public PropagatingEitherWay {
class MoveRelative : public PropagatingEitherWay
{
public:
MoveRelative(const std::string& name = "move relative",
const solvers::PlannerInterfacePtr& planner = solvers::PlannerInterfacePtr());
@ -59,30 +64,21 @@ public:
void computeForward(const InterfaceState& from) override;
void computeBackward(const InterfaceState& to) override;
void setGroup(const std::string& group) {
setProperty("group", group);
}
void setGroup(const std::string& group) { setProperty("group", group); }
/// setters for IK frame
void setIKFrame(const geometry_msgs::PoseStamped &pose) {
setProperty("ik_frame", pose);
}
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Isometry3d pose; pose = p;
Eigen::Isometry3d pose;
pose = p;
setIKFrame(pose, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Isometry3d::Identity(), link);
}
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
/// set minimum / maximum distance to move
void setMinDistance(double distance) {
setProperty("min_distance", distance);
}
void setMaxDistance(double distance) {
setProperty("max_distance", distance);
}
void setMinDistance(double distance) { setProperty("min_distance", distance); }
void setMaxDistance(double distance) { setProperty("max_distance", distance); }
void setMinMaxDistance(double min_distance, double max_distance) {
setProperty("min_distance", min_distance);
setProperty("max_distance", max_distance);
@ -93,25 +89,20 @@ public:
}
/// perform twist motion on specified link
void setDirection(const geometry_msgs::TwistStamped& twist) {
setProperty("direction", twist);
}
void setDirection(const geometry_msgs::TwistStamped& twist) { setProperty("direction", twist); }
/// translate link along given direction
void setDirection(const geometry_msgs::Vector3Stamped& direction) {
setProperty("direction", direction);
}
void setDirection(const geometry_msgs::Vector3Stamped& direction) { setProperty("direction", direction); }
/// move specified joint variables by given amount
void setDirection(const std::map<std::string, double>& joint_deltas) {
setProperty("direction", joint_deltas);
}
void setDirection(const std::map<std::string, double>& joint_deltas) { setProperty("direction", joint_deltas); }
protected:
// return false if trajectory shouldn't be stored
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
SubTrajectory &trajectory, Direction dir);
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& trajectory,
Direction dir);
protected:
solvers::PlannerInterfacePtr planner_;
};
} } }
}
}
}

View File

@ -44,12 +44,17 @@
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
namespace moveit { namespace core {
namespace moveit {
namespace core {
class RobotState;
} }
namespace moveit { namespace task_constructor { namespace stages {
}
}
namespace moveit {
namespace task_constructor {
namespace stages {
class MoveTo : public PropagatingEitherWay {
class MoveTo : public PropagatingEitherWay
{
public:
MoveTo(const std::string& name = "move to",
const solvers::PlannerInterfacePtr& planner = solvers::PlannerInterfacePtr());
@ -58,64 +63,52 @@ public:
void computeForward(const InterfaceState& from) override;
void computeBackward(const InterfaceState& to) override;
void setGroup(const std::string& group) {
setProperty("group", group);
}
void setGroup(const std::string& group) { setProperty("group", group); }
/// setters for IK frame
void setIKFrame(const geometry_msgs::PoseStamped &pose) {
setProperty("ik_frame", pose);
}
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Isometry3d pose; pose = p;
Eigen::Isometry3d pose;
pose = p;
setIKFrame(pose, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Isometry3d::Identity(), link);
}
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
/// move link to given pose
void setGoal(const geometry_msgs::PoseStamped& pose) {
setProperty("goal", pose);
}
void setGoal(const geometry_msgs::PoseStamped& pose) { setProperty("goal", pose); }
/// move link to given point, keeping current orientation
void setGoal(const geometry_msgs::PointStamped& point) {
setProperty("goal", point);
}
void setGoal(const geometry_msgs::PointStamped& point) { setProperty("goal", point); }
/// move joint model group to given named pose
void setGoal(const std::string& named_joint_pose) {
setProperty("goal", named_joint_pose);
}
void setGoal(const std::string& named_joint_pose) { setProperty("goal", named_joint_pose); }
/// move joints specified in msg to their target values
void setGoal(const moveit_msgs::RobotState& robot_state) {
setProperty("goal", robot_state);
}
void setGoal(const moveit_msgs::RobotState& robot_state) { setProperty("goal", robot_state); }
/// move joints by name to their mapped target value
void setGoal(const std::map<std::string, double>& joints);
void setPathConstraints(moveit_msgs::Constraints path_constraints){
void setPathConstraints(moveit_msgs::Constraints path_constraints) {
setProperty("path_constraints", std::move(path_constraints));
}
protected:
// return false if trajectory shouldn't be stored
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
SubTrajectory &trajectory, Direction dir);
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& trajectory,
Direction dir);
bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state);
bool getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg,
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
decltype(std::declval<SolutionBase>().markers())& markers);
decltype(std::declval<SolutionBase>().markers()) & markers);
bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
decltype(std::declval<SolutionBase>().markers())&);
decltype(std::declval<SolutionBase>().markers()) &);
protected:
solvers::PlannerInterfacePtr planner_;
};
} } }
}
}
}

View File

@ -40,7 +40,8 @@
#include <moveit/task_constructor/container.h>
#include <geometry_msgs/TwistStamped.h>
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(CartesianPath)
@ -64,77 +65,62 @@ namespace stages {
* The end effector postures corresponding to pre-grasp and grasp as well as
* the end effector's Cartesian pose needs to be provided by an external grasp stage.
*/
class PickPlaceBase : public SerialContainer {
class PickPlaceBase : public SerialContainer
{
solvers::CartesianPathPtr cartesian_solver_;
Stage* grasp_stage_ = nullptr;
Stage* approach_stage_ = nullptr;
Stage* lift_stage_ = nullptr;
public:
PickPlaceBase(Stage::pointer &&grasp_stage, const std::string& name, bool forward);
PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& name, bool forward);
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
void setEndEffector(const std::string& eef) {
properties().set<std::string>("eef", eef);
}
void setObject(const std::string& object) {
properties().set<std::string>("object", object);
}
void setEndEffector(const std::string& eef) { properties().set<std::string>("eef", eef); }
void setObject(const std::string& object) { properties().set<std::string>("object", object); }
solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; }
void setApproachRetract(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance);
void setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance);
void setLiftPlace(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance);
void setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance);
void setLiftPlace(const std::map<std::string, double>& joints);
};
/// specialization of PickPlaceBase to realize picking
class Pick : public PickPlaceBase {
class Pick : public PickPlaceBase
{
public:
Pick(Stage::pointer&& grasp_stage = Stage::pointer(), const std::string& name = "pick")
: PickPlaceBase(std::move(grasp_stage), name, true)
{}
: PickPlaceBase(std::move(grasp_stage), name, true) {}
void setApproachMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance) {
void setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) {
setApproachRetract(motion, min_distance, max_distance);
}
void setLiftMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance) {
void setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) {
setLiftPlace(motion, min_distance, max_distance);
}
void setLiftMotion(const std::map<std::string, double>& joints) {
setLiftPlace(joints);
}
void setLiftMotion(const std::map<std::string, double>& joints) { setLiftPlace(joints); }
};
/// specialization of PickPlaceBase to realize placing
class Place : public PickPlaceBase {
class Place : public PickPlaceBase
{
public:
Place(Stage::pointer&& ungrasp_stage = Stage::pointer(), const std::string& name = "place")
: PickPlaceBase(std::move(ungrasp_stage), name, false)
{}
: PickPlaceBase(std::move(ungrasp_stage), name, false) {}
void setRetractMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance) {
void setRetractMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) {
setApproachRetract(motion, min_distance, max_distance);
}
void setPlaceMotion(const geometry_msgs::TwistStamped& motion,
double min_distance, double max_distance) {
void setPlaceMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) {
setLiftPlace(motion, min_distance, max_distance);
}
void setPlaceMotion(const std::map<std::string, double>& joints) {
setLiftPlace(joints);
}
void setPlaceMotion(const std::map<std::string, double>& joints) { setLiftPlace(joints); }
};
} } }
}
}
}

View File

@ -43,7 +43,9 @@ MOVEIT_CLASS_FORWARD(RobotState)
}
}
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
/** Stage Wrapper to filter generated solutions by custom criteria
*
@ -51,19 +53,19 @@ namespace moveit { namespace task_constructor { namespace stages {
* Solutions are accepted if predicate(s) == true.
* Rejected solutions are forwarded as failures with an optional comment
*/
class PredicateFilter : public WrapperBase {
class PredicateFilter : public WrapperBase
{
public:
typedef std::function<bool(const SolutionBase&, std::string&)> Predicate;
PredicateFilter(const std::string &name, Stage::pointer &&child = Stage::pointer());
PredicateFilter(const std::string& name, Stage::pointer&& child = Stage::pointer());
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
void onNewSolution(const SolutionBase &s) override;
void onNewSolution(const SolutionBase& s) override;
void setPredicate(const Predicate& p){
setProperty("predicate", p);
}
void setPredicate(const Predicate& p) { setProperty("predicate", p); }
};
} } }
}
}
}

View File

@ -41,8 +41,14 @@
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry>
namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } }
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
}
}
namespace moveit {
namespace task_constructor {
namespace stages {
class GenerateGraspPose;
@ -56,7 +62,8 @@ class GenerateGraspPose;
* Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class
* provides the common functionality for grasping (forward = true) and ungrasping (forward = false).
*/
class SimpleGraspBase : public SerialContainer {
class SimpleGraspBase : public SerialContainer
{
moveit::core::RobotModelConstPtr model_;
protected:
@ -71,32 +78,32 @@ public:
void setObject(const std::string& object) { setProperty("object", object); }
/// set properties of IK solver
void setIKFrame(const geometry_msgs::PoseStamped &transform) { setProperty("ik_frame", transform); }
void setIKFrame(const geometry_msgs::PoseStamped& transform) { setProperty("ik_frame", transform); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& t, const std::string& link) {
Eigen::Isometry3d transform; transform = t;
Eigen::Isometry3d transform;
transform = t;
setIKFrame(transform, link);
}
void setIKFrame(const std::string& link) {
setIKFrame(Eigen::Isometry3d::Identity(), link);
}
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); }
};
/// specialization of SimpleGraspBase to realize grasping
class SimpleGrasp : public SimpleGraspBase {
class SimpleGrasp : public SimpleGraspBase
{
public:
SimpleGrasp(Stage::pointer&& generator = Stage::pointer(), const std::string& name = "grasp");
};
/// specialization of SimpleGraspBase to realize ungrasping
class SimpleUnGrasp : public SimpleGraspBase {
class SimpleUnGrasp : public SimpleGraspBase
{
public:
SimpleUnGrasp(Stage::pointer&& generator = Stage::pointer(), const std::string& name = "ungrasp");
};
} } }
}
}
}

View File

@ -59,7 +59,8 @@ namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory)
}
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
class SolutionBase;
MOVEIT_CLASS_FORWARD(InterfaceState)
@ -68,30 +69,29 @@ typedef std::weak_ptr<Interface> InterfaceWeakPtr;
MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(Introspection)
/** InterfaceState describes a potential start or goal state for a planning stage.
*
* A start or goal state for planning is essentially defined by the state of a planning scene.
*/
class InterfaceState {
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
friend class Interface; // allow Interface to set owner_ and priority_
class InterfaceState
{
friend class SolutionBase; // addIncoming() / addOutgoing() should be called only by SolutionBase
friend class Interface; // allow Interface to set owner_ and priority_
public:
/** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
* Preference ordering considers high-depth first and within same depth, minimal cost paths.
*/
struct Priority : public std::pair<unsigned int, double> {
struct Priority : public std::pair<unsigned int, double>
{
Priority() : Priority(0, 0.0) {}
Priority(unsigned int depth, double cost)
: std::pair<unsigned int, double>(depth, cost) {}
Priority(unsigned int depth, double cost) : std::pair<unsigned int, double>(depth, cost) {}
inline unsigned int depth() const { return this->first; }
inline double cost() const { return this->second; }
Priority operator+(const Priority& other) const {
return Priority(this->depth() + other.depth(),
this->cost() + other.cost());
return Priority(this->depth() + other.depth(), this->cost() + other.cost());
}
bool operator<(const Priority& other) const;
};
@ -112,9 +112,7 @@ public:
const PropertyMap& properties() const { return properties_; }
/// states are ordered by priority
inline bool operator<(const InterfaceState& other) const {
return this->priority_ < other.priority_;
}
inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; }
inline const Priority& priority() const { return priority_; }
Interface* owner() const { return owner_; }
@ -135,45 +133,50 @@ private:
};
/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
class Interface : public ordered<InterfaceState*> {
class Interface : public ordered<InterfaceState*>
{
typedef ordered<InterfaceState*> base_type;
public:
// iterators providing convinient access to stored InterfaceState
class iterator : public base_type::iterator {
class iterator : public base_type::iterator
{
public:
iterator(base_type::iterator other) : base_type::iterator(other) {}
InterfaceState& operator*() const noexcept
{ return *base_type::iterator::operator*(); }
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
InterfaceState* operator->() const noexcept
{ return base_type::iterator::operator*(); }
InterfaceState* operator->() const noexcept { return base_type::iterator::operator*(); }
};
class const_iterator : public base_type::const_iterator {
class const_iterator : public base_type::const_iterator
{
public:
const_iterator(base_type::const_iterator other) : base_type::const_iterator(other) {}
const_iterator(base_type::iterator other) : base_type::const_iterator(other) {}
const InterfaceState& operator*() const noexcept
{ return *base_type::const_iterator::operator*(); }
const InterfaceState& operator*() const noexcept { return *base_type::const_iterator::operator*(); }
const InterfaceState* operator->() const noexcept
{ return base_type::const_iterator::operator*(); }
const InterfaceState* operator->() const noexcept { return base_type::const_iterator::operator*(); }
};
enum Direction { FORWARD, BACKWARD, START=FORWARD, END=BACKWARD };
enum Direction
{
FORWARD,
BACKWARD,
START = FORWARD,
END = BACKWARD
};
typedef std::function<void(iterator it, bool updated)> NotifyFunction;
Interface(const NotifyFunction &notify = NotifyFunction());
Interface(const NotifyFunction& notify = NotifyFunction());
/// add a new InterfaceState
void add(InterfaceState &state);
void add(InterfaceState& state);
/// remove a state from the interface and return it as a one-element list
container_type remove(iterator it);
/// update state's priority (and call notify_ if it really has changed)
void updatePriority(InterfaceState *state, const InterfaceState::Priority &priority);
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
private:
const NotifyFunction notify_;
@ -187,10 +190,10 @@ private:
using base_type::remove_if;
};
class StagePrivate;
/// abstract base class for solutions (primitive and sequences)
class SolutionBase {
class SolutionBase
{
public:
virtual ~SolutionBase() = default;
@ -200,14 +203,14 @@ public:
template <Interface::Direction dir>
inline const InterfaceState::Solutions& trajectories() const;
inline void setStartState(const InterfaceState& state){
inline void setStartState(const InterfaceState& state) {
// only allow setting once (by Stage)
assert(start_ == NULL || start_ == &state);
start_ = &state;
const_cast<InterfaceState&>(state).addOutgoing(this);
}
inline void setEndState(const InterfaceState& state){
inline void setEndState(const InterfaceState& state) {
// only allow setting once (by Stage)
assert(end_ == NULL || end_ == &state);
end_ = &state;
@ -229,25 +232,20 @@ public:
const auto& markers() const { return markers_; }
/// append this solution to Solution msg
virtual void fillMessage(moveit_task_constructor_msgs::Solution &solution,
virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const = 0;
void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info,
Introspection* introspection = nullptr) const;
void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const;
/// order solutions by their cost
bool operator<(const SolutionBase& other) const {
return this->cost_ < other.cost_;
}
bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
protected:
SolutionBase(StagePrivate* creator = nullptr, double cost = 0.0, std::string comment = "")
: creator_(creator), cost_(cost), comment_(std::move(comment))
{
}
: creator_(creator), cost_(cost), comment_(std::move(comment)) {}
private:
// back-pointer to creating stage, allows to access sub-solutions
StagePrivate *creator_;
StagePrivate* creator_;
// associated cost
double cost_;
// comment for this solution, e.g. explanation of failure
@ -261,20 +259,19 @@ private:
};
MOVEIT_CLASS_FORWARD(SolutionBase)
/// SubTrajectory connects interface states of ComputeStages
class SubTrajectory : public SolutionBase {
class SubTrajectory : public SolutionBase
{
public:
SubTrajectory(const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(),
double cost = 0.0, std::string comment = "")
: SolutionBase(nullptr, cost, std::move(comment)), trajectory_(trajectory)
{}
SubTrajectory(
const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(),
double cost = 0.0, std::string comment = "")
: SolutionBase(nullptr, cost, std::move(comment)), trajectory_(trajectory) {}
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
void fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection* introspection = nullptr) const override;
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
private:
// actual trajectory, might be empty
@ -282,27 +279,24 @@ private:
};
MOVEIT_CLASS_FORWARD(SubTrajectory)
/** Sequence of individual sub solutions
*
* A solution sequence describes a solution that is composed from several individual
* sub solutions that need to be chained together to yield the overall solutions.
*/
class SolutionSequence : public SolutionBase {
class SolutionSequence : public SolutionBase
{
public:
typedef std::vector<const SolutionBase*> container_type;
explicit SolutionSequence()
: SolutionBase()
{}
explicit SolutionSequence() : SolutionBase() {}
SolutionSequence(container_type&& subsolutions, double cost = 0.0, StagePrivate* creator = nullptr)
: SolutionBase(creator, cost), subsolutions_(std::move(subsolutions))
{}
: SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {}
void push_back(const SolutionBase& solution);
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution &msg, Introspection *introspection) const override;
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); }
inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); }
@ -313,22 +307,23 @@ private:
};
MOVEIT_CLASS_FORWARD(SolutionSequence)
template <> inline
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
template <>
inline const InterfaceState::Solutions& SolutionBase::trajectories<Interface::FORWARD>() const {
return end_->outgoingTrajectories();
}
template <> inline
const InterfaceState::Solutions& SolutionBase::trajectories<Interface::BACKWARD>() const {
template <>
inline 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) {
template <>
struct less<moveit::task_constructor::SolutionBase*>
{
bool operator()(const moveit::task_constructor::SolutionBase* x, const moveit::task_constructor::SolutionBase* y) {
return *x < *y;
}
};

View File

@ -46,12 +46,15 @@
#include <moveit/macros/class_forward.h>
namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(RobotState)
}}
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
MOVEIT_CLASS_FORWARD(RobotState)
}
}
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
MOVEIT_CLASS_FORWARD(Stage)
MOVEIT_CLASS_FORWARD(ContainerBase)
@ -63,18 +66,19 @@ class TaskPrivate;
* Actually a tasks wraps a single container (by default a SerialContainer),
* which serves as the root of all stages.
*/
class Task : protected WrapperBase {
class Task : protected WrapperBase
{
public:
PRIVATE_CLASS(Task)
// +1 TODO: move into MoveIt! core
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
const std::string &ns = "move_group",
const std::string &planning_plugin_param_name = "planning_plugin",
const std::string &adapter_plugins_param_name = "request_adapters");
static planning_pipeline::PlanningPipelinePtr
createPlanner(const moveit::core::RobotModelConstPtr& model, const std::string& ns = "move_group",
const std::string& planning_plugin_param_name = "planning_plugin",
const std::string& adapter_plugins_param_name = "request_adapters");
Task(const std::string& id = "",
ContainerBase::pointer &&container = std::make_unique<SerialContainer>("task pipeline"));
Task(Task &&other);
ContainerBase::pointer&& container = std::make_unique<SerialContainer>("task pipeline"));
Task(Task&& other);
Task& operator=(Task&& other);
~Task();
@ -87,17 +91,17 @@ public:
void loadRobotModel(const std::string& robot_description = "robot_description");
// TODO: use Stage::insert as well?
void add(Stage::pointer &&stage);
void add(Stage::pointer&& stage);
void clear() final;
/// enable introspection publishing for use with rviz
void enableIntrospection(bool enable = true);
Introspection &introspection();
Introspection& introspection();
typedef std::function<void(const Task &t)> TaskCallback;
typedef std::function<void(const Task& t)> TaskCallback;
typedef std::list<TaskCallback> TaskCallbackList;
/// add function to be called after each top-level iteration
TaskCallbackList::const_iterator addTaskCallback(TaskCallback &&cb);
TaskCallbackList::const_iterator addTaskCallback(TaskCallback&& cb);
/// remove function callback
void erase(TaskCallbackList::const_iterator which);
@ -114,7 +118,7 @@ public:
void execute(const SolutionBase& s);
/// print current task state (number of found solutions and propagated states) to std::cout
void printState(std::ostream &os = std::cout) const;
void printState(std::ostream& os = std::cout) const;
size_t numSolutions() const { return solutions().size(); }
const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }
@ -125,24 +129,20 @@ public:
// +1 TODO: convenient access to arbitrary stage by name. traverse hierarchy using / separator?
/// access stage tree
ContainerBase *stages();
const ContainerBase *stages() const;
ContainerBase* stages();
const ContainerBase* stages() const;
/// properties access
PropertyMap& properties();
const PropertyMap& properties() const {
return const_cast<Task*>(this)->properties();
}
const PropertyMap& properties() const { return const_cast<Task*>(this)->properties(); }
void setProperty(const std::string& name, const boost::any& value);
/// overload: const char* values are stored as std::string
inline void setProperty(const std::string& name, const char* value) {
setProperty(name, std::string(value));
}
inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
protected:
bool canCompute() const override;
void compute() override;
void onNewSolution(const SolutionBase &s) override;
void onNewSolution(const SolutionBase& s) override;
private:
};
@ -151,5 +151,5 @@ inline std::ostream& operator<<(std::ostream& os, const Task& task) {
task.printState(os);
return os;
}
} }
}
}

View File

@ -42,13 +42,16 @@
#include <moveit/task_constructor/task.h>
namespace robot_model_loader {
MOVEIT_CLASS_FORWARD(RobotModelLoader)
MOVEIT_CLASS_FORWARD(RobotModelLoader)
}
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
class TaskPrivate : public WrapperBasePrivate {
class TaskPrivate : public WrapperBasePrivate
{
friend class Task;
public:
TaskPrivate(Task* me, const std::string& id);
const std::string& id() const { return id_; }
@ -65,8 +68,8 @@ private:
// introspection and monitoring
std::unique_ptr<Introspection> introspection_;
std::list<Task::TaskCallback> task_cbs_; // functions to monitor task's planning progress
std::list<Task::TaskCallback> task_cbs_; // functions to monitor task's planning progress
};
PIMPL_FUNCTIONS(Task)
} }
}
}

View File

@ -40,27 +40,23 @@
#include <type_traits>
namespace moveit { namespace task_constructor {
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 T, typename _ = void>
struct is_container : std::false_type
{};
template<typename... Ts>
struct is_container_helper {};
template <typename... Ts>
struct is_container_helper
{};
template<typename T>
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 {};
} }
T, std::conditional_t<false, is_container_helper<typename T::const_iterator, decltype(std::declval<T>().cbegin()),
decltype(std::declval<T>().cend())>,
void> > : public std::true_type
{};
}
}

View File

@ -42,11 +42,11 @@
#include <initializer_list>
/** template class to compose flags from enums in a type-safe fashion */
template<typename Enum>
template <typename Enum>
class Flags
{
static_assert((sizeof(Enum) <= sizeof(int)),
"Flags uses an int as storage, this enum will overflow!");
static_assert((sizeof(Enum) <= sizeof(int)), "Flags uses an int as storage, this enum will overflow!");
public:
typedef typename std::conditional<std::is_unsigned<Enum>::value, unsigned int, signed int>::type Int;
typedef Enum enum_type;
@ -58,15 +58,36 @@ public:
constexpr inline Flags(Enum f) noexcept : i(Int(f)) {}
// initialization from initializer_list
constexpr inline Flags(std::initializer_list<Enum> flags) noexcept
: i(initializer_list_helper(flags.begin(), flags.end())) {}
: i(initializer_list_helper(flags.begin(), flags.end())) {}
const inline Flags &operator&=(int mask) noexcept { i &= mask; return *this; }
const inline Flags &operator&=(unsigned int mask) noexcept { i &= mask; return *this; }
const inline Flags &operator&=(Enum mask) noexcept { i &= Int(mask); return *this; }
const inline Flags &operator|=(Flags f) noexcept { i |= f.i; return *this; }
const inline Flags &operator|=(Enum f) noexcept { i |= Int(f); return *this; }
const inline Flags &operator^=(Flags f) noexcept { i ^= f.i; return *this; }
const inline Flags &operator^=(Enum f) noexcept { i ^= Int(f); return *this; }
const inline Flags& operator&=(int mask) noexcept {
i &= mask;
return *this;
}
const inline Flags& operator&=(unsigned int mask) noexcept {
i &= mask;
return *this;
}
const inline Flags& operator&=(Enum mask) noexcept {
i &= Int(mask);
return *this;
}
const inline Flags& operator|=(Flags f) noexcept {
i |= f.i;
return *this;
}
const inline Flags& operator|=(Enum f) noexcept {
i |= Int(f);
return *this;
}
const inline Flags& operator^=(Flags f) noexcept {
i ^= f.i;
return *this;
}
const inline Flags& operator^=(Enum f) noexcept {
i ^= Int(f);
return *this;
}
constexpr inline operator Int() const noexcept { return i; }
@ -81,12 +102,15 @@ public:
constexpr inline bool operator!() const noexcept { return !i; }
constexpr inline bool testFlag(Enum f) const noexcept { return (i & Int(f)) == Int(f) && (Int(f) != 0 || i == Int(f) ); }
constexpr inline bool testFlag(Enum f) const noexcept {
return (i & Int(f)) == Int(f) && (Int(f) != 0 || i == Int(f));
}
private:
constexpr inline Flags(Int i) : i(i) {}
constexpr static inline Int initializer_list_helper(typename std::initializer_list<Enum>::const_iterator it,
typename std::initializer_list<Enum>::const_iterator end) noexcept {
constexpr static inline Int
initializer_list_helper(typename std::initializer_list<Enum>::const_iterator it,
typename std::initializer_list<Enum>::const_iterator end) noexcept {
return (it == end ? Int(0) : (Int(*it) | initializer_list_helper(it + 1, end)));
}

View File

@ -1,4 +1,4 @@
<package>
<package format="2">
<name>moveit_task_constructor_core</name>
<version>0.0.0</version>
<description>MoveIt Task Pipeline</description>
@ -9,27 +9,18 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>eigen_conversions</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>moveit_core</build_depend>
<build_depend>moveit_ros_planning</build_depend>
<build_depend>moveit_ros_planning_interface</build_depend>
<build_depend>moveit_task_constructor_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>rviz_marker_tools</build_depend>
<run_depend>eigen_conversions</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>moveit_core</run_depend>
<run_depend>moveit_ros_planning</run_depend>
<run_depend>moveit_ros_planning_interface</run_depend>
<run_depend>moveit_task_constructor_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>rviz_marker_tools</run_depend>
<depend>eigen_conversions</depend>
<depend>geometry_msgs</depend>
<depend>roscpp</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_task_constructor_msgs</depend>
<depend>visualization_msgs</depend>
<depend>rviz_marker_tools</depend>
<test_depend>rosunit</test_depend>
<test_depend>rostest</test_depend>
<export>
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml"/>

File diff suppressed because it is too large Load Diff

View File

@ -48,39 +48,42 @@
#include <boost/bimap.hpp>
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
namespace {
std::string getProcessId() {
char our_hostname[256] = {0};
gethostname(our_hostname, sizeof(our_hostname)-1);
char our_hostname[256] = { 0 };
gethostname(our_hostname, sizeof(our_hostname) - 1);
return std::to_string(getpid()) + "@" + our_hostname;
}
}
class IntrospectionPrivate {
class IntrospectionPrivate
{
public:
IntrospectionPrivate(const TaskPrivate *task)
: nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace
, task_(task)
, process_id_(getProcessId())
{
IntrospectionPrivate(const TaskPrivate* task)
: nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace
, task_(task)
, process_id_(getProcessId()) {
resetMaps();
task_description_publisher_ = nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
task_statistics_publisher_ = nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1, true);
task_description_publisher_ =
nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 1, true);
task_statistics_publisher_ =
nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1, true);
solution_publisher_ = nh_.advertise<moveit_task_constructor_msgs::Solution>(SOLUTION_TOPIC, 1, true);
}
void resetMaps () {
void resetMaps() {
// reset maps
stage_to_id_map_.clear();
stage_to_id_map_[task_] = 0; // root is task having ID = 0
stage_to_id_map_[task_] = 0; // root is task having ID = 0
id_solution_bimap_.clear();
}
ros::NodeHandle nh_;
/// associated task
const TaskPrivate *task_;
const TaskPrivate* task_;
const std::string process_id_;
/// publish task detailed description and current state
@ -96,31 +99,25 @@ public:
boost::bimap<uint32_t, const SolutionBase*> id_solution_bimap_;
};
Introspection::Introspection(const TaskPrivate* task)
: impl(new IntrospectionPrivate(task))
{
Introspection::Introspection(const TaskPrivate* task) : impl(new IntrospectionPrivate(task)) {
impl->get_solution_service_ = impl->nh_.advertiseService(GET_SOLUTION_SERVICE, &Introspection::getSolution, this);
}
Introspection::~Introspection()
{
Introspection::~Introspection() {
delete impl;
}
void Introspection::publishTaskDescription()
{
void Introspection::publishTaskDescription() {
::moveit_task_constructor_msgs::TaskDescription msg;
impl->task_description_publisher_.publish(fillTaskDescription(msg));
}
void Introspection::publishTaskState()
{
void Introspection::publishTaskState() {
::moveit_task_constructor_msgs::TaskStatistics msg;
impl->task_statistics_publisher_.publish(fillTaskStatistics(msg));
}
void Introspection::reset()
{
void Introspection::reset() {
// send empty task description message to indicate reset
::moveit_task_constructor_msgs::TaskDescription msg;
msg.process_id = impl->process_id_;
@ -130,29 +127,24 @@ void Introspection::reset()
impl->resetMaps();
}
void Introspection::registerSolution(const SolutionBase &s)
{
void Introspection::registerSolution(const SolutionBase& s) {
solutionId(s);
}
void Introspection::fillSolution(moveit_task_constructor_msgs::Solution &msg,
const SolutionBase &s)
{
void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s) {
s.fillMessage(msg, this);
msg.process_id = impl->process_id_;
msg.task_id = impl->task_->id();
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
}
void Introspection::publishSolution(const SolutionBase &s)
{
void Introspection::publishSolution(const SolutionBase& s) {
moveit_task_constructor_msgs::Solution msg;
fillSolution(msg, s);
impl->solution_publisher_.publish(msg);
}
void Introspection::publishAllSolutions(bool wait)
{
void Introspection::publishAllSolutions(bool wait) {
for (const auto& solution : impl->task_->stages()->solutions()) {
publishSolution(*solution);
@ -172,36 +164,32 @@ const SolutionBase* Introspection::solutionFromId(uint id) const {
return it->second;
}
bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request &req,
moveit_task_constructor_msgs::GetSolution::Response &res)
{
bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request& req,
moveit_task_constructor_msgs::GetSolution::Response& res) {
const SolutionBase* solution = solutionFromId(req.solution_id);
if (!solution) return false;
if (!solution)
return false;
fillSolution(res.solution, *solution);
return true;
}
uint32_t Introspection::stageId(const Stage* const s)
{
uint32_t Introspection::stageId(const Stage* const s) {
return impl->stage_to_id_map_.insert(std::make_pair(s->pimpl(), impl->stage_to_id_map_.size())).first->second;
}
uint32_t Introspection::stageId(const Stage* const s) const
{
uint32_t Introspection::stageId(const Stage* const s) const {
auto it = impl->stage_to_id_map_.find(s->pimpl());
if (it == impl->stage_to_id_map_.end())
throw std::runtime_error("unknown stage");
return it->second;
}
uint32_t Introspection::solutionId(const SolutionBase& s)
{
uint32_t Introspection::solutionId(const SolutionBase& s) {
auto result = impl->id_solution_bimap_.left.insert(std::make_pair(1 + impl->id_solution_bimap_.size(), &s));
return result.first->first;
}
void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s)
{
void Introspection::fillStageStatistics(const Stage& stage, moveit_task_constructor_msgs::StageStatistics& s) {
// successfull solutions
for (const auto& solution : stage.solutions())
s.solved.push_back(solutionId(*solution));
@ -213,10 +201,9 @@ void Introspection::fillStageStatistics(const Stage& stage, moveit_task_construc
s.num_failed = stage.numFailures();
}
moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription &msg)
{
ContainerBase::StageCallback stageProcessor =
[this, &msg](const Stage& stage, int) -> bool {
moveit_task_constructor_msgs::TaskDescription&
Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription& msg) {
ContainerBase::StageCallback stageProcessor = [this, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent
moveit_task_constructor_msgs::StageDescription desc;
desc.id = stageId(&stage);
@ -234,7 +221,7 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio
}
auto it = impl->stage_to_id_map_.find(stage.pimpl()->parent()->pimpl());
assert (it != impl->stage_to_id_map_.cend());
assert(it != impl->stage_to_id_map_.cend());
desc.parent_id = it->second;
// finally store in msg
@ -250,12 +237,11 @@ moveit_task_constructor_msgs::TaskDescription& Introspection::fillTaskDescriptio
return msg;
}
moveit_task_constructor_msgs::TaskStatistics& Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics &msg)
{
ContainerBase::StageCallback stageProcessor =
[this, &msg](const Stage& stage, int) -> bool {
moveit_task_constructor_msgs::TaskStatistics&
Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics& msg) {
ContainerBase::StageCallback stageProcessor = [this, &msg](const Stage& stage, int) -> bool {
// this method is called for each child stage of a given parent
moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg
moveit_task_constructor_msgs::StageStatistics stat; // create new Stage msg
stat.id = stageId(&stage);
fillStageStatistics(stage, stat);
@ -271,5 +257,5 @@ moveit_task_constructor_msgs::TaskStatistics& Introspection::fillTaskStatistics(
msg.process_id = impl->process_id_;
return msg;
}
} }
}
}

View File

@ -5,29 +5,28 @@
namespace vm = visualization_msgs;
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
/** generate marker msgs to visualize the planning scene, calling the given callback for each of them
* object_names: set of links to include (or all if empty) */
void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr &scene,
const MarkerCallback& callback,
const std::vector<std::string> &object_names)
{
void generateMarkersForObjects(const planning_scene::PlanningSceneConstPtr& scene, const MarkerCallback& callback,
const std::vector<std::string>& object_names) {
scene->printKnownObjects(std::cout);
/*
const std::vector<std::string>* names = object_names.empty() ? &scene->getCollisionObjectMsg()
: &link_names;
for (const auto &name : *names) {
visualization_msgs::MarkerArray markers;
robot_state.getRobotMarkers(markers, {name}, false);
for (auto &marker : markers.markers)
callback(marker, name);
}
*/
/*
const std::vector<std::string>* names = object_names.empty() ? &scene->getCollisionObjectMsg()
: &link_names;
for (const auto &name : *names) {
visualization_msgs::MarkerArray markers;
robot_state.getRobotMarkers(markers, {name}, false);
for (auto &marker : markers.markers)
callback(marker, name);
}
*/
}
visualization_msgs::Marker& createGeometryMarker(visualization_msgs::Marker& marker, const urdf::Geometry& geom, const urdf::Pose& pose,
const urdf::Color& color) {
visualization_msgs::Marker& createGeometryMarker(visualization_msgs::Marker& marker, const urdf::Geometry& geom,
const urdf::Pose& pose, const urdf::Color& color) {
rviz_marker_tools::makeFromGeometry(marker, geom);
marker.pose.position.x = pose.position.x;
marker.pose.position.y = pose.position.y;
@ -58,20 +57,41 @@ const urdf::Color& materialColor(const urdf::ModelInterface& model, const std::s
}
// type traits to access collision/visual array or single element
template <class T> const std::vector<T>& elements_vector(const urdf::Link& link);
template <> const std::vector<urdf::CollisionSharedPtr>& elements_vector(const urdf::Link& link) { return link.collision_array; }
template <> const std::vector<urdf::VisualSharedPtr>& elements_vector(const urdf::Link& link) { return link.visual_array; }
template <class T>
const std::vector<T>& elements_vector(const urdf::Link& link);
template <>
const std::vector<urdf::CollisionSharedPtr>& elements_vector(const urdf::Link& link) {
return link.collision_array;
}
template <>
const std::vector<urdf::VisualSharedPtr>& elements_vector(const urdf::Link& link) {
return link.visual_array;
}
template <class T> const T& element(const urdf::Link& link);
template <> const urdf::CollisionSharedPtr& element(const urdf::Link& link) { return link.collision; }
template <> const urdf::VisualSharedPtr& element(const urdf::Link& link) { return link.visual; }
template <class T>
const T& element(const urdf::Link& link);
template <>
const urdf::CollisionSharedPtr& element(const urdf::Link& link) {
return link.collision;
}
template <>
const urdf::VisualSharedPtr& element(const urdf::Link& link) {
return link.visual;
}
template <class T> const std::string& materialName(const T& element);
template <> const std::string& materialName(const urdf::Visual& element) { return element.material_name; }
template <> const std::string& materialName(const urdf::Collision& element) { static std::string empty; return empty; }
template <class T>
const std::string& materialName(const T& element);
template <>
const std::string& materialName(const urdf::Visual& element) {
return element.material_name;
}
template <>
const std::string& materialName(const urdf::Collision& element) {
static std::string empty;
return empty;
}
std::vector<std::string> linkNames(const std::vector<const moveit::core::LinkModel*>& link_models)
{
std::vector<std::string> linkNames(const std::vector<const moveit::core::LinkModel*>& link_models) {
std::vector<std::string> names;
names.reserve(link_models.size());
for (const moveit::core::LinkModel* link : link_models)
@ -81,29 +101,28 @@ std::vector<std::string> linkNames(const std::vector<const moveit::core::LinkMod
/** generate marker msgs to visualize the robot state, calling the given callback for each of them
* link_names: set of links to include (or all if empty) */
template <class T> // with T = urdf::Visual or urdf::Collision
void generateMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<std::string> &link_names = {})
{
const std::vector<std::string>* names = link_names.empty() ? &robot_state.getRobotModel()->getLinkModelNames()
: &link_names;
template <class T> // with T = urdf::Visual or urdf::Collision
void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<std::string>& link_names = {}) {
const std::vector<std::string>* names =
link_names.empty() ? &robot_state.getRobotModel()->getLinkModelNames() : &link_names;
const urdf::ModelInterfaceSharedPtr& model = robot_state.getRobotModel()->getURDF();
if (!model) return;
if (!model)
return;
visualization_msgs::Marker m;
m.header.frame_id = robot_state.getRobotModel()->getModelFrame();
// code adapted from rviz::RobotLink::createVisual() / createCollision()
for (const auto &name : *names) {
for (const auto& name : *names) {
const urdf::LinkConstSharedPtr& link = model->getLink(name);
if (!link) return;
if (!link)
return;
bool valid_found = false;
auto element_handler = [&](const T& element){
auto element_handler = [&](const T& element) {
if (element && element->geometry) {
createGeometryMarker(m, *element->geometry, element->origin,
materialColor(*model, materialName(*element)));
createGeometryMarker(m, *element->geometry, element->origin, materialColor(*model, materialName(*element)));
m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose);
callback(m, name);
valid_found = true;
@ -111,7 +130,7 @@ void generateMarkers(const moveit::core::RobotState &robot_state,
};
// either we have an array of collision/visual elements
for(const auto& element : elements_vector<T>(*link))
for (const auto& element : elements_vector<T>(*link))
element_handler(element);
// or there is a single such element
@ -120,34 +139,29 @@ void generateMarkers(const moveit::core::RobotState &robot_state,
}
}
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<std::string> &link_names) {
void generateCollisionMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<std::string>& link_names) {
generateMarkers<urdf::CollisionSharedPtr>(robot_state, callback, link_names);
}
void generateCollisionMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback &callback,
const std::vector<const moveit::core::LinkModel*> &link_models) {
void generateCollisionMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<const moveit::core::LinkModel*>& link_models) {
generateMarkers<urdf::CollisionSharedPtr>(robot_state, callback, linkNames(link_models));
}
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback& callback,
const std::vector<std::string> &link_names) {
void generateVisualMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<std::string>& link_names) {
generateMarkers<urdf::VisualSharedPtr>(robot_state, callback, link_names);
}
void generateVisualMarkers(const moveit::core::RobotState &robot_state,
const MarkerCallback &callback,
const std::vector<const moveit::core::LinkModel*> &link_models) {
void generateVisualMarkers(const moveit::core::RobotState& robot_state, const MarkerCallback& callback,
const std::vector<const moveit::core::LinkModel*>& link_models) {
generateMarkers<urdf::VisualSharedPtr>(robot_state, callback, linkNames(link_models));
}
/** generate marker msgs to visualize the planning scene, calling the given callback for each of them
* calls generateMarkersForRobot() and generateMarkersForObjects() */
void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr &scene,
const MarkerCallback &callback) {
void generateMarkersForScene(const planning_scene::PlanningSceneConstPtr& scene, const MarkerCallback& callback) {
generateMarkers<urdf::VisualSharedPtr>(scene->getCurrentState(), callback);
generateMarkersForObjects(scene, callback);
}
} }
}
}

View File

@ -38,10 +38,10 @@
#include <moveit/task_constructor/merge.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups)
{
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups) {
if (groups.size() <= 1)
throw std::runtime_error("Expected multiple groups");
@ -66,9 +66,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& groups,
std::vector<const moveit::core::JointModel*> joints,
std::vector<const moveit::core::JointModel*>& duplicates,
std::string& names)
{
std::vector<const moveit::core::JointModel*>& duplicates, std::string& names) {
duplicates.clear();
names.clear();
for (const moveit::core::JointModelGroup* jmg : groups) {
@ -78,7 +76,8 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
if (jm->getType() != moveit::core::JointModel::FIXED && // fixed joints are OK
std::find(duplicates.begin(), duplicates.end(), jm) == duplicates.end()) {
duplicates.push_back(jm);
if (!names.empty()) names.append(", ");
if (!names.empty())
names.append(", ");
names.append(jm->getName());
}
continue;
@ -89,14 +88,14 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
return duplicates.size() > 0;
}
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group)
{
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) {
if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions");
const std::vector<const moveit::core::JointModel*> *merged_joints
= merged_group ? &merged_group->getJointModels() : nullptr;
const std::vector<const moveit::core::JointModel*>* merged_joints =
merged_group ? &merged_group->getJointModels() : nullptr;
std::set<const moveit::core::JointModel*> jset;
std::vector<const moveit::core::JointModelGroup*> groups;
groups.reserve(sub_trajectories.size());
@ -118,7 +117,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend())
throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName());
}
} else // accumulate set of joints
} else // accumulate set of joints
jset.insert(joints.cbegin(), joints.cend());
max_num_joints = std::max(max_num_joints, joints.size());
@ -129,8 +128,10 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
if (num_merged != num_joints) {
// overlapping joint groups: analyse in more detail
std::vector<const moveit::core::JointModel*> joints;
if (merged_joints) joints = *merged_joints;
else joints.insert(joints.end(), jset.cbegin(), jset.cend());
if (merged_joints)
joints = *merged_joints;
else
joints.insert(joints.end(), jset.cbegin(), jset.cend());
std::vector<const moveit::core::JointModel*> duplicates;
std::string names;
@ -178,5 +179,5 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
timing.computeTimeStamps(*merged_traj, 1.0, 1.0);
return merged_traj;
}
} }
}
}

View File

@ -46,8 +46,10 @@ namespace task_constructor {
const std::string LOGNAME = "Properties";
class PropertyTypeRegistry {
struct Entry {
class PropertyTypeRegistry
{
struct Entry
{
std::string name_;
PropertySerializerBase::SerializeFunction serialize_;
PropertySerializerBase::DeserializeFunction deserialize_;
@ -62,9 +64,8 @@ class PropertyTypeRegistry {
TypeNameMap names_;
public:
PropertyTypeRegistry() : dummy_{"", PropertySerializerBase::dummySerialize,
PropertySerializerBase::dummyDeserialize}
{}
PropertyTypeRegistry()
: dummy_{ "", PropertySerializerBase::dummySerialize, PropertySerializerBase::dummyDeserialize } {}
inline bool insert(const std::type_index& type_index, const std::string& type_name,
PropertySerializerBase::SerializeFunction serialize,
PropertySerializerBase::DeserializeFunction deserialize);
@ -88,12 +89,11 @@ static PropertyTypeRegistry registry_singleton_;
bool PropertyTypeRegistry::insert(const std::type_index& type_index, const std::string& type_name,
PropertySerializerBase::SerializeFunction serialize,
PropertySerializerBase::DeserializeFunction deserialize)
{
PropertySerializerBase::DeserializeFunction deserialize) {
if (type_index == std::type_index(typeid(boost::any)))
return false;
auto it_inserted = types_.insert(std::make_pair(type_index, Entry {type_name, serialize, deserialize}));
auto it_inserted = types_.insert(std::make_pair(type_index, Entry{ type_name, serialize, deserialize }));
if (!it_inserted.second)
return false; // was already registered before
@ -105,68 +105,54 @@ bool PropertyTypeRegistry::insert(const std::type_index& type_index, const std::
bool PropertySerializerBase::insert(const std::type_index& type_index, const std::string& type_name,
PropertySerializerBase::SerializeFunction serialize,
PropertySerializerBase::DeserializeFunction deserialize)
{
PropertySerializerBase::DeserializeFunction deserialize) {
return registry_singleton_.insert(type_index, type_name, serialize, deserialize);
}
Property::Property(const type_info& type_info, const std::string& description, const boost::any& default_value)
: description_(description)
, type_info_(type_info)
, default_(default_value)
, value_()
, initialized_from_(-1)
{
: description_(description), type_info_(type_info), default_(default_value), value_(), initialized_from_(-1) {
// default value's type should match declared type by construction
assert(default_.empty() || default_.type() == type_info_ || type_info_ == typeid(boost::any));
reset();
}
Property::Property() : Property(typeid(boost::any), "", boost::any())
{
}
Property::Property() : Property(typeid(boost::any), "", boost::any()) {}
void Property::setValue(const boost::any &value) {
void Property::setValue(const boost::any& value) {
setCurrentValue(value);
default_ = value_;
initialized_from_ = 0;
}
void Property::setCurrentValue(const boost::any &value)
{
void Property::setCurrentValue(const boost::any& value) {
if (!value.empty() && type_info_ != typeid(boost::any) && value.type() != type_info_)
throw Property::type_error(value.type().name(), type_info_.name());
value_ = value;
initialized_from_ = 1; // manually initialized TODO: use enums
initialized_from_ = 1; // manually initialized TODO: use enums
}
void Property::setDefaultValue(const boost::any& value)
{
void Property::setDefaultValue(const boost::any& value) {
if (!value.empty() && type_info_ != typeid(boost::any) && value.type() != type_info_)
throw Property::type_error(value.type().name(), type_info_.name());
default_ = value;
}
void Property::reset()
{
void Property::reset() {
if (initialized_from_ == 0) // TODO: use enum
return; // keep manually set values
boost::any().swap(value_);
initialized_from_ = -1; // set to max value
}
std::string Property::serialize(const boost::any& value)
{
std::string Property::serialize(const boost::any& value) {
if (value.empty())
return "";
return registry_singleton_.entry(value.type()).serialize_(value);
}
boost::any Property::deserialize(const std::string& type_name, const std::string& wire)
{
boost::any Property::deserialize(const std::string& type_name, const std::string& wire) {
if (type_name != Property::typeName(typeid(std::string)) && wire.empty())
return boost::any();
else
@ -174,25 +160,24 @@ boost::any Property::deserialize(const std::string& type_name, const std::string
}
std::string Property::typeName() const {
if (value().empty()) return typeName(type_info_);
else return typeName(value().type());
if (value().empty())
return typeName(type_info_);
else
return typeName(value().type());
}
std::string Property::typeName(const type_info& type_info)
{
std::string Property::typeName(const type_info& type_info) {
if (type_info == typeid(boost::any))
return "";
else
return registry_singleton_.entry(type_info).name_;
}
bool Property::initsFrom(Property::SourceFlags source) const
{
bool Property::initsFrom(Property::SourceFlags source) const {
return source & source_flags_;
}
Property& Property::configureInitFrom(SourceFlags source, const Property::InitializerFunction &f)
{
Property& Property::configureInitFrom(SourceFlags source, const Property::InitializerFunction& f) {
if (source != source_flags_ && initializer_)
throw error("Property was already configured for initialization from another source id");
@ -201,15 +186,12 @@ Property& Property::configureInitFrom(SourceFlags source, const Property::Initia
return *this;
}
Property &Property::configureInitFrom(SourceFlags source, const std::string &name)
{
Property& Property::configureInitFrom(SourceFlags source, const std::string& name) {
return configureInitFrom(source, [name](const PropertyMap& other) { return fromName(other, name); });
}
Property& PropertyMap::declare(const std::string &name, const Property::type_info& type_info,
const std::string &description, const boost::any &default_value)
{
Property& PropertyMap::declare(const std::string& name, const Property::type_info& type_info,
const std::string& description, const boost::any& default_value) {
auto it_inserted = props_.insert(std::make_pair(name, Property(type_info, description, default_value)));
// if name was already declared, the new declaration should match in type (except it was boost::any)
if (!it_inserted.second && it_inserted.first->second.type_info_ != typeid(boost::any) &&
@ -218,35 +200,30 @@ Property& PropertyMap::declare(const std::string &name, const Property::type_inf
return it_inserted.first->second;
}
bool PropertyMap::hasProperty(const std::string& name) const
{
bool PropertyMap::hasProperty(const std::string& name) const {
auto it = props_.find(name);
return it != props_.end();
}
Property& PropertyMap::property(const std::string &name)
{
Property& PropertyMap::property(const std::string& name) {
auto it = props_.find(name);
if (it == props_.end())
throw Property::undeclared(name);
return it->second;
}
void PropertyMap::exposeTo(PropertyMap& other, const std::set<std::string> &properties) const
{
void PropertyMap::exposeTo(PropertyMap& other, const std::set<std::string>& properties) const {
for (const std::string& name : properties)
exposeTo(other, name, name);
}
void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const
{
void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const {
const Property& p = property(name);
other.declare(other_name, p.type_info_, p.description_, p.default_);
}
void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set<std::string> &properties)
{
for (auto &pair : props_) {
void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set<std::string>& properties) {
for (auto& pair : props_) {
if (properties.empty() || properties.count(pair.first))
try {
pair.second.configureInitFrom(source, std::bind(&fromName, std::placeholders::_1, pair.first));
@ -260,7 +237,7 @@ void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set
template <>
void PropertyMap::set<boost::any>(const std::string& name, const boost::any& value) {
auto range = props_.equal_range(name);
if (range.first == range.second) { // name is not yet declared
if (range.first == range.second) { // name is not yet declared
if (value.empty())
throw Property::undeclared(name, "trying to set undeclared property '" + name + "' with NULL value");
auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any())));
@ -269,35 +246,31 @@ void PropertyMap::set<boost::any>(const std::string& name, const boost::any& val
range.first->second.setValue(value);
}
void PropertyMap::setCurrent(const std::string &name, const boost::any &value)
{
void PropertyMap::setCurrent(const std::string& name, const boost::any& value) {
property(name).setCurrentValue(value);
}
const boost::any &PropertyMap::get(const std::string &name) const
{
const boost::any& PropertyMap::get(const std::string& name) const {
return property(name).value();
}
size_t PropertyMap::countDefined(const std::vector<std::string> &list) const
{
size_t PropertyMap::countDefined(const std::vector<std::string>& list) const {
size_t count = 0u;
for (const std::string& name : list) {
if (!get(name).empty()) ++count;
if (!get(name).empty())
++count;
}
return count;
}
void PropertyMap::reset()
{
void PropertyMap::reset() {
for (auto& pair : props_)
pair.second.reset();
}
void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMap &other)
{
void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMap& other) {
for (auto& pair : props_) {
Property &p = pair.second;
Property& p = pair.second;
// don't override value previously set by higher-priority source
// MANUAL > CURRENT > PARENT > INTERFACE
@ -316,48 +289,36 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa
} catch (const Property::undefined&) {
}
ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ <<
" -> " << source << ": " << Property::serialize(value));
ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << " -> " << source << ": "
<< Property::serialize(value));
p.setCurrentValue(value);
p.initialized_from_ = source;
}
}
boost::any fromName(const PropertyMap& other, const std::string& other_name)
{
boost::any fromName(const PropertyMap& other, const std::string& other_name) {
return other.get(other_name);
}
Property::error::error(const std::string& msg) : std::runtime_error(msg), msg_("Property: " + msg) {}
Property::error::error(const std::string& msg)
: std::runtime_error(msg)
, msg_("Property: " + msg)
{}
void Property::error::setName(const std::string& name)
{
void Property::error::setName(const std::string& name) {
property_name_ = name;
// compose message from property name and runtime_errors' msg
msg_ = "Property '" + name + "': " + std::runtime_error::what();
}
Property::undeclared::undeclared(const std::string& name, const std::string& msg)
: Property::error(msg)
{
Property::undeclared::undeclared(const std::string& name, const std::string& msg) : Property::error(msg) {
setName(name);
}
Property::undefined::undefined(const std::string& name, const std::string& msg)
: Property::error(msg)
{
Property::undefined::undefined(const std::string& name, const std::string& msg) : Property::error(msg) {
setName(name);
}
static boost::format type_error_fmt("type (%1%) doesn't match property's declared type (%2%)");
Property::type_error::type_error(const std::string& current_type, const std::string& declared_type)
: Property::error(boost::str(type_error_fmt % current_type % declared_type))
{}
: Property::error(boost::str(type_error_fmt % current_type % declared_type)) {}
} // namespace task_constructor
} // namespace moveit
} // namespace task_constructor
} // namespace moveit

View File

@ -40,27 +40,23 @@
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
namespace moveit { namespace task_constructor { namespace solvers {
namespace moveit {
namespace task_constructor {
namespace solvers {
CartesianPath::CartesianPath()
{
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");
}
void CartesianPath::init(const core::RobotModelConstPtr &robot_model)
{
}
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_msgs::Constraints& path_constraints)
{
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
if (!link) {
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
@ -68,39 +64,31 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
}
// reach pose of forward kinematics
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link),
jmg, timeout, result, path_constraints);
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints);
}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Isometry3d &target,
const moveit::core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr &result,
const moveit_msgs::Constraints& path_constraints)
{
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
kinematic_constraints::KinematicConstraintSet kcs(sandbox_scene->getRobotModel());
kcs.add(path_constraints, sandbox_scene->getTransforms());
auto isValid = [&sandbox_scene, &kcs](moveit::core::RobotState* state,
const moveit::core::JointModelGroup* jmg,
const double* joint_positions) {
auto isValid = [&sandbox_scene, &kcs](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())
&& kcs.decide(*state).satisfied;
return !sandbox_scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName()) &&
kcs.decide(*state).satisfied;
};
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);
jmg, trajectory, &link, target, true, props.get<double>("step_size"), props.get<double>("jump_threshold"),
isValid);
if (!trajectory.empty()) {
result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg));
@ -108,12 +96,12 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
result->addSuffixWayPoint(waypoint, 0.0);
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result,
props.get<double>("max_velocity_scaling_factor"),
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
}
return achieved_fraction >= props.get<double>("min_fraction");
}
} } }
}
}
}

View File

@ -40,25 +40,22 @@
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
namespace moveit { namespace task_constructor { namespace solvers {
namespace moveit {
namespace task_constructor {
namespace solvers {
JointInterpolationPlanner::JointInterpolationPlanner()
{
JointInterpolationPlanner::JointInterpolationPlanner() {
auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step");
}
void JointInterpolationPlanner::init(const core::RobotModelConstPtr &robot_model)
{
}
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& robot_model) {}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup *jmg,
double timeout,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints)
{
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
// Get maximum joint distance
@ -90,22 +87,19 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
// add timing, TODO: use a generic method to add timing via plugins
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result,
props.get<double>("max_velocity_scaling_factor"),
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return true;
}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel &link,
const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints)
{
throw std::runtime_error("Not yet implemented");
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
throw std::runtime_error("Not yet implemented");
}
}
}
}
} } }

View File

@ -44,39 +44,38 @@
#include <moveit/kinematic_constraints/utils.h>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace solvers {
namespace moveit {
namespace task_constructor {
namespace solvers {
PipelinePlanner::PipelinePlanner()
{
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<moveit_msgs::WorkspaceParameters>("workspace_parameters", moveit_msgs::WorkspaceParameters(), "allowed workspace of mobile base?");
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");
p.declare<bool>("display_motion_plans", false,
"publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
"publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
p.declare<bool>("publish_planning_requests", false,
"publish motion planning requests on topic " + planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
"publish motion planning requests on topic " +
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
}
void PipelinePlanner::init(const core::RobotModelConstPtr &robot_model)
{
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
planner_ = Task::createPlanner(robot_model);
planner_->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
planner_->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
}
void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req,
const PropertyMap& p,
const moveit::core::JointModelGroup *jmg,
double timeout)
{
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;
@ -89,38 +88,30 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req,
}
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 moveit_msgs::Constraints& path_constraints)
{
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
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"));
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(to->getCurrentState(), jmg,
props.get<double>("goal_joint_tolerance"));
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res;
if(!planner_->generatePlan(from, req, 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::Isometry3d& target_eigen,
const moveit::core::JointModelGroup *jmg,
double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints)
{
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout);
@ -131,17 +122,17 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
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"));
link.getName(), target, props.get<double>("goal_position_tolerance"),
props.get<double>("goal_orientation_tolerance"));
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res;
if(!planner_->generatePlan(from, req, res))
if (!planner_->generatePlan(from, req, res))
return false;
result = res.trajectory_;
return true;
}
} } }
}
}
}

View File

@ -38,13 +38,15 @@
#include <moveit/task_constructor/solvers/planner_interface.h>
namespace moveit { namespace task_constructor { namespace solvers {
namespace moveit {
namespace task_constructor {
namespace solvers {
PlannerInterface::PlannerInterface()
{
PlannerInterface::PlannerInterface() {
auto& p = properties();
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");
}
} } }
}
}
}

View File

@ -45,48 +45,46 @@
#include <ros/console.h>
#include <utility>
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
void InitStageException::push_back(const Stage &stage, const std::string &msg)
{
void InitStageException::push_back(const Stage& stage, const std::string& msg) {
errors_.emplace_back(std::make_pair(&stage, msg));
}
void InitStageException::append(InitStageException &other)
{
void InitStageException::append(InitStageException& other) {
errors_.splice(errors_.end(), other.errors_);
}
const char *InitStageException::what() const noexcept
{
const char* InitStageException::what() const noexcept {
static const char* msg = "Error initializing stage(s)";
return msg;
}
std::ostream& operator<<(std::ostream& os, const InitStageException& e) {
os << "Error initializing stage" << (e.errors_.size() > 1 ? "s" : "") << ":" << std::endl;
for (const auto &pair : e.errors_)
for (const auto& pair : e.errors_)
os << pair.first->name() << ": " << pair.second << std::endl;
return os;
}
StagePrivate::StagePrivate(Stage* me, const std::string& name)
: me_(me), name_(name), parent_(nullptr), introspection_(nullptr) {}
StagePrivate::StagePrivate(Stage *me, const std::string &name)
: me_(me), name_(name), parent_(nullptr), introspection_(nullptr)
{}
InterfaceFlags StagePrivate::interfaceFlags() const
{
InterfaceFlags StagePrivate::interfaceFlags() const {
InterfaceFlags f;
if (starts()) f |= READS_START;
if (ends()) f |= READS_END;
if (prevEnds()) f |= WRITES_PREV_END;
if (nextStarts()) f |= WRITES_NEXT_START;
if (starts())
f |= READS_START;
if (ends())
f |= READS_END;
if (prevEnds())
f |= WRITES_PREV_END;
if (nextStarts())
f |= WRITES_NEXT_START;
return f;
}
void StagePrivate::validateConnectivity() const
{
void StagePrivate::validateConnectivity() const {
// check that the required interface is provided
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
@ -94,8 +92,7 @@ void StagePrivate::validateConnectivity() const
throw InitStageException(*me(), "required interface is not satisfied");
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution)
{
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
solution->setCreator(this);
if (introspection_)
introspection_->registerSolution(*solution);
@ -111,8 +108,7 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution)
return true;
}
void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, SolutionBasePtr solution)
{
void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, SolutionBasePtr solution) {
assert(nextStarts());
if (!storeSolution(solution))
return; // solution dropped
@ -129,8 +125,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
newSolution(solution);
}
void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, SolutionBasePtr solution)
{
void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, SolutionBasePtr solution) {
assert(prevEnds());
if (!storeSolution(solution))
return; // solution dropped
@ -147,13 +142,12 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
newSolution(solution);
}
void StagePrivate::spawn(InterfaceState&& state, SolutionBasePtr solution)
{
void StagePrivate::spawn(InterfaceState&& state, SolutionBasePtr solution) {
assert(prevEnds() && nextStarts());
if (!storeSolution(solution))
return; // solution dropped
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
auto to = states_.insert(states_.end(), std::move(state));
solution->setStartState(*from);
@ -167,8 +161,7 @@ void StagePrivate::spawn(InterfaceState&& state, SolutionBasePtr solution)
newSolution(solution);
}
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, SolutionBasePtr solution)
{
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, SolutionBasePtr solution) {
if (!storeSolution(solution))
return; // solution dropped
@ -178,8 +171,7 @@ void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to,
newSolution(solution);
}
void StagePrivate::newSolution(const SolutionBasePtr& solution)
{
void StagePrivate::newSolution(const SolutionBasePtr& solution) {
// call solution callbacks for both, valid solutions and failures
for (const auto& cb : solution_cbs_)
cb(*solution);
@ -188,9 +180,7 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution)
parent()->onNewSolution(*solution);
}
Stage::Stage(StagePrivate *impl)
: pimpl_(impl)
{
Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
assert(impl);
auto& p = properties();
p.declare<double>("timeout", "timeout per run (s)");
@ -200,12 +190,11 @@ Stage::Stage(StagePrivate *impl)
"set of interface properties to forward");
}
Stage::~Stage()
{
Stage::~Stage() {
delete pimpl_;
}
Stage::operator StagePrivate *() {
Stage::operator StagePrivate*() {
return pimpl();
}
@ -213,8 +202,7 @@ Stage::operator const StagePrivate*() const {
return pimpl();
}
void Stage::reset()
{
void Stage::reset() {
auto impl = pimpl();
// clear solutions + associated states
impl->solutions_.clear();
@ -222,8 +210,10 @@ void Stage::reset()
impl->num_failures_ = 0u;
impl->states_.clear();
// clear pull interfaces
if (impl->starts_) impl->starts_->clear();
if (impl->ends_) impl->ends_->clear();
if (impl->starts_)
impl->starts_->clear();
if (impl->ends_)
impl->ends_->clear();
// reset push interfaces
impl->prev_ends_.reset();
impl->next_starts_.reset();
@ -231,8 +221,7 @@ void Stage::reset()
impl->properties_.reset();
}
void Stage::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void Stage::init(const moveit::core::RobotModelConstPtr& robot_model) {
// init properties once from parent
auto impl = pimpl();
impl->properties_.reset();
@ -240,7 +229,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& robot_model)
try {
ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'");
impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
} catch (const Property::error &e) {
} catch (const Property::error& e) {
std::ostringstream oss;
oss << e.what();
// skip this stage and start error reporting at parent
@ -250,7 +239,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& robot_model)
}
}
const ContainerBase *Stage::parent() const {
const ContainerBase* Stage::parent() const {
return pimpl_->parent_;
}
@ -258,13 +247,11 @@ const std::string& Stage::name() const {
return pimpl_->name_;
}
void Stage::setName(const std::string& name)
{
void Stage::setName(const std::string& name) {
pimpl_->name_ = name;
}
void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest)
{
void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) {
const PropertyMap& src = source.properties();
PropertyMap& dst = dest.properties();
for (const auto& name : forwardedProperties()) {
@ -274,29 +261,24 @@ void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest
}
}
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb)
{
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback&& cb) {
auto impl = pimpl();
impl->solution_cbs_.emplace_back(std::move(cb));
return --impl->solution_cbs_.cend();
}
void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which)
{
void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) {
pimpl()->solution_cbs_.erase(which);
}
const ordered<SolutionBaseConstPtr>& Stage::solutions() const
{
const ordered<SolutionBaseConstPtr>& Stage::solutions() const {
return pimpl()->solutions_;
}
const std::list<SolutionBaseConstPtr>& Stage::failures() const
{
const std::list<SolutionBaseConstPtr>& Stage::failures() const {
return pimpl()->failures_;
}
size_t Stage::numFailures() const
{
size_t Stage::numFailures() const {
return pimpl()->num_failures_;
}
@ -308,9 +290,7 @@ bool Stage::storeFailures() const {
return pimpl()->storeFailures();
}
PropertyMap &Stage::properties()
{
PropertyMap& Stage::properties() {
return pimpl()->properties_;
}
@ -318,9 +298,9 @@ void Stage::setProperty(const std::string& name, const boost::any& value) {
pimpl()->properties_.set(name, value);
}
void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std::ostream& os)
{
if (property_name.empty()) return;
void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std::ostream& os) {
if (property_name.empty())
return;
os << "\nin stage '" << name() << "': ";
try {
const auto& p = properties_.property(property_name);
@ -330,100 +310,108 @@ void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std
} else
os << "declared, but undefined";
if (p.initsFrom(Stage::PARENT)) os << ", inherits from parent";
if (p.initsFrom(Stage::INTERFACE)) os << ", initializes from interface";
} catch (const Property::undeclared &e) {
if (p.initsFrom(Stage::PARENT))
os << ", inherits from parent";
if (p.initsFrom(Stage::INTERFACE))
os << ", initializes from interface";
} catch (const Property::undeclared& e) {
os << "undeclared";
}
if (parent()->parent())
parent()->pimpl()->composePropertyErrorMsg(property_name, os);
}
void Stage::reportPropertyError(const Property::error& e)
{
void Stage::reportPropertyError(const Property::error& e) {
std::ostringstream oss;
oss << e.what();
pimpl()->composePropertyErrorMsg(e.name(), oss);
throw std::runtime_error(oss.str());
}
template<InterfaceFlag own, InterfaceFlag other>
template <InterfaceFlag own, InterfaceFlag other>
const char* direction(const StagePrivate& stage) {
InterfaceFlags f = stage.interfaceFlags();
bool own_if = f & own;
bool other_if = f & other;
bool reverse = own & START_IF_MASK;
if (own_if && other_if) return "<>";
if (!own_if && !other_if) return "--";
if (other_if ^ reverse) return "->";
if (own_if && other_if)
return "<>";
if (!own_if && !other_if)
return "--";
if (other_if ^ reverse)
return "->";
return "<-";
}
const char* flowSymbol(InterfaceFlags f) {
if (f == UNKNOWN)
return "?"; // unknown interface
return "?"; // unknown interface
// f should have either INPUT or OUTPUT flags set (not both)
assert(static_cast<bool>(f & START_IF_MASK) ^ static_cast<bool>(f & END_IF_MASK));
if (f & START_IF_MASK) {
if (f == READS_START) return "";
if (f == WRITES_PREV_END) return "";
if (f == READS_START)
return "";
if (f == WRITES_PREV_END)
return "";
} else if (f & END_IF_MASK) {
if (f == READS_END) return "";
if (f == WRITES_NEXT_START) return "";
if (f == READS_END)
return "";
if (f == WRITES_NEXT_START)
return "";
}
return "";
}
std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
// starts
for (const InterfaceConstPtr& i : {impl.prevEnds(), impl.starts()}) {
for (const InterfaceConstPtr& i : { impl.prevEnds(), impl.starts() }) {
os << std::setw(3);
if (i) os << i->size();
else os << "-";
if (i)
os << i->size();
else
os << "-";
}
// trajectories
os << std::setw(5) << direction<READS_START, WRITES_PREV_END>(impl)
<< std::setw(3) << impl.solutions_.size()
os << std::setw(5) << direction<READS_START, WRITES_PREV_END>(impl) << std::setw(3) << impl.solutions_.size()
<< std::setw(5) << direction<READS_END, WRITES_NEXT_START>(impl);
// ends
for (const InterfaceConstPtr& i : {impl.ends(), impl.nextStarts()}) {
for (const InterfaceConstPtr& i : { impl.ends(), impl.nextStarts() }) {
os << std::setw(3);
if (i) os << i->size();
else os << "-";
if (i)
os << i->size();
else
os << "-";
}
// name
os << " / " << impl.name();
return os;
}
ComputeBase::ComputeBase(ComputeBasePrivate* impl) : Stage(impl) {}
ComputeBase::ComputeBase(ComputeBasePrivate *impl)
: Stage(impl)
{
}
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay *me, PropagatingEitherWay::Direction dir, const std::string &name)
: ComputeBasePrivate(me, name), required_interface_dirs_(dir)
{
PropagatingEitherWayPrivate::PropagatingEitherWayPrivate(PropagatingEitherWay* me, PropagatingEitherWay::Direction dir,
const std::string& name)
: ComputeBasePrivate(me, name), required_interface_dirs_(dir) {
initInterface(required_interface_dirs_);
}
// initialize pull interfaces to match requested propagation directions
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir)
{
void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction dir) {
if (dir & PropagatingEitherWay::FORWARD) {
if (!starts_) // keep existing interface if possible
starts_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1)));
starts_.reset(
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedStarts, this, std::placeholders::_1)));
} else {
starts_.reset();
}
if (dir & PropagatingEitherWay::BACKWARD) {
if (!ends_) // keep existing interface if possible
ends_.reset(new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1)));
ends_.reset(
new Interface(std::bind(&PropagatingEitherWayPrivate::dropFailedEnds, this, std::placeholders::_1)));
} else {
ends_.reset();
}
@ -438,8 +426,7 @@ void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
initInterface(PropagatingEitherWay::Direction(dir));
}
void PropagatingEitherWayPrivate::validateConnectivity() const
{
void PropagatingEitherWayPrivate::validateConnectivity() const {
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if (actual == UNKNOWN)
@ -451,16 +438,15 @@ void PropagatingEitherWayPrivate::validateConnectivity() const
if ((actual & READS_END) && !(actual & WRITES_PREV_END))
errors.push_back(*me(), "Cannot push backwards");
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY &&
(required & actual) != required)
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate " <<
(actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
if (required_interface_dirs_ == PropagatingEitherWay::BOTHWAY && (required & actual) != required)
ROS_WARN_STREAM_NAMED("PropagatingEitherWay", "Cannot propagate "
<< (actual & PROPAGATE_FORWARDS ? "backwards" : "forwards"));
if (errors) throw errors;
if (errors)
throw errors;
}
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const
{
InterfaceFlags PropagatingEitherWayPrivate::requiredInterface() const {
InterfaceFlags f;
if (required_interface_dirs_ & PropagatingEitherWay::FORWARD)
f |= PROPAGATE_FORWARDS;
@ -480,31 +466,29 @@ void PropagatingEitherWayPrivate::dropFailedEnds(Interface::iterator state) {
ends_->remove(state);
}
inline bool PropagatingEitherWayPrivate::hasStartState() const{
inline bool PropagatingEitherWayPrivate::hasStartState() const {
return starts_ && !starts_->empty();
}
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState(){
const InterfaceState& PropagatingEitherWayPrivate::fetchStartState() {
assert(hasStartState());
return *starts_->remove(starts_->begin()).front();
}
inline bool PropagatingEitherWayPrivate::hasEndState() const{
inline bool PropagatingEitherWayPrivate::hasEndState() const {
return ends_ && !ends_->empty();
}
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
const InterfaceState& PropagatingEitherWayPrivate::fetchEndState() {
assert(hasEndState());
return *ends_->remove(ends_->begin()).front();
}
bool PropagatingEitherWayPrivate::canCompute() const
{
bool PropagatingEitherWayPrivate::canCompute() const {
return hasStartState() || hasEndState();
}
void PropagatingEitherWayPrivate::compute()
{
void PropagatingEitherWayPrivate::compute() {
PropagatingEitherWay* me = static_cast<PropagatingEitherWay*>(me_);
if (hasStartState()) {
@ -521,29 +505,22 @@ void PropagatingEitherWayPrivate::compute()
}
}
PropagatingEitherWay::PropagatingEitherWay(const std::string& name)
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, AUTO, name)) {}
PropagatingEitherWay::PropagatingEitherWay(const std::string &name)
: PropagatingEitherWay(new PropagatingEitherWayPrivate(this, AUTO, name))
{
}
PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate* impl) : ComputeBase(impl) {}
PropagatingEitherWay::PropagatingEitherWay(PropagatingEitherWayPrivate *impl)
: ComputeBase(impl)
{
}
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir)
{
void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir) {
auto impl = pimpl();
if (impl->required_interface_dirs_ == dir) return;
if (impl->required_interface_dirs_ == dir)
return;
if (impl->prevEnds() || impl->nextStarts())
throw std::runtime_error("Cannot change direction after being connected");
impl->required_interface_dirs_ = dir;
impl->initInterface(impl->required_interface_dirs_);
}
void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_model) {
Stage::init(robot_model);
auto impl = pimpl();
@ -557,61 +534,44 @@ void PropagatingEitherWay::init(const moveit::core::RobotModelConstPtr& robot_mo
// otherwise the interface is already fixed and well-defined
}
void PropagatingEitherWay::sendForward(const InterfaceState& from,
InterfaceState&& to,
SubTrajectory&& t) {
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
pimpl()->sendForward(from, std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
}
void PropagatingEitherWay::sendBackward(InterfaceState&& from,
const InterfaceState& to,
SubTrajectory&& t) {
void PropagatingEitherWay::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) {
pimpl()->sendBackward(std::move(from), to, std::make_shared<SubTrajectory>(std::move(t)));
}
PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward *me, const std::string &name)
: PropagatingEitherWayPrivate(me, PropagatingEitherWay::FORWARD, name)
{
PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward* me, const std::string& name)
: PropagatingEitherWayPrivate(me, PropagatingEitherWay::FORWARD, name) {
// indicate, that we don't accept new states from ends_ interface
ends_.reset();
}
PropagatingForward::PropagatingForward(const std::string& name)
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name))
{}
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name)) {}
void PropagatingForward::computeBackward(const InterfaceState &to)
{
assert(false); // This should never be called
void PropagatingForward::computeBackward(const InterfaceState& to) {
assert(false); // This should never be called
}
PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward *me, const std::string &name)
: PropagatingEitherWayPrivate(me, PropagatingEitherWay::BACKWARD, name)
{
PropagatingBackwardPrivate::PropagatingBackwardPrivate(PropagatingBackward* me, const std::string& name)
: PropagatingEitherWayPrivate(me, PropagatingEitherWay::BACKWARD, name) {
// indicate, that we don't accept new states from starts_ interface
starts_.reset();
}
PropagatingBackward::PropagatingBackward(const std::string& name)
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name)) {}
PropagatingBackward::PropagatingBackward(const std::string &name)
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name))
{}
void PropagatingBackward::computeForward(const InterfaceState &from)
{
assert(false); // This should never be called
void PropagatingBackward::computeForward(const InterfaceState& from) {
assert(false); // This should never be called
}
GeneratorPrivate::GeneratorPrivate(Generator *me, const std::string &name)
: ComputeBasePrivate(me, name)
{}
GeneratorPrivate::GeneratorPrivate(Generator* me, const std::string& name) : ComputeBasePrivate(me, name) {}
InterfaceFlags GeneratorPrivate::requiredInterface() const {
return InterfaceFlags({WRITES_NEXT_START, WRITES_PREV_END});
return InterfaceFlags({ WRITES_NEXT_START, WRITES_PREV_END });
}
bool GeneratorPrivate::canCompute() const {
@ -622,32 +582,22 @@ void GeneratorPrivate::compute() {
static_cast<Generator*>(me_)->compute();
}
Generator::Generator(GeneratorPrivate* impl) : ComputeBase(impl) {}
Generator::Generator(const std::string& name) : Generator(new GeneratorPrivate(this, name)) {}
Generator::Generator(GeneratorPrivate* impl)
: ComputeBase(impl)
{}
Generator::Generator(const std::string &name)
: Generator(new GeneratorPrivate(this, name))
{}
void Generator::spawn(InterfaceState&& state, SubTrajectory&& t)
{
void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) {
pimpl()->spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
}
MonitoringGeneratorPrivate::MonitoringGeneratorPrivate(MonitoringGenerator* me, const std::string& name)
: GeneratorPrivate(me, name), monitored_(nullptr), registered_(false) {}
MonitoringGeneratorPrivate::MonitoringGeneratorPrivate(MonitoringGenerator *me, const std::string &name)
: GeneratorPrivate(me, name), monitored_(nullptr), registered_(false)
{}
MonitoringGenerator::MonitoringGenerator(const std::string &name, Stage* monitored)
: Generator(new MonitoringGeneratorPrivate(this, name))
{
MonitoringGenerator::MonitoringGenerator(const std::string& name, Stage* monitored)
: Generator(new MonitoringGeneratorPrivate(this, name)) {
setMonitoredStage(monitored);
}
void MonitoringGenerator::setMonitoredStage(Stage* monitored)
{
void MonitoringGenerator::setMonitoredStage(Stage* monitored) {
auto impl = pimpl();
if (impl->monitored_ == monitored)
return;
@ -660,32 +610,30 @@ void MonitoringGenerator::setMonitoredStage(Stage* monitored)
impl->monitored_ = monitored;
}
void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void MonitoringGenerator::init(const moveit::core::RobotModelConstPtr& robot_model) {
Generator::init(robot_model);
auto impl = pimpl();
if (!impl->monitored_)
throw InitStageException(*this, "no monitored stage defined");
if (!impl->registered_) { // register only once
impl->cb_ = impl->monitored_->addSolutionCallback(std::bind(&MonitoringGeneratorPrivate::solutionCB, impl, std::placeholders::_1));
impl->cb_ = impl->monitored_->addSolutionCallback(
std::bind(&MonitoringGeneratorPrivate::solutionCB, impl, std::placeholders::_1));
impl->registered_ = true;
}
}
void MonitoringGeneratorPrivate::solutionCB(const SolutionBase &s)
{
void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) {
// forward only successful solutions to monitor
if(!s.isFailure())
if (!s.isFailure())
static_cast<MonitoringGenerator*>(me())->onNewSolution(s);
}
ConnectingPrivate::ConnectingPrivate(Connecting *me, const std::string &name)
: ComputeBasePrivate(me, name)
{
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this, std::placeholders::_1, std::placeholders::_2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2)));
ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) {
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
}
InterfaceFlags ConnectingPrivate::requiredInterface() const {
@ -693,19 +641,18 @@ InterfaceFlags ConnectingPrivate::requiredInterface() const {
}
template <>
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::BACKWARD>
(Interface::const_iterator first, Interface::const_iterator second) {
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::BACKWARD>(Interface::const_iterator first,
Interface::const_iterator second) {
return std::make_pair(first, second);
}
template <>
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>
(Interface::const_iterator first, Interface::const_iterator second) {
ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(Interface::const_iterator first,
Interface::const_iterator second) {
return std::make_pair(second, first);
}
template <Interface::Direction other>
void ConnectingPrivate::newState(Interface::iterator it, bool updated)
{
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
// TODO: only consider interface states with priority depth > threshold
if (!std::isfinite(it->priority().cost())) {
// remove pending pairs, if cost updated to infinity
@ -716,7 +663,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated)
if (updated) {
// many pairs might be affected: sort
pending.sort();
} else { // new state: insert all pairs with other interface
} else { // new state: insert all pairs with other interface
InterfacePtr other_interface = pullInterface(other);
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (!std::isfinite(oit->priority().cost()))
@ -727,7 +674,7 @@ void ConnectingPrivate::newState(Interface::iterator it, bool updated)
}
}
bool ConnectingPrivate::canCompute() const{
bool ConnectingPrivate::canCompute() const {
return !pending.empty();
}
@ -738,21 +685,15 @@ void ConnectingPrivate::compute() {
static_cast<Connecting*>(me_)->compute(from, to);
}
Connecting::Connecting(const std::string& name) : ComputeBase(new ConnectingPrivate(this, name)) {}
Connecting::Connecting(const std::string &name)
: ComputeBase(new ConnectingPrivate(this, name))
{
}
void Connecting::reset()
{
void Connecting::reset() {
pimpl()->pending.clear();
ComputeBase::reset();
}
/// compare consistency of planning scenes
bool Connecting::compatible(const InterfaceState& from_state, const InterfaceState& to_state) const
{
bool Connecting::compatible(const InterfaceState& from_state, const InterfaceState& to_state) const {
const planning_scene::PlanningSceneConstPtr& from = from_state.scene();
const planning_scene::PlanningSceneConstPtr& to = to_state.scene();
@ -775,8 +716,9 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
}
for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)){
to_it = to_object->shape_poses_.cbegin();
from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_pair.first);
return false; // transforms do not match
}
@ -794,19 +736,18 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
for (const moveit::core::AttachedBody* from_object : from_attached) {
auto it = std::find_if(to_attached.cbegin(), to_attached.cend(),
[from_object](const moveit::core::AttachedBody* object) {
return object->getName() == from_object->getName();
});
[from_object](const moveit::core::AttachedBody* object) {
return object->getName() == from_object->getName();
});
if (it == to_attached.cend()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object->getName());
return false;
}
const moveit::core::AttachedBody* to_object = *it;
if (from_object->getAttachedLink() != to_object->getAttachedLink()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different attach links: "
<< from_object->getName() << " attached to "
<< from_object->getAttachedLinkName() << " / "
<< to_object->getAttachedLinkName());
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different attach links: " << from_object->getName()
<< " attached to " << from_object->getAttachedLinkName() << " / "
<< to_object->getAttachedLinkName());
return false; // links not matching
}
if (from_object->getFixedTransforms().size() != to_object->getFixedTransforms().size()) {
@ -814,8 +755,9 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
return false; // shapes not matching
}
for (auto from_it = from_object->getFixedTransforms().cbegin(), from_end = from_object->getFixedTransforms().cend(),
to_it = to_object->getFixedTransforms().cbegin(); from_it != from_end; ++from_it, ++to_it)
for (auto from_it = from_object->getFixedTransforms().cbegin(),
from_end = from_object->getFixedTransforms().cend(), to_it = to_object->getFixedTransforms().cbegin();
from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object->getName());
return false; // transforms do not match
@ -832,5 +774,5 @@ std::ostream& operator<<(std::ostream& os, const Stage& stage) {
os << *stage.pimpl();
return os;
}
} }
}
}

View File

@ -49,34 +49,33 @@
#include <iterator>
#include <ros/console.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
: WrapperBase(name, std::move(child))
{
ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("group", "name of active group (derived from eef if not provided)");
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)");
p.declare<uint32_t>("max_ik_solutions", 1);
p.declare<bool>("ignore_collisions", false);
p.declare<double>("min_solution_distance", 0.1, "minimum distance between seperate IK solutions for the same target");
p.declare<double>("min_solution_distance", 0.1,
"minimum distance between seperate IK solutions for the same target");
// ik_frame and target_pose are read from the interface
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
p.declare<geometry_msgs::PoseStamped>("target_pose", "goal pose for ik frame");
}
void ComputeIK::setIKFrame(const Eigen::Isometry3d &pose, const std::string &link)
{
void ComputeIK::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
setIKFrame(pose_msg);
}
void ComputeIK::setTargetPose(const Eigen::Isometry3d &pose, const std::string &frame)
{
void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = frame;
tf::poseEigenToMsg(pose, pose_msg.pose);
@ -90,10 +89,9 @@ namespace {
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
// TODO: move into MoveIt! core, lift active_components_only_ from fcl to common interface
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d pose, const robot_model::LinkModel* link,
collision_detection::CollisionResult* collision_result = nullptr)
{
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d pose,
const robot_model::LinkModel* link,
collision_detection::CollisionResult* collision_result = nullptr) {
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
// consider all rigidly connected parent links as well
@ -130,9 +128,8 @@ bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene,
return res.collision;
}
std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap &contacts,
const std::string& separator)
{
std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap& contacts,
const std::string& separator) {
std::string result;
for (const auto& contact : contacts) {
if (!result.empty())
@ -143,12 +140,12 @@ std::string listCollisionPairs(const collision_detection::CollisionResult::Conta
}
bool validateEEF(const PropertyMap& props, const moveit::core::RobotModelConstPtr& robot_model,
const moveit::core::JointModelGroup*& jmg, std::string* msg)
{
const moveit::core::JointModelGroup*& jmg, std::string* msg) {
try {
const std::string& eef = props.get<std::string>("eef");
if (!robot_model->hasEndEffector(eef)) {
if (msg) *msg = "Unknown end effector: " + eef;
if (msg)
*msg = "Unknown end effector: " + eef;
return false;
} else
jmg = robot_model->getEndEffector(eef);
@ -157,13 +154,13 @@ bool validateEEF(const PropertyMap& props, const moveit::core::RobotModelConstPt
return true;
}
bool validateGroup(const PropertyMap& props, const moveit::core::RobotModelConstPtr& robot_model,
const moveit::core::JointModelGroup* eef_jmg,
const moveit::core::JointModelGroup*& jmg, std::string* msg)
{
const moveit::core::JointModelGroup* eef_jmg, const moveit::core::JointModelGroup*& jmg,
std::string* msg) {
try {
const std::string& group = props.get<std::string>("group");
if (!(jmg = robot_model->getJointModelGroup(group))) {
if (msg) *msg = "Unknown group: " + group;
if (msg)
*msg = "Unknown group: " + group;
return false;
}
} catch (const Property::undefined&) {
@ -176,18 +173,20 @@ bool validateGroup(const PropertyMap& props, const moveit::core::RobotModelConst
return true;
}
} // anonymous namespace
} // anonymous namespace
void ComputeIK::reset()
{
void ComputeIK::reset() {
upstream_solutions_.clear();
WrapperBase::reset();
}
void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model) {
InitStageException errors;
try { WrapperBase::init(robot_model); } catch (InitStageException &e) { errors.append(e); }
try {
WrapperBase::init(robot_model);
} catch (InitStageException& e) {
errors.append(e);
}
// all properties can be derived from the interface state
// however, if they are defined already now, we validate here
@ -201,13 +200,13 @@ void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model)
if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg))
errors.push_back(*this, msg);
if (errors) throw errors;
if (errors)
throw errors;
}
void ComputeIK::onNewSolution(const SolutionBase &s)
{
void ComputeIK::onNewSolution(const SolutionBase& s) {
assert(s.start() && s.end());
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
// It's safe to store a pointer to the solution, as the generating stage stores it
upstream_solutions_.push(&s);
@ -217,12 +216,11 @@ bool ComputeIK::canCompute() const {
return !upstream_solutions_.empty() || WrapperBase::canCompute();
}
void ComputeIK::compute()
{
if(WrapperBase::canCompute())
void ComputeIK::compute() {
if (WrapperBase::canCompute())
WrapperBase::compute();
if(upstream_solutions_.empty())
if (upstream_solutions_.empty())
return;
const SolutionBase& s = *upstream_solutions_.pop();
@ -264,7 +262,8 @@ void ComputeIK::compute()
tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) {
if (!sandbox_scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
ROS_WARN_STREAM_NAMED("ComputeIK",
"Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
return;
}
// transform target_pose w.r.t. planning frame
@ -275,10 +274,10 @@ void ComputeIK::compute()
const robot_model::LinkModel* link = nullptr;
geometry_msgs::PoseStamped ik_pose_msg;
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
if (value.empty()) { // property undefined
// determine IK link from eef/group
if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second)
: jmg->getOnlyOneEndEffectorTip())) {
if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) :
jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
return;
}
@ -291,8 +290,8 @@ void ComputeIK::compute()
if (robot_model->hasLinkModel(ik_pose_msg.header.frame_id)) {
link = robot_model->getLinkModel(ik_pose_msg.header.frame_id);
} else {
const robot_state::AttachedBody* attached
= sandbox_scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id);
const robot_state::AttachedBody* attached =
sandbox_scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id);
if (!attached) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
return;
@ -328,7 +327,8 @@ void ComputeIK::compute()
failure_markers.push_back(marker);
};
const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
->getParentJointModel()->getDescendantLinkModels();
->getParentJointModel()
->getDescendantLinkModels();
if (colliding) {
SubTrajectory solution;
generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
@ -341,10 +341,9 @@ void ComputeIK::compute()
} else
generateVisualMarkers(sandbox_state, appender, links_to_visualize);
// determine joint values of robot pose to compare IK solution with for costs
std::vector<double> compare_pose;
const std::string &compare_pose_name = props.get<std::string>("default_pose");
const std::string& compare_pose_name = props.get<std::string>("default_pose");
if (!compare_pose_name.empty()) {
robot_state::RobotState compare_state(robot_model);
compare_state.setToDefaultValues(jmg, compare_pose_name);
@ -355,11 +354,11 @@ void ComputeIK::compute()
double min_solution_distance = props.get<double>("min_solution_distance");
IKSolutions ik_solutions;
auto isValid = [sandbox_scene, ignore_collisions, min_solution_distance, &ik_solutions]
(robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const double* joint_positions) {
for (const auto& sol : ik_solutions){
auto isValid = [sandbox_scene, ignore_collisions, min_solution_distance, &ik_solutions](
robot_state::RobotState* state, const robot_model::JointModelGroup* jmg, const double* joint_positions) {
for (const auto& sol : ik_solutions) {
if (jmg->distance(joint_positions, sol.data()) < min_solution_distance)
return false; // too close to already found solution
return false; // too close to already found solution
}
state->setJointGroupPositions(jmg, joint_positions);
ik_solutions.emplace_back();
@ -374,9 +373,9 @@ void ComputeIK::compute()
double remaining_time = timeout();
auto start_time = std::chrono::steady_clock::now();
while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) {
if(tried_current_state_as_seed)
if (tried_current_state_as_seed)
sandbox_state.setToRandomPositions(jmg);
tried_current_state_as_seed= true;
tried_current_state_as_seed = true;
size_t previous = ik_solutions.size();
bool succeeded = sandbox_state.setFromIK(jmg, target_pose, link->getName(), remaining_time, isValid);
@ -396,10 +395,10 @@ void ComputeIK::compute()
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
if (succeeded && i+1 == ik_solutions.size())
if (succeeded && i + 1 == ik_solutions.size())
// compute cost as distance to compare_pose
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
else // found an IK solution, but this was not valid
else // found an IK solution, but this was not valid
solution.markAsFailure();
// set scene's robot state
@ -432,5 +431,6 @@ void ComputeIK::compute()
spawn(InterfaceState(scene), std::move(solution));
}
}
} } }
}
}
}

View File

@ -40,12 +40,11 @@
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
Connect::Connect(const std::string& name, const GroupPlannerVector& planners)
: Connecting(name)
, planner_(planners)
{
Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) {
setTimeout(1.0);
auto& p = properties();
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
@ -53,16 +52,14 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners)
"constraints to maintain during trajectory");
}
void Connect::reset()
{
void Connect::reset() {
Connecting::reset();
merged_jmg_.reset();
subsolutions_.clear();
states_.clear();
}
void Connect::init(const core::RobotModelConstPtr& robot_model)
{
void Connect::init(const core::RobotModelConstPtr& robot_model) {
Connecting::init(robot_model);
InitStageException errors;
@ -92,7 +89,8 @@ void Connect::init(const core::RobotModelConstPtr& robot_model)
std::vector<const moveit::core::JointModel*> duplicates;
std::string names;
if (findDuplicates(groups, merged_jmg_->getJointModels(), duplicates, names)) {
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": overlapping joint groups: " << names << ". Disabling merging.");
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": overlapping joint groups: " << names
<< ". Disabling merging.");
merged_jmg_.reset(); // fallback to serial connect
}
}
@ -102,8 +100,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model)
throw errors;
}
bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& to_state) const
{
bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& to_state) const {
if (!Connecting::compatible(from_state, to_state))
return false;
@ -113,8 +110,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
// compose set of joint names we plan for
std::set<std::string> planned_joint_names;
for (const GroupPlannerVector::value_type& pair : planner_) {
const moveit::core::JointModelGroup *jmg = from.getJointModelGroup(pair.first);
const auto &names = jmg->getJointModelNames();
const moveit::core::JointModelGroup* jmg = from.getJointModelGroup(pair.first);
const auto& names = jmg->getJointModelNames();
planned_joint_names.insert(names.begin(), names.end());
}
// all active joints that we don't plan for should match
@ -123,19 +120,18 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
continue; // ignore joints we plan for
const unsigned int num = jm->getVariableCount();
Eigen::Map<const Eigen::VectorXd> positions_from (from.getJointPositions(jm), num);
Eigen::Map<const Eigen::VectorXd> positions_to (to.getJointPositions(jm), num);
Eigen::Map<const Eigen::VectorXd> positions_from(from.getJointPositions(jm), num);
Eigen::Map<const Eigen::VectorXd> positions_to(to.getJointPositions(jm), num);
if (!(positions_from - positions_to).isZero(1e-4)) {
ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName()
<< ": [" << positions_from.transpose()
<< "] != [" << positions_to.transpose() << "]");
ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose()
<< "] != [" << positions_to.transpose() << "]");
return false;
}
}
return true;
}
void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
const auto& props = properties();
double timeout = this->timeout();
MergeMode mode = props.get<MergeMode>("merge_mode");
@ -153,7 +149,7 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff();
const moveit::core::JointModelGroup *jmg = final_goal_state.getJointModelGroup(pair.first);
const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first);
final_goal_state.copyJointGroupPositions(jmg, positions);
robot_state::RobotState& goal_state = end->getCurrentStateNonConst();
goal_state.setJointGroupPositions(jmg, positions);
@ -181,10 +177,10 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
connect(from, to, solution);
}
SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const InterfaceState &from, const InterfaceState &to)
{
SolutionSequencePtr
Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const InterfaceState& from, const InterfaceState& to) {
assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
auto scene_it = intermediate_scenes.begin();
planning_scene::PlanningSceneConstPtr start_ps = *scene_it;
@ -192,13 +188,13 @@ SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::
// calculate cost
double cost = 0;
for (const auto& trajectory: sub_trajectories) {
for (const double& distance: trajectory->getWayPointDurations())
for (const auto& trajectory : sub_trajectories) {
for (const double& distance : trajectory->getWayPointDurations())
cost += distance;
}
SolutionSequence::container_type sub_solutions;
for (const auto &sub : sub_trajectories) {
for (const auto& sub : sub_trajectories) {
planning_scene::PlanningSceneConstPtr end_ps = *++scene_it;
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
@ -224,12 +220,11 @@ SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::
SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const moveit::core::RobotState& state)
{
const moveit::core::RobotState& state) {
// calculate cost
double cost = 0;
for (const auto& trajectory: sub_trajectories) {
for (const double& distance: trajectory->getWayPointDurations())
for (const auto& trajectory : sub_trajectories) {
for (const double& distance : trajectory->getWayPointDurations())
cost += distance;
}
@ -244,10 +239,12 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
return SubTrajectoryPtr();
// check merged trajectory for collisions
if (!intermediate_scenes.front()->isPathValid(*trajectory, properties().get<moveit_msgs::Constraints>("path_constraints")))
if (!intermediate_scenes.front()->isPathValid(*trajectory,
properties().get<moveit_msgs::Constraints>("path_constraints")))
return SubTrajectoryPtr();
return std::make_shared<SubTrajectory>(trajectory, cost);
}
} } }
}
}
}

View File

@ -43,26 +43,24 @@
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <ros/ros.h>
namespace moveit {
namespace task_constructor {
namespace stages {
namespace moveit { namespace task_constructor { namespace stages {
CurrentState::CurrentState(const std::string &name)
: Generator(name)
{
auto &p = properties();
CurrentState::CurrentState(const std::string& name) : Generator(name) {
auto& p = properties();
Property& timeout = p.property("timeout");
timeout.setDescription("max time to wait for get_planning_scene service");
timeout.setValue(-1.0);
}
void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void CurrentState::init(const moveit::core::RobotModelConstPtr& robot_model) {
Generator::init(robot_model);
robot_model_ = robot_model;
scene_.reset();
}
bool CurrentState::canCompute() const{
bool CurrentState::canCompute() const {
return !scene_;
}
@ -78,16 +76,14 @@ void CurrentState::compute() {
moveit_msgs::GetPlanningScene::Response res;
req.components.components =
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE
| moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES
| moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY
| moveit_msgs::PlanningSceneComponents::OCTOMAP
| moveit_msgs::PlanningSceneComponents::TRANSFORMS
| moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX
| moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING
| moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS | moveit_msgs::PlanningSceneComponents::ROBOT_STATE |
moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS |
moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES |
moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | moveit_msgs::PlanningSceneComponents::OCTOMAP |
moveit_msgs::PlanningSceneComponents::TRANSFORMS |
moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX |
moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING |
moveit_msgs::PlanningSceneComponents::OBJECT_COLORS;
if (client.call(req, res)) {
scene_->setPlanningSceneMsg(res.scene);
@ -97,8 +93,9 @@ void CurrentState::compute() {
}
ROS_WARN("failed to acquire current PlanningScene");
}
} } }
}
}
}
/// register plugin
#include <pluginlib/class_list_macros.h>

View File

@ -48,38 +48,32 @@
namespace vm = visualization_msgs;
namespace cd = collision_detection;
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
FixCollisionObjects::FixCollisionObjects(const std::string &name)
: PropagatingEitherWay(name)
{
FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) {
auto& p = properties();
p.declare<double>("max_penetration", "maximally corrected penetration depth");
p.declare<geometry_msgs::Vector3>("direction", "direction vector to use for corrections");
}
void FixCollisionObjects::computeForward(const InterfaceState &from)
{
void FixCollisionObjects::computeForward(const InterfaceState& from) {
planning_scene::PlanningScenePtr to = from.scene()->diff();
sendForward(from, InterfaceState(to), fixCollisions(*to));
}
void FixCollisionObjects::computeBackward(const InterfaceState &to)
{
void FixCollisionObjects::computeBackward(const InterfaceState& to) {
planning_scene::PlanningScenePtr from = to.scene()->diff();
sendBackward(InterfaceState(from), to, fixCollisions(*from));
}
bool computeCorrection(const std::vector<cd::Contact>& contacts, Eigen::Vector3d& correction, double max_penetration)
{
bool computeCorrection(const std::vector<cd::Contact>& contacts, Eigen::Vector3d& correction, double max_penetration) {
correction.setZero();
for (const cd::Contact& c : contacts) {
if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT &&
c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) {
ROS_WARN_STREAM_NAMED("FixCollisionObjects", "Cannot fix collision between "
<< c.body_name_1 << " and " << c.body_name_2);
if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT && c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) {
ROS_WARN_STREAM_NAMED("FixCollisionObjects", "Cannot fix collision between " << c.body_name_1 << " and "
<< c.body_name_2);
return false;
}
if (c.body_type_1 == cd::BodyTypes::WORLD_OBJECT)
@ -94,8 +88,7 @@ bool computeCorrection(const std::vector<cd::Contact>& contacts, Eigen::Vector3d
return true;
}
SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene) const
{
SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene& scene) const {
SubTrajectory result;
const auto& props = properties();
double max_penetration = props.get<double>("max_penetration");
@ -110,7 +103,6 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &
req.verbose = false;
req.distance = false;
vm::Marker m;
m.header.frame_id = scene.getPlanningFrame();
m.ns = "collisions";
@ -119,8 +111,7 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &
while (!failure) {
res.clear();
scene.getCollisionWorld()->checkRobotCollision(req, res, *scene.getCollisionRobotUnpadded(),
scene.getCurrentState(),
scene.getAllowedCollisionMatrix());
scene.getCurrentState(), scene.getAllowedCollisionMatrix());
if (!res.collision)
return result;
@ -133,10 +124,11 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &
failure = depth > max_penetration;
// marker indicating correction
const cd::Contact &c = info.second.front();
const cd::Contact& c = info.second.front();
rviz_marker_tools::setColor(m.color, failure ? rviz_marker_tools::RED : rviz_marker_tools::GREEN);
tf::poseEigenToMsg(Eigen::Translation3d(c.pos) *
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), correction), m.pose);
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), correction),
m.pose);
rviz_marker_tools::makeArrow(m, depth, true);
result.markers().push_back(m);
if (failure)
@ -155,5 +147,6 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &
result.markAsFailure();
return result;
}
} } }
}
}
}

View File

@ -40,34 +40,31 @@
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
typedef std::vector<geometry_msgs::PoseStamped> PosesList;
FixedCartesianPoses::FixedCartesianPoses(const std::string& name)
: MonitoringGenerator(name)
{
FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) {
auto& p = properties();
p.declare<PosesList>("poses", PosesList(), "target poses to spawn");
}
void FixedCartesianPoses::addPose(const geometry_msgs::PoseStamped& pose)
{
void FixedCartesianPoses::addPose(const geometry_msgs::PoseStamped& pose) {
moveit::task_constructor::Property& poses = properties().property("poses");
if (!poses.defined())
poses.setValue(PosesList({pose}));
poses.setValue(PosesList({ pose }));
else
boost::any_cast<PosesList&>(poses.value()).push_back(pose);
}
void FixedCartesianPoses::reset()
{
void FixedCartesianPoses::reset() {
upstream_solutions_.clear();
MonitoringGenerator::reset();
}
void FixedCartesianPoses::onNewSolution(const SolutionBase& s)
{
void FixedCartesianPoses::onNewSolution(const SolutionBase& s) {
// It's safe to store a pointer to this solution, as the generating stage stores it
upstream_solutions_.push(&s);
}
@ -81,8 +78,7 @@ void FixedCartesianPoses::compute() {
return;
planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff();
for (geometry_msgs::PoseStamped pose : properties().get<PosesList>("poses"))
{
for (geometry_msgs::PoseStamped pose : properties().get<PosesList>("poses")) {
if (pose.header.frame_id.empty())
pose.header.frame_id = scene->getPlanningFrame();
else if (!scene->knowsFrameTransform(pose.header.frame_id)) {
@ -101,5 +97,6 @@ void FixedCartesianPoses::compute() {
spawn(std::move(state), std::move(trajectory));
}
}
} } }
}
}
}

View File

@ -37,24 +37,22 @@
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
FixedState::FixedState(const std::string &name)
: Generator(name)
{}
FixedState::FixedState(const std::string& name) : Generator(name) {}
void FixedState::setState(const planning_scene::PlanningScenePtr& scene)
{
void FixedState::setState(const planning_scene::PlanningScenePtr& scene) {
scene_ = scene;
}
void FixedState::reset()
{
void FixedState::reset() {
Generator::reset();
ran_ = false;
}
bool FixedState::canCompute() const{
bool FixedState::canCompute() const {
return !ran_ && scene_;
}
@ -62,5 +60,6 @@ void FixedState::compute() {
spawn(InterfaceState(scene_), 0.0);
ran_ = true;
}
} } }
}
}
}

View File

@ -44,11 +44,11 @@
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
GenerateGraspPose::GenerateGraspPose(const std::string& name)
: GeneratePose(name)
{
GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(name) {
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector");
p.declare<std::string>("object");
@ -58,11 +58,13 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name)
p.declare<boost::any>("grasp", "grasp posture");
}
void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model)
{
void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) {
InitStageException errors;
try { GeneratePose::init(robot_model); }
catch (InitStageException &e) { errors.append(e); }
try {
GeneratePose::init(robot_model);
} catch (InitStageException& e) {
errors.append(e);
}
const auto& props = properties();
@ -85,11 +87,11 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model)
errors.push_back(*this, "unknown end effector pose: " + name);
}
if (errors) throw errors;
if (errors)
throw errors;
}
void GenerateGraspPose::onNewSolution(const SolutionBase& s)
{
void GenerateGraspPose::onNewSolution(const SolutionBase& s) {
planning_scene::PlanningSceneConstPtr scene = s.end()->scene();
const auto& props = properties();
@ -120,14 +122,14 @@ void GenerateGraspPose::compute() {
const std::string& eef = props.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
robot_state::RobotState &robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg , props.get<std::string>("pregrasp"));
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg, props.get<std::string>("pregrasp"));
geometry_msgs::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = props.get<std::string>("object");
double current_angle_ = 0.0;
while (current_angle_ < 2.*M_PI && current_angle_ > -2.*M_PI) {
while (current_angle_ < 2. * M_PI && current_angle_ > -2. * M_PI) {
// rotate object pose about z-axis
Eigen::Isometry3d target_pose(Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()));
current_angle_ += props.get<double>("angle_delta");
@ -135,7 +137,7 @@ void GenerateGraspPose::compute() {
InterfaceState state(scene);
tf::poseEigenToMsg(target_pose, target_pose_msg.pose);
state.properties().set("target_pose", target_pose_msg);
props.exposeTo(state.properties(), {"pregrasp", "grasp"});
props.exposeTo(state.properties(), { "pregrasp", "grasp" });
SubTrajectory trajectory;
trajectory.setCost(0.0);
@ -147,5 +149,6 @@ void GenerateGraspPose::compute() {
spawn(std::move(state), std::move(trajectory));
}
}
} } }
}
}
}

View File

@ -45,18 +45,17 @@
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
GeneratePlacePose::GeneratePlacePose(const std::string& name)
: GeneratePose(name)
{
GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(name) {
auto& p = properties();
p.declare<std::string>("object");
p.declare<geometry_msgs::PoseStamped>("ik_frame");
}
void GeneratePlacePose::onNewSolution(const SolutionBase& s)
{
void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
planning_scene::PlanningSceneConstPtr scene = s.end()->scene();
const auto& props = properties();
@ -90,8 +89,7 @@ void GeneratePlacePose::compute() {
const moveit::core::RobotState& robot_state = scene->getCurrentState();
const auto& props = properties();
const moveit::core::AttachedBody* object
= robot_state.getAttachedBody(props.get<std::string>("object"));
const moveit::core::AttachedBody* object = robot_state.getAttachedBody(props.get<std::string>("object"));
// current object_pose w.r.t. planning frame
const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0];
@ -108,8 +106,8 @@ void GeneratePlacePose::compute() {
Eigen::Isometry3d object_to_ik = orig_object_pose.inverse() * ik_frame;
// spawn the nominal target object pose, considering flip about z and rotations about z-axis
auto spawner = [&s, &scene, &object_to_ik, this]
(const Eigen::Isometry3d& nominal, uint z_flips, uint z_rotations = 10) {
auto spawner = [&s, &scene, &object_to_ik, this](const Eigen::Isometry3d& nominal, uint z_flips,
uint z_rotations = 10) {
for (uint flip = 0; flip < z_flips; ++flip) {
// flip about object's x-axis
Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX());
@ -117,8 +115,8 @@ void GeneratePlacePose::compute() {
// rotate object at target pose about world's z-axis
Eigen::Vector3d pos = object.translation();
object.pretranslate(-pos)
.prerotate(Eigen::AngleAxisd(i * 2.*M_PI / z_rotations, Eigen::Vector3d::UnitZ()))
.pretranslate(pos);
.prerotate(Eigen::AngleAxisd(i * 2. * M_PI / z_rotations, Eigen::Vector3d::UnitZ()))
.pretranslate(pos);
// target ik_frame's pose w.r.t. planning frame
geometry_msgs::PoseStamped target_pose_msg;
@ -140,26 +138,27 @@ void GeneratePlacePose::compute() {
if (object->getShapes().size() == 1) {
switch (object->getShapes()[0]->type) {
case shapes::CYLINDER:
spawner(target_pose, 2);
return;
case shapes::CYLINDER:
spawner(target_pose, 2);
return;
case shapes::BOX: { // consider 180/90 degree rotations about z axis
const double *dims = static_cast<const shapes::Box&>(*object->getShapes()[0]).size;
spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2);
return;
}
case shapes::SPHERE: // keep original orientation and rotate about world's z
target_pose.linear() = orig_object_pose.linear();
spawner(target_pose, 1);
return;
default:
break;
case shapes::BOX: { // consider 180/90 degree rotations about z axis
const double* dims = static_cast<const shapes::Box&>(*object->getShapes()[0]).size;
spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2);
return;
}
case shapes::SPHERE: // keep original orientation and rotate about world's z
target_pose.linear() = orig_object_pose.linear();
spawner(target_pose, 1);
return;
default:
break;
}
}
// any other case: only try given target pose
spawner(target_pose, 1, 1);
}
} } }
}
}
}

View File

@ -40,23 +40,21 @@
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
GeneratePose::GeneratePose(const std::string& name)
: MonitoringGenerator(name)
{
GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) {
auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states");
}
void GeneratePose::reset()
{
void GeneratePose::reset() {
upstream_solutions_.clear();
MonitoringGenerator::reset();
}
void GeneratePose::onNewSolution(const SolutionBase& s)
{
void GeneratePose::onNewSolution(const SolutionBase& s) {
// It's safe to store a pointer to this solution, as the generating stage stores it
upstream_solutions_.push(&s);
}
@ -88,5 +86,6 @@ void GeneratePose::compute() {
spawn(std::move(state), std::move(trajectory));
}
} } }
}
}
}

View File

@ -40,63 +40,58 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
ModifyPlanningScene::ModifyPlanningScene(const std::string &name)
: PropagatingEitherWay(name)
{}
ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {}
void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach)
{
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;
Names& o = it_inserted.first->second.first;
o.insert(o.end(), objects.begin(), objects.end());
}
void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) {
collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, allow}));
collision_matrix_edits_.push_back(CollisionMatrixPairs({ first, second, allow }));
}
void ModifyPlanningScene::allowCollisions(const std::string &first, const moveit::core::JointModelGroup &jmg, bool allow)
{
void ModifyPlanningScene::allowCollisions(const std::string& first, const moveit::core::JointModelGroup& jmg,
bool allow) {
const auto& links = jmg.getLinkModelNamesWithCollisionGeometry();
if (!links.empty())
allowCollisions(Names({first}), links, allow);
allowCollisions(Names({ first }), links, allow);
}
void ModifyPlanningScene::computeForward(const InterfaceState &from){
void ModifyPlanningScene::computeForward(const InterfaceState& from) {
sendForward(from, apply(from, false), SubTrajectory());
}
void ModifyPlanningScene::computeBackward(const InterfaceState &to)
{
void ModifyPlanningScene::computeBackward(const InterfaceState& to) {
sendBackward(apply(to, true), to, SubTrajectory());
}
void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene,
const std::pair<std::string, std::pair<Names, bool> >& pair,
bool invert)
{
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;
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::allowCollisions(planning_scene::PlanningScene &scene,
const CollisionMatrixPairs& pairs,
bool invert)
{
void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs,
bool invert) {
collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst();
bool allow = invert ? !pairs.allow : pairs.allow;
if (pairs.second.empty()) {
for (const auto &name : pairs.first)
for (const auto& name : pairs.first)
acm.setEntry(name, allow);
} else
acm.setEntry(pairs.first, pairs.second, allow);
@ -104,17 +99,16 @@ void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene &scene,
// invert indicates, whether to detach instead of attach (and vice versa)
// as well as to forbid instead of allow collision (and vice versa)
InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert)
{
InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert) {
planning_scene::PlanningScenePtr scene = from.scene()->diff();
InterfaceState result(scene);
// attach/detach objects
for (const auto &pair : attach_objects_)
for (const auto& pair : attach_objects_)
attachObjects(*scene, pair, invert);
// allow/forbid collisions
for (const auto &pairs : collision_matrix_edits_)
for (const auto& pairs : collision_matrix_edits_)
allowCollisions(*scene, pairs, invert);
if (callback_)
@ -122,5 +116,6 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
return result;
}
} } }
}
}
}

View File

@ -41,12 +41,12 @@
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name)
, planner_(planner)
{
: PropagatingEitherWay(name), planner_(planner) {
auto& p = properties();
p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("group", "name of planning group");
@ -63,23 +63,20 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
"constraints to maintain during trajectory");
}
void MoveRelative::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link)
{
void MoveRelative::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
setIKFrame(pose_msg);
}
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) {
PropagatingEitherWay::init(robot_model);
planner_->init(robot_model);
}
static bool getJointStateFromOffset(const boost::any& direction,
const moveit::core::JointModelGroup* jmg,
moveit::core::RobotState& robot_state) {
static bool getJointStateFromOffset(const boost::any& direction, const moveit::core::JointModelGroup* jmg,
moveit::core::RobotState& robot_state) {
try {
const auto& accepted = jmg->getJointModelNames();
const auto& joints = boost::any_cast<std::map<std::string, double>>(direction);
@ -87,7 +84,8 @@ static bool getJointStateFromOffset(const boost::any& direction,
int index = robot_state.getRobotModel()->getVariableIndex(j.first);
auto jm = robot_state.getRobotModel()->getJointModel(index);
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end())
throw std::runtime_error("Cannot plan for joint '" + j.first + "' that is not part of group '" + jmg->getName() + "'");
throw std::runtime_error("Cannot plan for joint '" + j.first + "' that is not part of group '" +
jmg->getName() + "'");
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
robot_state.enforceBounds(jm);
@ -101,8 +99,8 @@ static bool getJointStateFromOffset(const boost::any& direction,
return false;
}
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
SubTrajectory &solution, Direction dir) {
bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene,
SubTrajectory& solution, Direction dir) {
scene = state.scene()->diff();
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
assert(robot_model);
@ -136,7 +134,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
geometry_msgs::PoseStamped ik_pose_msg;
const moveit::core::LinkModel* link;
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
if (value.empty()) { // property undefined
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("MoveRelative", "Failed to derive IK target link");
@ -158,9 +156,10 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
double linear_norm = 0.0, angular_norm = 0.0;
Eigen::Isometry3d target_eigen;
Eigen::Isometry3d link_pose = scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
Eigen::Isometry3d link_pose =
scene->getCurrentState().getGlobalLinkTransform(link); // take a copy here, pose will change on success
try { // try to extract Twist
try { // try to extract Twist
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(direction);
const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
tf::vectorMsgToEigen(target.twist.linear, linear);
@ -196,12 +195,14 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
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.linear() =
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
target_eigen.translation() += linear;
goto COMPUTE;
} catch (const boost::bad_any_cast&) { /* continue with Vector */ }
} catch (const boost::bad_any_cast&) { /* continue with Vector */
}
try { // try to extract Vector
try { // try to extract Vector
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(direction);
const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
tf::vectorMsgToEigen(target.vector, linear);
@ -226,7 +227,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
return false;
}
COMPUTE:
COMPUTE:
// transform target pose such that ik frame will reach there if link does
Eigen::Isometry3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
@ -253,7 +254,7 @@ COMPUTE:
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
solution.setComment(msg);
}
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
success = true;
}
@ -264,12 +265,11 @@ COMPUTE:
m.header.frame_id = scene->getPlanningFrame();
if (linear_norm > 1e-3) {
// +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder)
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN
: rviz_marker_tools::RED);
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) {
if (dir == BACKWARD) {
// flip arrow direction
quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());
// arrow tip at goal pose
@ -285,7 +285,8 @@ COMPUTE:
// store result
if (robot_trajectory) {
scene->setCurrentState(robot_trajectory->getLastWayPoint());
if (dir == BACKWARD) robot_trajectory->reverse();
if (dir == BACKWARD)
robot_trajectory->reverse();
solution.setTrajectory(robot_trajectory);
if (!success)
solution.markAsFailure();
@ -293,8 +294,7 @@ COMPUTE:
return true;
}
void MoveRelative::computeForward(const InterfaceState &from)
{
void MoveRelative::computeForward(const InterfaceState& from) {
planning_scene::PlanningScenePtr to;
SubTrajectory trajectory;
@ -304,8 +304,7 @@ void MoveRelative::computeForward(const InterfaceState &from)
silentFailure();
}
void MoveRelative::computeBackward(const InterfaceState &to)
{
void MoveRelative::computeBackward(const InterfaceState& to) {
planning_scene::PlanningScenePtr from;
SubTrajectory trajectory;
@ -314,5 +313,6 @@ void MoveRelative::computeBackward(const InterfaceState &to)
else
silentFailure();
}
} } }
}
}
}

View File

@ -42,12 +42,12 @@
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/conversions.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name)
, planner_(planner)
{
: PropagatingEitherWay(name), planner_(planner) {
auto& p = properties();
p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("group", "name of planning group");
@ -63,8 +63,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
"constraints to maintain during trajectory");
}
void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link)
{
void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
@ -76,8 +75,7 @@ void MoveTo::setGoal(const std::map<std::string, double>& joints) {
robot_state.joint_state.name.reserve(joints.size());
robot_state.joint_state.position.reserve(joints.size());
for (auto& joint : joints)
{
for (auto& joint : joints) {
robot_state.joint_state.name.push_back(joint.first);
robot_state.joint_state.position.push_back(joint.second);
}
@ -85,14 +83,12 @@ void MoveTo::setGoal(const std::map<std::string, double>& joints) {
setProperty("goal", robot_state);
}
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) {
PropagatingEitherWay::init(robot_model);
planner_->init(robot_model);
}
bool MoveTo::getJointStateGoal(const boost::any& goal,
const moveit::core::JointModelGroup* jmg,
bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::JointModelGroup* jmg,
moveit::core::RobotState& state) {
try {
// try named joint pose
@ -100,7 +96,8 @@ bool MoveTo::getJointStateGoal(const boost::any& goal,
if (!state.setToDefaultValues(jmg, named_joint_pose))
throw InitStageException(*this, "Unknown joint pose: " + named_joint_pose);
return true;
} catch (const boost::bad_any_cast&) {}
} catch (const boost::bad_any_cast&) {
}
try {
// try RobotState
@ -110,24 +107,24 @@ bool MoveTo::getJointStateGoal(const boost::any& goal,
// validate specified joints
const auto& accepted = jmg->getJointModelNames();
for (const auto &name : msg.joint_state.name)
for (const auto& name : msg.joint_state.name)
if (std::find(accepted.begin(), accepted.end(), name) == accepted.end())
throw InitStageException(*this, "Joint '" + name + "' is not part of group '" + jmg->getName() + "'");
for (const auto &name : msg.multi_dof_joint_state.joint_names)
for (const auto& name : msg.multi_dof_joint_state.joint_names)
if (std::find(accepted.begin(), accepted.end(), name) == accepted.end())
throw InitStageException(*this, "Joint '" + name + "' is not part of group '" + jmg->getName() + "'");
moveit::core::robotStateMsgToRobotState(msg, state, false);
return true;
} catch (const boost::bad_any_cast&) {}
} catch (const boost::bad_any_cast&) {
}
return false;
}
bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg,
const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d& target_eigen, decltype(std::declval<SolutionBase>().markers())& markers)
{
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
decltype(std::declval<SolutionBase>().markers()) & markers) {
try {
const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal);
tf::poseMsgToEigen(target.pose, target_eigen);
@ -148,9 +145,8 @@ bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStampe
}
bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d& target_eigen, decltype(std::declval<SolutionBase>().markers())&)
{
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
decltype(std::declval<SolutionBase>().markers()) &) {
try {
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
Eigen::Vector3d target_point;
@ -169,8 +165,8 @@ bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel*
return true;
}
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
SubTrajectory &solution, Direction dir) {
bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution,
Direction dir) {
scene = state.scene()->diff();
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
assert(robot_model);
@ -196,14 +192,14 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
if (getJointStateGoal(goal, jmg, scene->getCurrentStateNonConst())) {
// plan to joint-space target
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else { // Cartesian goal
} else { // Cartesian goal
const moveit::core::LinkModel* link;
Eigen::Isometry3d target_eigen;
// Cartesian targets require an IK reference frame
geometry_msgs::PoseStamped ik_pose_msg;
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
if (value.empty()) { // property undefined
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("MoveTo", "Failed to derive IK target link");
@ -236,14 +232,15 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
// set cost
double cost = 0;
for (const double& distance: robot_trajectory->getWayPointDurations()) {
for (const double& distance : robot_trajectory->getWayPointDurations()) {
cost += distance;
}
// store result
if (robot_trajectory) {
scene->setCurrentState(robot_trajectory->getLastWayPoint());
if (dir == BACKWARD) robot_trajectory->reverse();
if (dir == BACKWARD)
robot_trajectory->reverse();
solution.setTrajectory(robot_trajectory);
solution.setCost(cost);
if (!success)
@ -254,7 +251,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
// -1 TODO: move these as default implementation to PropagateEitherWay?
// Essentially, here compute() is a class-specific worker function
void MoveTo::computeForward(const InterfaceState &from){
void MoveTo::computeForward(const InterfaceState& from) {
planning_scene::PlanningScenePtr to;
SubTrajectory trajectory;
@ -264,8 +261,7 @@ void MoveTo::computeForward(const InterfaceState &from){
silentFailure();
}
void MoveTo::computeBackward(const InterfaceState &to)
{
void MoveTo::computeBackward(const InterfaceState& to) {
planning_scene::PlanningScenePtr from;
SubTrajectory trajectory;
@ -274,5 +270,6 @@ void MoveTo::computeBackward(const InterfaceState &to)
else
silentFailure();
}
} } }
}
}
}

View File

@ -7,11 +7,12 @@
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& name, bool forward)
: SerialContainer(name)
{
: SerialContainer(name) {
PropertyMap& p = properties();
p.declare<std::string>("object", "name of object to grasp");
p.declare<std::string>("eef", "end effector name");
@ -22,7 +23,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na
p.declare<std::string>("eef_parent_group", "JMG of eef's parent");
cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
auto init_ik_frame = [](const PropertyMap& other) -> boost::any {
geometry_msgs::PoseStamped pose;
@ -46,7 +47,7 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na
}
grasp_stage_ = grasp_stage.get();
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"});
grasp_stage->properties().configureInitFrom(Stage::PARENT, { "eef", "object" });
insert(std::move(grasp_stage), insertion_position);
{
@ -60,16 +61,16 @@ PickPlaceBase::PickPlaceBase(Stage::pointer&& grasp_stage, const std::string& na
}
}
void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
// inherit properties from parent
PropertyMap* p = &properties();
p->performInitFrom(Stage::PARENT, parent()->properties());
// init internal properties
const std::string &eef = p->get<std::string>("eef");
const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef);
if (!jmg) throw InitStageException(*this, "unknown end effector: " + eef);
const std::string& eef = p->get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
if (!jmg)
throw InitStageException(*this, "unknown end effector: " + eef);
p->set<std::string>("eef_group", jmg->getName());
p->set<std::string>("eef_parent_group", jmg->getEndEffectorParentGroup().first);
@ -78,26 +79,25 @@ void PickPlaceBase::init(const moveit::core::RobotModelConstPtr& robot_model)
SerialContainer::init(robot_model);
}
void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
{
void PickPlaceBase::setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance,
double max_distance) {
auto& p = approach_stage_->properties();
p.set("direction", motion);
p.set("min_distance", min_distance);
p.set("max_distance", max_distance);
}
void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance)
{
void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) {
auto& p = lift_stage_->properties();
p.set("direction", motion);
p.set("min_distance", min_distance);
p.set("max_distance", max_distance);
}
void PickPlaceBase::setLiftPlace(const std::map<std::string, double>& joints)
{
void PickPlaceBase::setLiftPlace(const std::map<std::string, double>& joints) {
auto& p = lift_stage_->properties();
p.set("joints", joints);
}
} } }
}
}
}

View File

@ -44,21 +44,25 @@
#include <functional>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
PredicateFilter::PredicateFilter(const std::string &name, Stage::pointer &&child)
: WrapperBase(name, std::move(child))
{
auto& p = properties();
PredicateFilter::PredicateFilter(const std::string& name, Stage::pointer&& child)
: WrapperBase(name, std::move(child)) {
auto& p = properties();
p.declare<Predicate>("predicate", "predicate to filter wrapped solutions");
p.declare<bool>("ignore_filter", false);
}
void PredicateFilter::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void PredicateFilter::init(const moveit::core::RobotModelConstPtr& robot_model) {
InitStageException errors;
try { WrapperBase::init(robot_model); } catch (InitStageException &e) { errors.append(e); }
try {
WrapperBase::init(robot_model);
} catch (InitStageException& e) {
errors.append(e);
}
const auto& props = properties();
@ -69,11 +73,11 @@ void PredicateFilter::init(const moveit::core::RobotModelConstPtr& robot_model)
errors.append(e);
}
if (errors) throw errors;
if (errors)
throw errors;
}
void PredicateFilter::onNewSolution(const SolutionBase &s)
{
void PredicateFilter::onNewSolution(const SolutionBase& s) {
const auto& props = properties();
std::string comment = s.comment();
@ -84,5 +88,6 @@ void PredicateFilter::onNewSolution(const SolutionBase &s)
liftSolution(s, cost, comment);
}
} } }
}
}
}

View File

@ -46,19 +46,18 @@
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace stages {
namespace moveit {
namespace task_constructor {
namespace stages {
SimpleGraspBase::SimpleGraspBase(const std::string& name)
: SerialContainer(name)
{
SimpleGraspBase::SimpleGraspBase(const std::string& name) : SerialContainer(name) {
PropertyMap& p = properties();
p.declare<std::string>("eef", "end-effector to grasp with");
p.declare<std::string>("object", "object to grasp");
}
// TODO: Use AttachedBody's detach_posture_ to store the inverse grasping trajectory to re-open the gripper.
void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward)
{
void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward) {
// properties provided by the grasp generator via its Interface or its PropertyMap
const std::set<std::string>& grasp_prop_names = { "object", "eef", "pregrasp", "grasp" };
@ -76,9 +75,9 @@ void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward)
PropertyMap& p = ik->properties();
p.declare<std::string>("object");
p.configureInitFrom(Stage::INTERFACE, {"target_pose"}); // derived from child's solution
p.configureInitFrom(Stage::PARENT, {"max_ik_solutions", "timeout", "object"}); // derived from parent
p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, {"eef", "ik_frame"}); // derive from both
p.configureInitFrom(Stage::INTERFACE, { "target_pose" }); // derived from child's solution
p.configureInitFrom(Stage::PARENT, { "max_ik_solutions", "timeout", "object" }); // derived from parent
p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, { "eef", "ik_frame" }); // derive from both
p.exposeTo(properties(), { "max_ik_solutions", "timeout", "ik_frame" });
insert(std::unique_ptr<ComputeIK>(ik), 0); // ComputeIK always goes upfront
}
@ -91,12 +90,12 @@ void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward)
p.declare<std::string>("object");
p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, { "eef", "object" });
allow_touch->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
allow_touch->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p) {
collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
const std::string& eef = p.get<std::string>("eef");
const std::string& object = p.get<std::string>("object");
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
->getLinkModelNamesWithCollisionGeometry(), forward);
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)->getLinkModelNamesWithCollisionGeometry(),
forward);
});
insert(std::unique_ptr<ModifyPlanningScene>(allow_touch), insertion_position);
}
@ -125,21 +124,20 @@ void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward)
p.declare<std::string>("object");
p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, { "eef", "object" });
attach->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
const std::string& eef = p.get<std::string>("eef");
moveit_msgs::AttachedCollisionObject obj;
obj.object.operation = forward ? (int8_t) moveit_msgs::CollisionObject::ADD
: (int8_t) moveit_msgs::CollisionObject::REMOVE;
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
obj.object.id = p.get<std::string>("object");
scene->processAttachedCollisionObjectMsg(obj);
});
attach->setCallback([forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p) {
const std::string& eef = p.get<std::string>("eef");
moveit_msgs::AttachedCollisionObject obj;
obj.object.operation =
forward ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE;
obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second;
obj.object.id = p.get<std::string>("object");
scene->processAttachedCollisionObjectMsg(obj);
});
insert(std::unique_ptr<ModifyPlanningScene>(attach), insertion_position);
}
}
void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
{
void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
model_ = robot_model;
SerialContainer::init(robot_model);
}
@ -151,15 +149,13 @@ void SimpleGraspBase::setIKFrame(const Eigen::Isometry3d& pose, const std::strin
setIKFrame(pose_msg);
}
SimpleGrasp::SimpleGrasp(std::unique_ptr<Stage>&& generator, const std::string& name)
: SimpleGraspBase(name)
{
SimpleGrasp::SimpleGrasp(std::unique_ptr<Stage>&& generator, const std::string& name) : SimpleGraspBase(name) {
setup(std::move(generator), true);
}
SimpleUnGrasp::SimpleUnGrasp(std::unique_ptr<Stage>&& generator, const std::string& name)
: SimpleGraspBase(name) {
SimpleUnGrasp::SimpleUnGrasp(std::unique_ptr<Stage>&& generator, const std::string& name) : SimpleGraspBase(name) {
setup(std::move(generator), false);
}
} } }
}
}
}

View File

@ -43,33 +43,25 @@
#include <moveit/planning_scene/planning_scene.h>
#include <assert.h>
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
const planning_scene::PlanningScenePtr&
ensureUpdated(const planning_scene::PlanningScenePtr& scene) {
const planning_scene::PlanningScenePtr& ensureUpdated(const planning_scene::PlanningScenePtr& scene) {
// ensure scene's state is updated
if (scene->getCurrentState().dirty())
scene->getCurrentStateNonConst().update();
return scene;
}
InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps)
: scene_(ensureUpdated(ps))
{
}
InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps) : scene_(ensureUpdated(ps)) {}
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps)
: scene_(ps)
{
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps) : scene_(ps) {
if (scene_->getCurrentState().dirty())
ROS_ERROR_NAMED("InterfaceState", "Dirty PlanningScene! Please only forward clean ones into InterfaceState.");
}
InterfaceState::InterfaceState(const InterfaceState &other)
: scene_(other.scene_), properties_(other.properties_), priority_(other.priority_)
{
}
InterfaceState::InterfaceState(const InterfaceState& other)
: scene_(other.scene_), properties_(other.properties_), priority_(other.priority_) {}
bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other) const {
// infinite costs go always last
@ -86,13 +78,10 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other)
return this->depth() > other.depth();
}
Interface::Interface(const Interface::NotifyFunction &notify)
: notify_(notify)
{}
Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {}
// Announce a new InterfaceState
void Interface::add(InterfaceState &state) {
void Interface::add(InterfaceState& state) {
// require valid scene
assert(state.scene());
// incoming and outgoing must not contain elements both
@ -113,37 +102,36 @@ void Interface::add(InterfaceState &state) {
it->priority_ = InterfaceState::Priority(1, state.incomingTrajectories().front()->cost());
else if (!state.outgoingTrajectories().empty())
it->priority_ = InterfaceState::Priority(1, state.outgoingTrajectories().front()->cost());
else // otherwise, assume priority was well defined before
else // otherwise, assume priority was well defined before
assert(it->priority_ >= InterfaceState::Priority(1, 0.0));
// move list node into interface's state list (sorted by priority)
moveFrom(it, container);
// and finally call notify callback
if (notify_) notify_(it, false);
if (notify_)
notify_(it, false);
}
Interface::container_type Interface::remove(iterator it)
{
Interface::container_type Interface::remove(iterator it) {
container_type result;
moveTo(it, result, result.end());
it->owner_ = nullptr;
return result;
}
void Interface::updatePriority(InterfaceState *state, const InterfaceState::Priority& priority)
{
void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) {
if (priority != state->priority()) {
auto it = std::find_if(begin(), end(), [state](const InterfaceState* other) { return state == other; });
// state should be part of the interface
assert(it != end());
state->priority_ = priority;
update(it);
if (notify_) notify_(it, true);
if (notify_)
notify_(it, true);
}
}
void SolutionBase::setCreator(StagePrivate *creator)
{
void SolutionBase::setCreator(StagePrivate* creator) {
assert(creator_ == nullptr || creator_ == creator); // creator must only set once
creator_ = creator;
}
@ -152,13 +140,11 @@ void SolutionBase::setCost(double cost) {
cost_ = cost;
}
void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info,
Introspection* introspection) const
{
void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const {
info.id = introspection ? introspection->solutionId(*this) : 0;
info.cost = this->cost();
info.comment = this->comment();
const Introspection *ci = introspection;
const Introspection* ci = introspection;
info.stage_id = ci ? ci->stageId(this->creator()->me()) : 0;
const auto& markers = this->markers();
@ -166,8 +152,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info,
std::copy(markers.begin(), markers.end(), info.markers.begin());
}
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection *introspection) const {
void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
msg.sub_trajectory.emplace_back();
moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back();
SolutionBase::fillInfo(t.info, introspection);
@ -178,16 +163,11 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution &msg,
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
}
void SolutionSequence::push_back(const SolutionBase& solution)
{
void SolutionSequence::push_back(const SolutionBase& solution) {
subsolutions_.push_back(&solution);
}
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution &msg,
Introspection* introspection) const
{
void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
moveit_task_constructor_msgs::SubSolution sub_msg;
SolutionBase::fillInfo(sub_msg.info, introspection);
@ -221,5 +201,5 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution &msg,
}
}
}
} }
}
}

View File

@ -49,35 +49,32 @@
#include <functional>
namespace {
std::string rosNormalizeName(const std::string &name) {
std::string rosNormalizeName(const std::string& name) {
std::string n;
n.reserve(name.size());
// drop invalid initial chars
auto read = name.begin(), end = name.end();
while(read != end && !isalpha(*read) && *read != '/' && *read != '~')
while (read != end && !isalpha(*read) && *read != '/' && *read != '~')
++read;
// copy (and correct) remaining chars
while (read != end) {
char c = *read;
n.push_back( (isalnum(c) || c == '_') ? c : '_' );
n.push_back((isalnum(c) || c == '_') ? c : '_');
++read;
}
return n;
}
}
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
TaskPrivate::TaskPrivate(Task* me, const std::string& id)
: WrapperBasePrivate(me, std::string())
, id_(rosNormalizeName(id)), preempt_requested_(false)
{
}
: WrapperBasePrivate(me, std::string()), id_(rosNormalizeName(id)), preempt_requested_(false) {}
void swap(StagePrivate*& lhs, StagePrivate*& rhs)
{
void swap(StagePrivate*& lhs, StagePrivate*& rhs) {
// It only makes sense to swap pimpl instances of a Task!
// However, due to member protection rules, we can only implement it here
assert(typeid(lhs) == typeid(rhs));
@ -97,37 +94,34 @@ void swap(StagePrivate*& lhs, StagePrivate*& rhs)
(*it)->pimpl()->setHierarchy(static_cast<ContainerBase*>(rhs->me_), it);
}
const ContainerBase* TaskPrivate::stages() const
{
const ContainerBase* TaskPrivate::stages() const {
return children().empty() ? nullptr : static_cast<ContainerBase*>(children().front().get());
}
Task::Task(const std::string& id, ContainerBase::pointer &&container)
: WrapperBase(new TaskPrivate(this, id), std::move(container))
{
if (!id.empty()) stages()->setName(id);
Task::Task(const std::string& id, ContainerBase::pointer&& container)
: WrapperBase(new TaskPrivate(this, id), std::move(container)) {
if (!id.empty())
stages()->setName(id);
// monitor state on commandline
//addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
// addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
// enable introspection by default, but only if ros::init() was called
if (ros::isInitialized())
enableIntrospection(true);
}
Task::Task(Task&& other)
: WrapperBase(new TaskPrivate(this, std::string()), std::make_unique<SerialContainer>())
{
Task::Task(Task&& other) : WrapperBase(new TaskPrivate(this, std::string()), std::make_unique<SerialContainer>()) {
*this = std::move(other);
}
Task& Task::operator=(Task&& other)
{
Task& Task::operator=(Task&& other) {
clear(); // remove all stages of current task
swap(this->pimpl_, other.pimpl_);
return *this;
}
struct PlannerCache {
struct PlannerCache
{
typedef std::tuple<std::string, std::string, std::string> PlannerID;
typedef std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline>> PlannerMap;
typedef std::list<std::pair<std::weak_ptr<const robot_model::RobotModel>, PlannerMap>> ModelList;
@ -152,27 +146,26 @@ struct PlannerCache {
}
};
planning_pipeline::PlanningPipelinePtr
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
const std::string& planning_plugin_param_name,
const std::string& adapter_plugins_param_name) {
planning_pipeline::PlanningPipelinePtr Task::createPlanner(const robot_model::RobotModelConstPtr& model,
const std::string& ns,
const std::string& planning_plugin_param_name,
const std::string& adapter_plugins_param_name) {
static PlannerCache cache;
PlannerCache::PlannerID id (ns, planning_plugin_param_name, adapter_plugins_param_name);
PlannerCache::PlannerID id(ns, planning_plugin_param_name, adapter_plugins_param_name);
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(model, id);
planning_pipeline::PlanningPipelinePtr planner = entry.lock();
if (!planner) {
// create new entry
planner = std::make_shared<planning_pipeline::PlanningPipeline>
(model, ros::NodeHandle(ns), planning_plugin_param_name, adapter_plugins_param_name);
planner = std::make_shared<planning_pipeline::PlanningPipeline>(
model, ros::NodeHandle(ns), planning_plugin_param_name, adapter_plugins_param_name);
// store in cache
entry = planner;
}
return planner;
}
Task::~Task()
{
Task::~Task() {
auto impl = pimpl();
clear(); // remove all stages
impl->robot_model_.reset();
@ -180,8 +173,7 @@ Task::~Task()
impl->robot_model_loader_.reset();
}
void Task::setRobotModel(const core::RobotModelConstPtr& robot_model)
{
void Task::setRobotModel(const core::RobotModelConstPtr& robot_model) {
auto impl = pimpl();
reset(); // solutions, scenes, etc become invalid
impl->robot_model_ = robot_model;
@ -195,7 +187,7 @@ void Task::loadRobotModel(const std::string& robot_description) {
throw Exception("Task failed to construct RobotModel");
}
void Task::add(Stage::pointer &&stage) {
void Task::add(Stage::pointer&& stage) {
if (!stage)
throw std::runtime_error("stage insertion failed: invalid stage pointer");
@ -203,49 +195,45 @@ void Task::add(Stage::pointer &&stage) {
throw std::runtime_error(std::string("insertion failed for stage: ") + stage->name());
}
void Task::clear()
{
void Task::clear() {
reset();
stages()->clear();
}
void Task::enableIntrospection(bool enable)
{
void Task::enableIntrospection(bool enable) {
auto impl = pimpl();
if (enable && !impl->introspection_)
impl->introspection_.reset(new Introspection(impl));
else if (!enable && impl->introspection_) {
// reset introspection instance of all stages
pimpl()->setIntrospection(nullptr);
pimpl()->traverseStages([](Stage& stage, int) {
stage.pimpl()->setIntrospection(nullptr);
return true;
}, 1, UINT_MAX);
pimpl()->traverseStages(
[](Stage& stage, int) {
stage.pimpl()->setIntrospection(nullptr);
return true;
},
1, UINT_MAX);
impl->introspection_.reset();
}
}
Introspection &Task::introspection()
{
Introspection& Task::introspection() {
auto impl = pimpl();
enableIntrospection(true);
return *impl->introspection_;
}
Task::TaskCallbackList::const_iterator Task::addTaskCallback(TaskCallback &&cb)
{
Task::TaskCallbackList::const_iterator Task::addTaskCallback(TaskCallback&& cb) {
auto impl = pimpl();
impl->task_cbs_.emplace_back(std::move(cb));
return --(impl->task_cbs_.cend());
}
void Task::erase(TaskCallbackList::const_iterator which)
{
void Task::erase(TaskCallbackList::const_iterator which) {
pimpl()->task_cbs_.erase(which);
}
void Task::reset()
{
void Task::reset() {
auto impl = pimpl();
// signal introspection, that this task was reset
if (impl->introspection_)
@ -254,55 +242,53 @@ void Task::reset()
WrapperBase::reset();
}
void Task::init()
{
void Task::init() {
auto impl = pimpl();
if (!impl->robot_model_)
loadRobotModel();
// initialize push connections of wrapped child
StagePrivate *child = wrapped()->pimpl();
StagePrivate* child = wrapped()->pimpl();
child->setPrevEnds(impl->pendingBackward());
child->setNextStarts(impl->pendingForward());
// and *afterwards* initialize all children recursively
stages()->init(impl->robot_model_);
// task expects its wrapped child to push to both ends, this triggers interface resolution
stages()->pimpl()->pruneInterface(InterfaceFlags({GENERATE}));
stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
// and *finally* validate connectivity
stages()->pimpl()->validateConnectivity();
// provide introspection instance to all stages
impl->setIntrospection(impl->introspection_.get());
impl->traverseStages([impl](Stage& stage, int) {
stage.pimpl()->setIntrospection(impl->introspection_.get());
return true;
}, 1, UINT_MAX);
impl->traverseStages(
[impl](Stage& stage, int) {
stage.pimpl()->setIntrospection(impl->introspection_.get());
return true;
},
1, UINT_MAX);
// first time publish task
if (impl->introspection_)
impl->introspection_->publishTaskDescription();
}
bool Task::canCompute() const
{
bool Task::canCompute() const {
return stages()->canCompute();
}
void Task::compute()
{
void Task::compute() {
stages()->compute();
}
bool Task::plan(size_t max_solutions)
{
bool Task::plan(size_t max_solutions) {
auto impl = pimpl();
reset();
init();
impl->preempt_requested_ = false;
while(ros::ok() && !impl->preempt_requested_ && canCompute() &&
(max_solutions == 0 || numSolutions() < max_solutions)) {
while (ros::ok() && !impl->preempt_requested_ && canCompute() &&
(max_solutions == 0 || numSolutions() < max_solutions)) {
compute();
for (const auto& cb : impl->task_cbs_)
cb(*this);
@ -313,13 +299,11 @@ bool Task::plan(size_t max_solutions)
return numSolutions() > 0;
}
void Task::preempt()
{
void Task::preempt() {
pimpl()->preempt_requested_ = true;
}
void Task::execute(const SolutionBase &s)
{
void Task::execute(const SolutionBase& s) {
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> ac("execute_task_solution");
ac.waitForServer();
@ -329,49 +313,41 @@ void Task::execute(const SolutionBase &s)
ac.waitForResult();
}
void Task::publishAllSolutions(bool wait)
{
void Task::publishAllSolutions(bool wait) {
enableIntrospection(true);
pimpl()->introspection_->publishAllSolutions(wait);
}
void Task::onNewSolution(const SolutionBase &s)
{
void Task::onNewSolution(const SolutionBase& s) {
auto impl = pimpl();
// no need to call WrapperBase::onNewSolution!
if (impl->introspection_)
impl->introspection_->publishSolution(s);
}
ContainerBase* Task::stages()
{
ContainerBase* Task::stages() {
return static_cast<ContainerBase*>(WrapperBase::wrapped());
}
const ContainerBase* Task::stages() const
{
const ContainerBase* Task::stages() const {
return const_cast<Task*>(this)->stages();
}
PropertyMap &Task::properties()
{
PropertyMap& Task::properties() {
// forward to wrapped() stage
return wrapped()->properties();
}
void Task::setProperty(const std::string &name, const boost::any &value)
{
void Task::setProperty(const std::string& name, const boost::any& value) {
// forward to wrapped() stage
wrapped()->setProperty(name, value);
}
std::string Task::id() const
{
std::string Task::id() const {
return pimpl()->id();
}
const core::RobotModelConstPtr& Task::getRobotModel() const
{
const core::RobotModelConstPtr& Task::getRobotModel() const {
auto impl = pimpl();
return impl->robot_model_;
}
@ -379,5 +355,5 @@ const core::RobotModelConstPtr& Task::getRobotModel() const
void Task::printState(std::ostream& os) const {
os << *stages();
}
} }
}
}

View File

@ -4,18 +4,23 @@ using namespace moveit::task_constructor;
::std::ostream& operator<<(::std::ostream& os, const InterfaceFlag& flag) {
switch (flag) {
case READS_START: return os << "READS_START";
case READS_END: return os << "READS_END";
case WRITES_NEXT_START: return os << "WRITES_NEXT_START";
case WRITES_PREV_END: return os << "WRITES_PREV_END";
default: return os << "unknown";
case READS_START:
return os << "READS_START";
case READS_END:
return os << "READS_END";
case WRITES_NEXT_START:
return os << "WRITES_NEXT_START";
case WRITES_PREV_END:
return os << "WRITES_PREV_END";
default:
return os << "unknown";
}
}
::std::ostream& operator<<(::std::ostream& os, const InterfaceFlags& flags) {
os << "InterfaceFlags(";
bool have_previous = false;
for (InterfaceFlag f : {READS_START,READS_END,WRITES_NEXT_START,WRITES_PREV_END}) {
for (InterfaceFlag f : { READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END }) {
if (flags & f) {
os << (have_previous ? ", " : "{") << f;
have_previous = true;

View File

@ -6,189 +6,185 @@
using namespace moveit::core;
namespace {
const std::string R_MODEL0 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"one_robot\">"
"<link name=\"base_link\">"
" <inertial>"
" <mass value=\"2.81\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision name=\"my_collision\">"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"<joint name=\"joint_a\" type=\"continuous\">"
" <axis xyz=\"0 0 1\"/>"
" <parent link=\"base_link\"/>"
" <child link=\"link_a\"/>"
" <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
"</joint>"
"<link name=\"link_a\">"
" <inertial>"
" <mass value=\"1.0\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"<joint name=\"joint_b\" type=\"fixed\">"
" <parent link=\"link_a\"/>"
" <child link=\"link_b\"/>"
" <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
"</joint>"
"<link name=\"link_b\">"
" <inertial>"
" <mass value=\"1.0\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
" <joint name=\"joint_c\" type=\"prismatic\">"
" <axis xyz=\"1 0 0\"/>"
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
" <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
"soft_upper_limit=\"0.089\"/>"
" <parent link=\"link_b\"/>"
" <child link=\"link_c\"/>"
" <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
" </joint>"
"<link name=\"link_c\">"
" <inertial>"
" <mass value=\"1.0\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
" <joint name=\"mim_f\" type=\"prismatic\">"
" <axis xyz=\"1 0 0\"/>"
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
" <parent link=\"link_c\"/>"
" <child link=\"link_d\"/>"
" <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
" <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
" </joint>"
" <joint name=\"joint_f\" type=\"prismatic\">"
" <axis xyz=\"1 0 0\"/>"
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
" <parent link=\"link_d\"/>"
" <child link=\"link_e\"/>"
" <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
" </joint>"
"<link name=\"link_d\">"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"<link name=\"link_e\">"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"</robot>";
const std::string R_MODEL0 = "<?xml version=\"1.0\" ?>"
"<robot name=\"one_robot\">"
"<link name=\"base_link\">"
" <inertial>"
" <mass value=\"2.81\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision name=\"my_collision\">"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"<joint name=\"joint_a\" type=\"continuous\">"
" <axis xyz=\"0 0 1\"/>"
" <parent link=\"base_link\"/>"
" <child link=\"link_a\"/>"
" <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
"</joint>"
"<link name=\"link_a\">"
" <inertial>"
" <mass value=\"1.0\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"<joint name=\"joint_b\" type=\"fixed\">"
" <parent link=\"link_a\"/>"
" <child link=\"link_b\"/>"
" <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
"</joint>"
"<link name=\"link_b\">"
" <inertial>"
" <mass value=\"1.0\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
" <joint name=\"joint_c\" type=\"prismatic\">"
" <axis xyz=\"1 0 0\"/>"
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
" <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
"soft_upper_limit=\"0.089\"/>"
" <parent link=\"link_b\"/>"
" <child link=\"link_c\"/>"
" <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
" </joint>"
"<link name=\"link_c\">"
" <inertial>"
" <mass value=\"1.0\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
" <joint name=\"mim_f\" type=\"prismatic\">"
" <axis xyz=\"1 0 0\"/>"
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
" <parent link=\"link_c\"/>"
" <child link=\"link_d\"/>"
" <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
" <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
" </joint>"
" <joint name=\"joint_f\" type=\"prismatic\">"
" <axis xyz=\"1 0 0\"/>"
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
" <parent link=\"link_d\"/>"
" <child link=\"link_e\"/>"
" <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
" </joint>"
"<link name=\"link_d\">"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"<link name=\"link_e\">"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"</robot>";
const std::string S_MODEL0 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"one_robot\">"
"<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
"<group name=\"base_from_joints\">"
"<joint name=\"base_joint\"/>"
"<joint name=\"joint_a\"/>"
"<joint name=\"joint_c\"/>"
"</group>"
"<group name=\"mim_joints\">"
"<joint name=\"joint_f\"/>"
"<joint name=\"mim_f\"/>"
"</group>"
"<group name=\"base_with_subgroups\">"
"<group name=\"base_from_base_to_tip\"/>"
"<joint name=\"joint_c\"/>"
"</group>"
"<group name=\"base_from_base_to_tip\">"
"<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
"<joint name=\"base_joint\"/>"
"</group>"
"<group name=\"base_with_bad_subgroups\">"
"<group name=\"error\"/>"
"</group>"
"<end_effector name=\"eef\" parent_link=\"link_b\" group=\"mim_joints\" parent_group=\"base_from_base_to_tip\"/>"
"</robot>";
"<?xml version=\"1.0\" ?>"
"<robot name=\"one_robot\">"
"<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
"<group name=\"base_from_joints\">"
"<joint name=\"base_joint\"/>"
"<joint name=\"joint_a\"/>"
"<joint name=\"joint_c\"/>"
"</group>"
"<group name=\"mim_joints\">"
"<joint name=\"joint_f\"/>"
"<joint name=\"mim_f\"/>"
"</group>"
"<group name=\"base_with_subgroups\">"
"<group name=\"base_from_base_to_tip\"/>"
"<joint name=\"joint_c\"/>"
"</group>"
"<group name=\"base_from_base_to_tip\">"
"<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
"<joint name=\"base_joint\"/>"
"</group>"
"<group name=\"base_with_bad_subgroups\">"
"<group name=\"error\"/>"
"</group>"
"<end_effector name=\"eef\" parent_link=\"link_b\" group=\"mim_joints\" parent_group=\"base_from_base_to_tip\"/>"
"</robot>";
}
RobotModelPtr getModel()
{
RobotModelPtr getModel() {
urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(R_MODEL0);
srdf::ModelSharedPtr srdf_model(new srdf::Model());
srdf_model->initString(*urdf_model, S_MODEL0);
return RobotModelPtr(new RobotModel(urdf_model, srdf_model));
}
moveit::core::RobotModelPtr loadModel()
{
moveit::core::RobotModelPtr loadModel() {
robot_model_loader::RobotModelLoader loader;
return loader.getModel();
}

View File

@ -2,9 +2,11 @@
#include <moveit/macros/class_forward.h>
namespace moveit { namespace core {
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(RobotModel)
} }
}
}
// get a hard-coded model
moveit::core::RobotModelPtr getModel();

View File

@ -21,18 +21,18 @@ using namespace moveit::task_constructor;
void spawnObject(const planning_scene::PlanningScenePtr& scene) {
moveit_msgs::CollisionObject o;
o.id= "object";
o.header.frame_id= "world";
o.id = "object";
o.header.frame_id = "world";
o.primitive_poses.resize(1);
o.primitive_poses[0].position.x= 0.3;
o.primitive_poses[0].position.y= 0.23;
o.primitive_poses[0].position.z= 0.10;
o.primitive_poses[0].orientation.w= 1.0;
o.primitive_poses[0].position.x = 0.3;
o.primitive_poses[0].position.y = 0.23;
o.primitive_poses[0].position.z = 0.10;
o.primitive_poses[0].orientation.w = 1.0;
o.primitives.resize(1);
o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER;
o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
o.primitives[0].dimensions.resize(2);
o.primitives[0].dimensions[0]= 0.23;
o.primitives[0].dimensions[1]= 0.03;
o.primitives[0].dimensions[0] = 0.23;
o.primitives[0].dimensions[1] = 0.03;
scene->processCollisionObjectMsg(o);
}
@ -73,7 +73,7 @@ TEST(PA10, pick) {
}
{
stages::Connect::GroupPlannerVector planners = {{"left_hand", pipeline}, {"left_arm", pipeline}};
stages::Connect::GroupPlannerVector planners = { { "left_hand", pipeline }, { "left_arm", pipeline } };
auto move = std::make_unique<stages::Connect>("connect", planners);
move->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(move));
@ -103,20 +103,21 @@ TEST(PA10, pick) {
gengrasp->setMonitoredStage(initial_stage);
auto filter = std::make_unique<stages::PredicateFilter>("filtered");
gengrasp->properties().exposeTo(filter->properties(), {"eef"});
gengrasp->properties().exposeTo(filter->properties(), { "eef" });
filter->properties().configureInitFrom(Stage::PARENT);
filter->insert(std::move(gengrasp));
filter->setPredicate([](const SolutionBase& s, std::string& comment) {
bool accept = s.cost() < 2;
if (!accept) comment += " (rejected)";
if (!accept)
comment += " (rejected)";
return accept;
});
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(filter));
PropertyMap &props = ik->properties();
props.configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
props.configureInitFrom(Stage::INTERFACE, {"target_pose"}); // derived from child's solution
ik->setIKFrame(Eigen::Translation3d(0,0,.05) * Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
PropertyMap& props = ik->properties();
props.configureInitFrom(Stage::PARENT, { "group", "eef", "default_pose" });
props.configureInitFrom(Stage::INTERFACE, { "target_pose" }); // derived from child's solution
ik->setIKFrame(Eigen::Translation3d(0, 0, .05) * Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitY()),
"lh_tool_frame");
ik->setMaxIKSolutions(1);
t.add(std::move(ik));
@ -126,7 +127,8 @@ TEST(PA10, pick) {
auto move = std::make_unique<stages::ModifyPlanningScene>("allow object collision");
move->restrictDirection(stages::ModifyPlanningScene::FORWARD);
move->allowCollisions("object", t.getRobotModel()->getJointModelGroup("left_hand")->getLinkModelNamesWithCollisionGeometry(), true);
move->allowCollisions(
"object", t.getRobotModel()->getJointModelGroup("left_hand")->getLinkModelNamesWithCollisionGeometry(), true);
t.add(std::move(move));
}
@ -147,7 +149,7 @@ TEST(PA10, pick) {
{
auto move = std::make_unique<stages::MoveRelative>("lift object", cartesian);
move->properties().configureInitFrom(Stage::PARENT, {"group"});
move->properties().configureInitFrom(Stage::PARENT, { "group" });
move->setMinMaxDistance(0.03, 0.05);
move->properties().set("marker_ns", std::string("lift"));
move->setIKFrame("lh_tool_frame");
@ -161,7 +163,7 @@ TEST(PA10, pick) {
{
auto move = std::make_unique<stages::MoveRelative>("shift object", cartesian);
move->properties().configureInitFrom(Stage::PARENT, {"group"});
move->properties().configureInitFrom(Stage::PARENT, { "group" });
move->setMinMaxDistance(0.1, 0.2);
move->properties().set("marker_ns", std::string("lift"));
move->setIKFrame("lh_tool_frame");
@ -176,7 +178,7 @@ TEST(PA10, pick) {
try {
t.plan();
} catch (const InitStageException &e) {
} catch (const InitStageException& e) {
ADD_FAILURE() << "planning failed with exception" << std::endl << e << t;
}
@ -185,7 +187,7 @@ TEST(PA10, pick) {
EXPECT_LE(solutions, 10u);
}
int main(int argc, char** argv){
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "pa10");
ros::AsyncSpinner spinner(1);

View File

@ -13,22 +13,22 @@
using namespace moveit::task_constructor;
void spawnObject(){
void spawnObject() {
moveit::planning_interface::PlanningSceneInterface psi;
moveit_msgs::CollisionObject o;
o.id= "object";
o.header.frame_id= "base_link";
o.id = "object";
o.header.frame_id = "base_link";
o.primitive_poses.resize(1);
o.primitive_poses[0].position.x= 0.53;
o.primitive_poses[0].position.y= 0.05;
o.primitive_poses[0].position.z= 0.84;
o.primitive_poses[0].orientation.w= 1.0;
o.primitive_poses[0].position.x = 0.53;
o.primitive_poses[0].position.y = 0.05;
o.primitive_poses[0].position.z = 0.84;
o.primitive_poses[0].orientation.w = 1.0;
o.primitives.resize(1);
o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER;
o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
o.primitives[0].dimensions.resize(2);
o.primitives[0].dimensions[0]= 0.23;
o.primitives[0].dimensions[1]= 0.03;
o.primitives[0].dimensions[0] = 0.23;
o.primitives[0].dimensions[1] = 0.03;
psi.applyCollisionObject(o);
}
@ -42,7 +42,7 @@ TEST(PR2, pick) {
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setPlannerId("RRTConnectkConfigDefault");
// connect to pick
stages::Connect::GroupPlannerVector planners = {{"left_arm", pipeline}, {"left_gripper", pipeline}};
stages::Connect::GroupPlannerVector planners = { { "left_arm", pipeline }, { "left_gripper", pipeline } };
auto connect = std::make_unique<stages::Connect>("connect", planners);
connect->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(connect));
@ -76,7 +76,7 @@ TEST(PR2, pick) {
try {
spawnObject();
t.plan();
} catch (const InitStageException &e) {
} catch (const InitStageException& e) {
ADD_FAILURE() << "planning failed with exception" << std::endl << e << t;
}
@ -85,7 +85,7 @@ TEST(PR2, pick) {
EXPECT_LE(solutions, 10u);
}
int main(int argc, char** argv){
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "pr2");
ros::AsyncSpinner spinner(1);

View File

@ -13,22 +13,22 @@
using namespace moveit::task_constructor;
void spawnObject(){
void spawnObject() {
moveit::planning_interface::PlanningSceneInterface psi;
moveit_msgs::CollisionObject o;
o.id= "object";
o.header.frame_id= "table_top";
o.id = "object";
o.header.frame_id = "table_top";
o.primitive_poses.resize(1);
o.primitive_poses[0].position.x= -0.2;
o.primitive_poses[0].position.y= 0.13;
o.primitive_poses[0].position.z= 0.12;
o.primitive_poses[0].orientation.w= 1.0;
o.primitive_poses[0].position.x = -0.2;
o.primitive_poses[0].position.y = 0.13;
o.primitive_poses[0].position.z = 0.12;
o.primitive_poses[0].orientation.w = 1.0;
o.primitives.resize(1);
o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER;
o.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER;
o.primitives[0].dimensions.resize(2);
o.primitives[0].dimensions[0]= 0.23;
o.primitives[0].dimensions[1]= 0.03;
o.primitives[0].dimensions[0] = 0.23;
o.primitives[0].dimensions[1] = 0.03;
psi.applyCollisionObject(o);
}
@ -44,7 +44,7 @@ TEST(UR5, pick) {
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setPlannerId("RRTConnectkConfigDefault");
// connect to pick
stages::Connect::GroupPlannerVector planners = {{"arm", pipeline}, {"gripper", pipeline}};
stages::Connect::GroupPlannerVector planners = { { "arm", pipeline }, { "gripper", pipeline } };
auto connect = std::make_unique<stages::Connect>("connect", planners);
connect->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(connect));
@ -57,7 +57,7 @@ TEST(UR5, pick) {
grasp_generator->setMonitoredStage(initial_stage);
auto grasp = std::make_unique<stages::SimpleGrasp>(std::unique_ptr<MonitoringGenerator>(grasp_generator));
grasp->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
grasp->setIKFrame(Eigen::Translation3d(.03, 0, 0), "s_model_tool0");
grasp->setMaxIKSolutions(8);
auto pick = std::make_unique<stages::Pick>(std::move(grasp));
@ -78,7 +78,7 @@ TEST(UR5, pick) {
try {
spawnObject();
t.plan();
} catch (const InitStageException &e) {
} catch (const InitStageException& e) {
ADD_FAILURE() << "planning failed with exception" << std::endl << e << t;
}
@ -87,7 +87,7 @@ TEST(UR5, pick) {
EXPECT_LE(solutions, 60u);
}
int main(int argc, char** argv){
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "ur5");
ros::AsyncSpinner spinner(1);

View File

@ -11,39 +11,54 @@
using namespace moveit::task_constructor;
class GeneratorMockup : public Generator {
class GeneratorMockup : public Generator
{
int runs = 0;
public:
GeneratorMockup(int runs = 0) : Generator("generator"), runs(runs) {}
bool canCompute() const override { return runs > 0; }
void compute() override { if (runs > 0) --runs; }
void compute() override {
if (runs > 0)
--runs;
}
};
class MonitoringGeneratorMockup : public MonitoringGenerator {
class MonitoringGeneratorMockup : public MonitoringGenerator
{
public:
MonitoringGeneratorMockup(Stage* monitored)
: MonitoringGenerator("monitoring generator", monitored) {}
MonitoringGeneratorMockup(Stage* monitored) : MonitoringGenerator("monitoring generator", monitored) {}
bool canCompute() const override { return false; }
void compute() override {}
void onNewSolution(const SolutionBase &s) override {}
void onNewSolution(const SolutionBase& s) override {}
};
class PropagatorMockup : public PropagatingEitherWay {
class PropagatorMockup : public PropagatingEitherWay
{
int fw_runs = 0;
int bw_runs = 0;
public:
PropagatorMockup(int fw = 0, int bw = 0) : PropagatingEitherWay("either way"), fw_runs(fw), bw_runs(bw) {}
void computeForward(const InterfaceState &from) override { if (fw_runs > 0) --fw_runs; }
void computeBackward(const InterfaceState &from) override { if (bw_runs > 0) --bw_runs; }
void computeForward(const InterfaceState& from) override {
if (fw_runs > 0)
--fw_runs;
}
void computeBackward(const InterfaceState& from) override {
if (bw_runs > 0)
--bw_runs;
}
};
class ForwardMockup : public PropagatorMockup {
class ForwardMockup : public PropagatorMockup
{
public:
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
restrictDirection(FORWARD);
setName("forward");
}
};
class BackwardMockup : public PropagatorMockup {
class BackwardMockup : public PropagatorMockup
{
public:
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
restrictDirection(BACKWARD);
@ -51,44 +66,64 @@ public:
}
};
class ConnectMockup : public Connecting {
class ConnectMockup : public Connecting
{
int runs = 0;
public:
ConnectMockup(int runs = 0) : Connecting("connect"), runs(runs) {}
void compute(const InterfaceState& from, const InterfaceState& to) override { if (runs > 0) --runs; }
void compute(const InterfaceState& from, const InterfaceState& to) override {
if (runs > 0)
--runs;
}
};
enum StageType { GEN, FW, BW, ANY, BOTH, CONN };
enum StageType
{
GEN,
FW,
BW,
ANY,
BOTH,
CONN
};
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
for (StageType t : types) {
switch (t) {
case GEN: c.insert(std::make_unique<GeneratorMockup>(runs)); break;
case FW: c.insert(std::make_unique<ForwardMockup>(runs)); break;
case BW: c.insert(std::make_unique<BackwardMockup>(runs)); break;
case ANY: c.insert(std::make_unique<PropagatorMockup>(runs, runs)); break;
case BOTH: {
auto s = std::make_unique<PropagatorMockup>(runs, runs);
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
c.insert(std::move(s));
break;
}
case CONN: c.insert(std::make_unique<ConnectMockup>(runs)); break;
case GEN:
c.insert(std::make_unique<GeneratorMockup>(runs));
break;
case FW:
c.insert(std::make_unique<ForwardMockup>(runs));
break;
case BW:
c.insert(std::make_unique<BackwardMockup>(runs));
break;
case ANY:
c.insert(std::make_unique<PropagatorMockup>(runs, runs));
break;
case BOTH: {
auto s = std::make_unique<PropagatorMockup>(runs, runs);
s->restrictDirection(PropagatingEitherWay::BOTHWAY);
c.insert(std::move(s));
break;
}
case CONN:
c.insert(std::make_unique<ConnectMockup>(runs));
break;
}
}
}
class NamedStage : public GeneratorMockup {
class NamedStage : public GeneratorMockup
{
public:
NamedStage(const std::string& name) : GeneratorMockup() {
setName(name);
}
NamedStage(const std::string& name) : GeneratorMockup() { setName(name); }
};
TEST(ContainerBase, positionForInsert) {
SerialContainer s;
SerialContainerPrivate *impl = s.pimpl();
SerialContainerPrivate* impl = s.pimpl();
EXPECT_EQ(impl->childByIndex(0, true), impl->children().end());
EXPECT_EQ(impl->childByIndex(1, true), impl->children().end());
@ -115,11 +150,11 @@ TEST(ContainerBase, positionForInsert) {
TEST(ContainerBase, findChild) {
SerialContainer s, *c2;
Stage *a, *b, *c1, *d;
s.insert(Stage::pointer(a=new NamedStage("a")));
s.insert(Stage::pointer(b=new NamedStage("b")));
s.insert(Stage::pointer(c1=new NamedStage("c")));
s.insert(Stage::pointer(a = new NamedStage("a")));
s.insert(Stage::pointer(b = new NamedStage("b")));
s.insert(Stage::pointer(c1 = new NamedStage("c")));
auto sub = Stage::pointer(c2 = new SerialContainer("c"));
c2->insert(Stage::pointer(d=new NamedStage("d")));
c2->insert(Stage::pointer(d = new NamedStage("d")));
s.insert(std::move(sub));
EXPECT_EQ(s.findChild("a"), a);
@ -129,9 +164,9 @@ TEST(ContainerBase, findChild) {
EXPECT_EQ(s.findChild("c/d"), d);
}
template <typename Container>
class InitTest : public ::testing::Test {
class InitTest : public ::testing::Test
{
protected:
moveit::core::RobotModelConstPtr robot_model;
Container container;
@ -139,15 +174,17 @@ protected:
InitTest() : ::testing::Test(), dummy(new Interface) {}
void pushInterface(bool start=true, bool end=true) {
void pushInterface(bool start = true, bool end = true) {
// pretend, that the container is connected
ContainerBasePrivate *impl = container.pimpl();
if (start) impl->setPrevEnds(dummy);
if (end) impl->setNextStarts(dummy);
ContainerBasePrivate* impl = container.pimpl();
if (start)
impl->setPrevEnds(dummy);
if (end)
impl->setNextStarts(dummy);
}
void reset(bool start=true, bool end=true) {
void reset(bool start = true, bool end = true) {
container.reset();
ContainerBasePrivate *impl = container.pimpl();
ContainerBasePrivate* impl = container.pimpl();
impl->setNextStarts(InterfacePtr());
impl->setPrevEnds(InterfacePtr());
pushInterface(start, end);
@ -160,16 +197,20 @@ protected:
container.init(robot_model);
// prune pull interfaces based on provided external interface (start, end)
InterfaceFlags accepted;
if (start) accepted |= WRITES_PREV_END;
if (end) accepted |= WRITES_NEXT_START;
if (start)
accepted |= WRITES_PREV_END;
if (end)
accepted |= WRITES_NEXT_START;
container.pimpl()->pruneInterface(accepted);
container.pimpl()->validateConnectivity();
if (!expect_failure) return; // as expected
if (!expect_failure)
return; // as expected
ADD_FAILURE() << "init() didn't recognize a failure condition as expected\n" << container;
} catch (const InitStageException &e) {
if (expect_failure) return; // as expected
} catch (const InitStageException& e) {
if (expect_failure)
return; // as expected
ADD_FAILURE() << "unexpected init failure: \n" << e << "\n" << container;
} catch (const std::exception &e) {
} catch (const std::exception& e) {
ADD_FAILURE() << "unexpected exception thrown:\n" << e.what();
} catch (...) {
ADD_FAILURE() << "unexpected unknown exception thrown";
@ -177,9 +218,10 @@ protected:
}
};
class SerialTest : public InitTest<SerialContainer> {
class SerialTest : public InitTest<SerialContainer>
{
protected:
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<StagePrivate*> &expected) {
void validateOrder(const SerialContainerPrivate* container, const std::initializer_list<StagePrivate*>& expected) {
size_t num = container->children().size();
ASSERT_TRUE(num == expected.size()) << "different list lengths";
@ -190,23 +232,25 @@ protected:
// validate order
size_t pos = 0;
auto exp_it = expected.begin();
for (auto it = container->children().begin(), end = container->children().end(); it != end; ++it, ++exp_it, ++pos) {
StagePrivate *child = (*it)->pimpl();
for (auto it = container->children().begin(), end = container->children().end(); it != end;
++it, ++exp_it, ++pos) {
StagePrivate* child = (*it)->pimpl();
EXPECT_EQ(child, *exp_it) << "wrong order";
EXPECT_EQ(child->parent()->pimpl(), container) << "wrong parent";
EXPECT_EQ(it, container->childByIndex(pos)) << "bad forward position resolution";
EXPECT_EQ(it, container->childByIndex(pos-num)) << "bad backward position resolution";
EXPECT_EQ(it, container->childByIndex(pos - num)) << "bad backward position resolution";
}
}
};
#define VALIDATE(...) {\
SCOPED_TRACE("validateOrder({" #__VA_ARGS__ "})"); \
validateOrder(impl, {__VA_ARGS__}); \
}
#define VALIDATE(...) \
{ \
SCOPED_TRACE("validateOrder({" #__VA_ARGS__ "})"); \
validateOrder(impl, { __VA_ARGS__ }); \
}
TEST_F(SerialTest, insertion_order) {
SerialContainerPrivate *impl = container.pimpl();
SerialContainerPrivate* impl = container.pimpl();
EXPECT_EQ(impl->parent(), nullptr);
EXPECT_THROW(container.init(robot_model), InitStageException);
@ -214,50 +258,51 @@ TEST_F(SerialTest, insertion_order) {
/***** inserting first stage *****/
auto g = std::make_unique<GeneratorMockup>();
StagePrivate *gp = g->pimpl();
StagePrivate* gp = g->pimpl();
ASSERT_TRUE(container.insert(std::move(g)));
EXPECT_FALSE(g); // ownership transferred to container
EXPECT_FALSE(g); // ownership transferred to container
VALIDATE(gp);
/***** inserting second stage *****/
auto f = std::make_unique<ForwardMockup>();
StagePrivate *fp = f->pimpl();
StagePrivate* fp = f->pimpl();
ASSERT_TRUE(container.insert(std::move(f)));
EXPECT_FALSE(f); // ownership transferred to container
EXPECT_FALSE(f); // ownership transferred to container
VALIDATE(gp, fp);
/***** inserting third stage *****/
auto f2 = std::make_unique<ForwardMockup>();
StagePrivate *fp2 = f2->pimpl();
StagePrivate* fp2 = f2->pimpl();
ASSERT_TRUE(container.insert(std::move(f2), 1));
EXPECT_FALSE(f2); // ownership transferred to container
EXPECT_FALSE(f2); // ownership transferred to container
VALIDATE(gp, fp2, fp);
/***** inserting another generator stage *****/
auto g2 = std::make_unique<GeneratorMockup>();
StagePrivate *gp2 = g2->pimpl();
StagePrivate* gp2 = g2->pimpl();
ASSERT_TRUE(container.insert(std::move(g2)));
VALIDATE(gp, fp2, fp, gp2);
}
#define EXPECT_INIT_FAILURE(start, end, ...) \
{ \
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
container.clear(); \
validateInit(start, end, { __VA_ARGS__ }, true); \
}
#define EXPECT_INIT_FAILURE(start, end, ...) {\
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
container.clear(); \
validateInit(start, end, {__VA_ARGS__}, true); \
}
#define EXPECT_INIT_SUCCESS(start, end, ...) {\
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
container.clear(); \
validateInit(start, end, {__VA_ARGS__}, false); \
}
#define EXPECT_INIT_SUCCESS(start, end, ...) \
{ \
SCOPED_TRACE("validateInit({" #__VA_ARGS__ "})"); \
container.clear(); \
validateInit(start, end, { __VA_ARGS__ }, false); \
}
TEST_F(SerialTest, init_empty) {
EXPECT_INIT_FAILURE(true, true); // no children
EXPECT_INIT_FAILURE(true, true); // no children
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_FAILURE(false, false); // no children
EXPECT_INIT_FAILURE(false, false); // no children
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags());
}
@ -266,18 +311,18 @@ TEST_F(SerialTest, init_connecting) {
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
EXPECT_INIT_FAILURE(true, true, CONN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({CONNECT, GENERATE}));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags({ CONNECT, GENERATE }));
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
EXPECT_INIT_FAILURE(false, false, CONN, CONN); // two connecting stages cannot be connected
}
TEST_F(SerialTest, init_generator) {
EXPECT_INIT_SUCCESS(true, true, GEN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes
EXPECT_INIT_FAILURE(false, false, GEN); // generator wants to push, but container cannot propagate pushes
EXPECT_INIT_FAILURE(true, true, GEN, GEN); // two generator stages cannot be connected
EXPECT_INIT_FAILURE(true, true, GEN, GEN); // two generator stages cannot be connected
}
TEST_F(SerialTest, init_forward) {
@ -303,7 +348,6 @@ TEST_F(SerialTest, init_forward) {
EXPECT_INIT_SUCCESS(true, true, BW, GEN, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_INIT_SUCCESS(false, true, ANY, FW);
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
@ -342,9 +386,9 @@ TEST_F(SerialTest, init_backward) {
TEST_F(SerialTest, interface_detection) {
// derive propagation direction from inner generator
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
EXPECT_INIT_SUCCESS(true, true, ANY, GEN, ANY); // <- <-> ->
auto it = container.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
@ -356,9 +400,9 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(GENERATE));
// derive propagation direction from inner connector
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
EXPECT_INIT_SUCCESS(false, false, ANY, CONN, ANY); // -> -- <-
it = container.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
@ -375,27 +419,27 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_INIT_SUCCESS(true, false, ANY); // should be pruned to BW
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->
it = container.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY, ANY); // <- <-
EXPECT_INIT_SUCCESS(true, false, ANY, ANY); // <- <-
it = container.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <>
EXPECT_INIT_SUCCESS(true, true, ANY, ANY); // <> <>
it = container.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_EQ(container.pimpl()->interfaceFlags(), PROPAGATE_BOTHWAYS);
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- --
EXPECT_INIT_FAILURE(false, false, ANY, ANY); // -- --
it = container.pimpl()->children().begin();
EXPECT_EQ( (*it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ((*it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ((*++it)->pimpl()->interfaceFlags(), UNKNOWN);
EXPECT_EQ(container.pimpl()->interfaceFlags(), UNKNOWN);
}
@ -405,37 +449,48 @@ TEST_F(SerialTest, nested_interface_detection) {
// direction imposed from outer generator
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, {ANY, ANY});
append(container, {GEN, ANY, ANY});
append(*inner, { ANY, ANY });
append(container, { GEN, ANY, ANY });
container.insert(Stage::pointer(inner), -2);
{SCOPED_TRACE("GEN - ANY - inner - ANY"); validateInit(true, true, {}, false);}
{
SCOPED_TRACE("GEN - ANY - inner - ANY");
validateInit(true, true, {}, false);
}
// direction imposed from inner generator
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, {ANY, GEN, ANY});
append(container, {ANY, ANY});
append(*inner, { ANY, GEN, ANY });
append(container, { ANY, ANY });
container.insert(Stage::pointer(inner), -2);
{SCOPED_TRACE("ANY - inner - ANY"); validateInit(true, true, {}, false);}
{
SCOPED_TRACE("ANY - inner - ANY");
validateInit(true, true, {}, false);
}
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, {GEN, ANY});
append(*inner, { GEN, ANY });
container.insert(Stage::pointer(inner));
{SCOPED_TRACE("inner - ANY"); validateInit(true, true, {ANY}, false);}
{
SCOPED_TRACE("inner - ANY");
validateInit(true, true, { ANY }, false);
}
// outer and inner generators conflict with each other
container.clear();
inner = new SerialContainer("inner serial");
append(*inner, {ANY, GEN, ANY});
append(container, {GEN, ANY, ANY});
append(*inner, { ANY, GEN, ANY });
append(container, { GEN, ANY, ANY });
container.insert(Stage::pointer(inner), -2);
{SCOPED_TRACE("GEN - ANY - inner - ANY"); validateInit(true, true, {}, true);}
{
SCOPED_TRACE("GEN - ANY - inner - ANY");
validateInit(true, true, {}, true);
}
}
class ParallelTest : public InitTest<Alternatives> {
};
class ParallelTest : public InitTest<Alternatives>
{};
TEST_F(ParallelTest, init_propagating) {
EXPECT_INIT_SUCCESS(true, true, BW, FW);
@ -533,7 +588,6 @@ TEST_F(ParallelTest, init_mismatching) {
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
}
TEST(Task, move) {
Task t1("foo");
t1.add(std::make_unique<GeneratorMockup>());
@ -559,7 +613,7 @@ TEST(Task, reuse) {
Task t("first");
t.setRobotModel(robot_model);
auto configure = [] (Task& t) {
auto configure = [](Task& t) {
auto ref = new stages::FixedState("fixed");
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
ref->setState(scene);
@ -581,7 +635,7 @@ TEST(Task, reuse) {
configure(t);
t.plan(1);
} catch (const InitStageException &e) {
} catch (const InitStageException& e) {
ADD_FAILURE() << "InitStageException:" << std::endl << e << t;
}
}

View File

@ -8,26 +8,49 @@
namespace mtc = moveit::task_constructor;
// type-trait functions for OrderedTest<T>
template <typename T> T create(int cost);
template <> int create(int cost) { return cost; }
template <> int* create(int cost) { return new int(cost); }
template <> mtc::SolutionBasePtr create(int cost) {
template <typename T>
T create(int cost);
template <>
int create(int cost) {
return cost;
}
template <>
int* create(int cost) {
return new int(cost);
}
template <>
mtc::SolutionBasePtr create(int cost) {
mtc::SolutionBasePtr s = std::make_shared<mtc::SubTrajectory>();
s->setCost(cost);
return s;
}
template <> mtc::SolutionBaseConstPtr create(int cost) {
template <>
mtc::SolutionBaseConstPtr create(int cost) {
return create<mtc::SolutionBasePtr>(cost);
}
template <typename T> int value(const T& item);
template <> int value(const int& item) { return item; }
template <> int value(int* const& item) { return *item; }
template <> int value(const mtc::SolutionBasePtr& item) { return item->cost(); }
template <> int value(const mtc::SolutionBaseConstPtr& item) { return item->cost(); }
template <typename T>
int value(const T& item);
template <>
int value(const int& item) {
return item;
}
template <>
int value(int* const& item) {
return *item;
}
template <>
int value(const mtc::SolutionBasePtr& item) {
return item->cost();
}
template <>
int value(const mtc::SolutionBaseConstPtr& item) {
return item->cost();
}
template <typename T>
class ValueOrPointeeLessTest : public ::testing::Test {
class ValueOrPointeeLessTest : public ::testing::Test
{
protected:
bool less(int lhs, int rhs) {
ValueOrPointeeLess<T> comp;
@ -44,7 +67,8 @@ TYPED_TEST(ValueOrPointeeLessTest, less) {
}
template <typename T>
class OrderedTest : public ::testing::Test, public ordered<T> {
class OrderedTest : public ::testing::Test, public ordered<T>
{
protected:
void pushAndValidate(int cost, const std::vector<int>& expected) {
this->insert(create<T>(cost));
@ -61,7 +85,7 @@ protected:
std::transform(this->c.begin(), this->c.end(), std::back_inserter(expected),
[](const T& item) -> int { return value<T>(item); });
while(!this->empty())
while (!this->empty())
actual.push_back(value<T>(this->pop()));
EXPECT_THAT(actual, ::testing::ElementsAreArray(expected));
@ -69,53 +93,52 @@ protected:
}
};
#define pushAndValidate(cost, ...) { \
SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \
this->pushAndValidate(cost, __VA_ARGS__); \
}
#define pushAndValidate(cost, ...) \
{ \
SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \
this->pushAndValidate(cost, __VA_ARGS__); \
}
TYPED_TEST_CASE(OrderedTest, TypeInstances);
TYPED_TEST(OrderedTest, sorting) {
pushAndValidate(2, {2});
pushAndValidate(1, {1,2});
pushAndValidate(3, {1,2,3});
pushAndValidate(2, { 2 });
pushAndValidate(1, { 1, 2 });
pushAndValidate(3, { 1, 2, 3 });
this->validatePop();
pushAndValidate(1, {1});
pushAndValidate(2, {1,2});
pushAndValidate(3, {1,2,3});
pushAndValidate(4, {1,2,3,4});
pushAndValidate(5, {1,2,3,4,5});
pushAndValidate(1, { 1 });
pushAndValidate(2, { 1, 2 });
pushAndValidate(3, { 1, 2, 3 });
pushAndValidate(4, { 1, 2, 3, 4 });
pushAndValidate(5, { 1, 2, 3, 4, 5 });
this->validatePop();
pushAndValidate(5, {5});
pushAndValidate(4, {4,5});
pushAndValidate(3, {3,4,5});
pushAndValidate(1, {1,3,4,5});
pushAndValidate(2, {1,2,3,4,5});
pushAndValidate(5, { 5 });
pushAndValidate(4, { 4, 5 });
pushAndValidate(3, { 3, 4, 5 });
pushAndValidate(1, { 1, 3, 4, 5 });
pushAndValidate(2, { 1, 2, 3, 4, 5 });
this->validatePop();
}
template<typename ValueType, typename CostType>
std::ostream& operator<<(std::ostream &os, const cost_ordered<ValueType, CostType> &queue) {
for (const auto &pair : queue.sorted())
template <typename ValueType, typename CostType>
std::ostream& operator<<(std::ostream& os, const cost_ordered<ValueType, CostType>& queue) {
for (const auto& pair : queue.sorted())
os << pair.cost() << ": " << pair.value() << std::endl;
return os;
}
template <typename ValueType, typename CostType = int>
class CostOrderedTest : public ::testing::Test {
class CostOrderedTest : public ::testing::Test
{
protected:
typedef std::deque<ValueType> container_type;
typedef cost_ordered<ValueType, std::deque<ValueType>, CostType> queue_type;
CostOrderedTest() {}
auto insert(ValueType value, CostType cost = CostType()) {
return queue.insert(value, cost);
}
auto insert(ValueType value, CostType cost = CostType()) { return queue.insert(value, cost); }
void fill(int first=1, int last=5) {
void fill(int first = 1, int last = 5) {
for (; first < last; ++first)
insert(first, first);
}
@ -128,7 +151,7 @@ protected:
typedef CostOrderedTest<int, int> CostOrderedTestInt;
TEST_F(CostOrderedTestInt, ordered_push) {
auto top = *insert(2,2);
auto top = *insert(2, 2);
for (int i = 3; i < 6; ++i) {
insert(i, i);
EXPECT_EQ(queue.top(), top);

View File

@ -10,10 +10,10 @@ TEST(InterfaceStatePriority, compare) {
EXPECT_TRUE(Prio(0, 0) == Prio(0, 0));
EXPECT_TRUE(Prio(0, inf) == Prio(0, inf));
EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller
EXPECT_TRUE(Prio(1, 0) < Prio(0, 0)); // higher depth is smaller
EXPECT_TRUE(Prio(1, inf) < Prio(0, inf));
EXPECT_TRUE(Prio(0, 0) < Prio(0, 1)); // higher cost is larger
EXPECT_TRUE(Prio(0, 0) < Prio(0, 1)); // higher cost is larger
EXPECT_TRUE(Prio(0, 0) < Prio(0, inf));
EXPECT_TRUE(Prio(0, inf) > Prio(0, 0));
}

View File

@ -107,7 +107,8 @@ TEST(Property, serialize) {
EXPECT_EQ(props.property("map").serialize(), "");
}
class InitFromTest : public ::testing::Test {
class InitFromTest : public ::testing::Test
{
protected:
void SetUp() {
master.declare<double>("double1", 1);
@ -134,7 +135,7 @@ TEST_F(InitFromTest, standard) {
}
TEST_F(InitFromTest, limited) {
slave.configureInitFrom(1, {"double1"}); // limit init to listed props
slave.configureInitFrom(1, { "double1" }); // limit init to listed props
slave.performInitFrom(1, master);
EXPECT_EQ(slave.get<double>("double1"), 1.0);
EXPECT_FALSE(slave.property("double2").defined());
@ -144,7 +145,7 @@ TEST_F(InitFromTest, limited) {
TEST_F(InitFromTest, sourceId) {
slave.configureInitFrom(1); // init all matching vars
slave.performInitFrom(2, master); // init with wrong sourceId -> no effect
slave.performInitFrom(2, master); // init with wrong sourceId -> no effect
EXPECT_FALSE(slave.property("double1").defined());
EXPECT_FALSE(slave.property("double2").defined());
EXPECT_FALSE(slave.property("double3").defined());
@ -158,7 +159,7 @@ TEST_F(InitFromTest, multipleSourceIds) {
}
TEST_F(InitFromTest, otherName) {
slave.property("double1").configureInitFrom(1, "double2"); // init double1 from double2
slave.property("double1").configureInitFrom(1, "double2"); // init double1 from double2
slave.performInitFrom(1, master);
EXPECT_EQ(slave.get<double>("double1"), 2.0);
EXPECT_FALSE(slave.property("double2").defined());

View File

@ -12,7 +12,8 @@
using namespace moveit::task_constructor;
using namespace planning_scene;
class GeneratorMockup : public Generator {
class GeneratorMockup : public Generator
{
PlanningScenePtr ps;
InterfacePtr prev;
InterfacePtr next;
@ -25,7 +26,7 @@ public:
pimpl()->setNextStarts(next);
}
void init(const moveit::core::RobotModelConstPtr &robot_model) override {
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset((new PlanningScene(robot_model)));
}
@ -37,7 +38,8 @@ public:
}
};
class ConnectMockup : public Connecting {
class ConnectMockup : public Connecting
{
public:
using Connecting::compatible;
void compute(const InterfaceState& from, const InterfaceState& to) override {}
@ -50,7 +52,7 @@ TEST(Stage, registerCallbacks) {
g.init(getModel());
uint called = 0;
auto cb = [&called](const SolutionBase &s) {
auto cb = [&called](const SolutionBase& s) {
++called;
return true;
};
@ -98,19 +100,19 @@ TEST(ModifyPlanningScene, allowCollisions) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
auto s = std::make_unique<stages::ModifyPlanningScene>();
std::string first="foo", second="boom";
std::string first = "foo", second = "boom";
s->allowCollisions(first, second, true);
s->allowCollisions(first, std::vector<const char*>{"ab", "abc"}, false);
s->allowCollisions(first, std::vector<const char*>{ "ab", "abc" }, false);
s->allowCollisions("foo", std::vector<const char*>{"bar", "boom"}, true);
s->allowCollisions("foo", std::set<const char*>{"ab", "abc"}, false);
s->allowCollisions("foo", std::vector<const char*>{ "bar", "boom" }, true);
s->allowCollisions("foo", std::set<const char*>{ "ab", "abc" }, false);
}
void spawnObject(PlanningScene& scene, const std::string& name, int type,
const std::vector<double>& pos = {0,0,0}) {
const std::vector<double>& pos = { 0, 0, 0 }) {
moveit_msgs::CollisionObject o;
o.id= name;
o.header.frame_id= scene.getPlanningFrame();
o.id = name;
o.header.frame_id = scene.getPlanningFrame();
o.primitive_poses.resize(1);
o.primitive_poses[0].position.x = pos[0];
o.primitive_poses[0].position.y = pos[1];
@ -118,29 +120,27 @@ void spawnObject(PlanningScene& scene, const std::string& name, int type,
o.primitive_poses[0].orientation.w = 1.0;
o.primitives.resize(1);
o.primitives[0].type= type;
o.primitives[0].type = type;
switch (type) {
case shape_msgs::SolidPrimitive::CYLINDER:
o.primitives[0].dimensions = {0.1, 0.02};
break;
case shape_msgs::SolidPrimitive::BOX:
o.primitives[0].dimensions = {0.1, 0.2, 0.3};
break;
case shape_msgs::SolidPrimitive::SPHERE:
o.primitives[0].dimensions = {0.05};
break;
case shape_msgs::SolidPrimitive::CYLINDER:
o.primitives[0].dimensions = { 0.1, 0.02 };
break;
case shape_msgs::SolidPrimitive::BOX:
o.primitives[0].dimensions = { 0.1, 0.2, 0.3 };
break;
case shape_msgs::SolidPrimitive::SPHERE:
o.primitives[0].dimensions = { 0.05 };
break;
}
scene.processCollisionObjectMsg(o);
}
void attachObject(PlanningScene& scene,
const std::string& object, const std::string& link, bool attach)
{
void attachObject(PlanningScene& scene, const std::string& object, const std::string& link, bool attach) {
moveit_msgs::AttachedCollisionObject obj;
obj.link_name = link;
obj.object.operation = attach ? (int8_t) moveit_msgs::CollisionObject::ADD
: (int8_t) moveit_msgs::CollisionObject::REMOVE;
obj.object.operation =
attach ? (int8_t)moveit_msgs::CollisionObject::ADD : (int8_t)moveit_msgs::CollisionObject::REMOVE;
obj.object.id = object;
scene.processAttachedCollisionObjectMsg(obj);
}
@ -159,7 +159,7 @@ TEST(Connect, compatible) {
spawnObject(*other, "object", shape_msgs::SolidPrimitive::BOX);
// EXPECT_FALSE(connect.compatible(scene, other)) << "different shapes";
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, {0.1,0,0});
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 });
EXPECT_FALSE(connect.compatible(scene, other)) << "different pose";
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER);
@ -173,7 +173,7 @@ TEST(Connect, compatible) {
other = scene->diff();
EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes, attached object";
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, {0.1,0,0});
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 });
attachObject(*other, "object", "base_link", true);
EXPECT_FALSE(connect.compatible(scene, other)) << "different pose";
}

View File

@ -1,4 +1,4 @@
<package>
<package format="2">
<name>moveit_task_constructor_msgs</name>
<version>0.0.0</version>
<description>Messages for MoveIt Task Pipeline</description>
@ -9,11 +9,9 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>
<depend>message_generation</depend>
<depend>moveit_msgs</depend>
<depend>visualization_msgs</depend>
<run_depend>message_runtime</run_depend>
<run_depend>moveit_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>
<exec_depend>message_runtime</exec_depend>
</package>

View File

@ -4,30 +4,32 @@
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry>
namespace urdf { class Geometry; }
namespace urdf {
class Geometry;
}
namespace rviz_marker_tools {
enum Color
{
BLACK = 0,
BROWN = 1,
BLUE = 2,
CYAN = 3,
GREY = 4,
DARK_GREY = 5,
GREEN = 6,
LIME_GREEN = 7,
MAGENTA = 8,
ORANGE = 9,
PURPLE = 10,
RED = 11,
PINK = 12,
WHITE = 13,
YELLOW = 14,
BLACK = 0,
BROWN = 1,
BLUE = 2,
CYAN = 3,
GREY = 4,
DARK_GREY = 5,
GREEN = 6,
LIME_GREEN = 7,
MAGENTA = 8,
ORANGE = 9,
PURPLE = 10,
RED = 11,
PINK = 12,
WHITE = 13,
YELLOW = 14,
};
std_msgs::ColorRGBA getColor(Color color, double alpha=1.0);
std_msgs::ColorRGBA& setColor(std_msgs::ColorRGBA& color, Color color_id, double alpha=1.0);
std_msgs::ColorRGBA getColor(Color color, double alpha = 1.0);
std_msgs::ColorRGBA& setColor(std_msgs::ColorRGBA& color, Color color_id, double alpha = 1.0);
std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& other, double fraction);
std_msgs::ColorRGBA& brighten(std_msgs::ColorRGBA& color, double fraction);
@ -48,7 +50,7 @@ visualization_msgs::Marker& makeYZPlane(visualization_msgs::Marker& m);
/// create a cone of given angle along the x-axis
visualization_msgs::Marker& makeCone(visualization_msgs::Marker& m, double angle);
visualization_msgs::Marker& makeSphere(visualization_msgs::Marker &m, double radius = 1.0);
visualization_msgs::Marker& makeSphere(visualization_msgs::Marker& m, double radius = 1.0);
/// create a cylinder along z-axis
visualization_msgs::Marker& makeCylinder(visualization_msgs::Marker& m, double diameter, double height);
@ -57,36 +59,38 @@ visualization_msgs::Marker& makeCylinder(visualization_msgs::Marker& m, double d
visualization_msgs::Marker& makeBox(visualization_msgs::Marker& m, double x, double y, double z);
/// create a mesh marker
visualization_msgs::Marker& makeMesh(visualization_msgs::Marker& m, const std::string& filename, double sx = 1.0, double sy = 1.0, double sz = 1.0);
visualization_msgs::Marker& makeMesh(visualization_msgs::Marker& m, const std::string& filename, double sx = 1.0,
double sy = 1.0, double sz = 1.0);
inline visualization_msgs::Marker& makeMesh(visualization_msgs::Marker& m, const std::string& filename, double scale) {
return makeMesh(m, filename, scale, scale, scale);
}
/// create an arrow along x-axis
visualization_msgs::Marker& makeArrow(visualization_msgs::Marker &m, double scale = 1.0, bool tip_at_origin = false);
visualization_msgs::Marker& makeArrow(visualization_msgs::Marker& m, double scale = 1.0, bool tip_at_origin = false);
/// create text marker
visualization_msgs::Marker &makeText(visualization_msgs::Marker &m, const std::string &text);
visualization_msgs::Marker& makeText(visualization_msgs::Marker& m, const std::string& text);
/// create marker from urdf::Geom
visualization_msgs::Marker& makeFromGeometry(visualization_msgs::Marker &m, const urdf::Geometry& geom);
visualization_msgs::Marker& makeFromGeometry(visualization_msgs::Marker& m, const urdf::Geometry& geom);
template <typename T>
void appendFrame(T& container, const geometry_msgs::PoseStamped& pose, double scale=1.0,
const std::string& ns = "frame", double diameter_fraction=0.1) {
void appendFrame(T& container, const geometry_msgs::PoseStamped& pose, double scale = 1.0,
const std::string& ns = "frame", double diameter_fraction = 0.1) {
visualization_msgs::Marker m;
makeCylinder(m, scale*diameter_fraction, scale);
makeCylinder(m, scale * diameter_fraction, scale);
m.ns = ns;
m.header = pose.header;
// x-axis
m.pose = composePoses(pose.pose, Eigen::Translation3d(scale / 2.0, 0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY()));
m.pose = composePoses(pose.pose, Eigen::Translation3d(scale / 2.0, 0, 0) *
Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY()));
setColor(m.color, RED);
container.push_back(m);
// y-axis
m.pose = composePoses(pose.pose, Eigen::Translation3d(0, scale / 2.0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX()));
m.pose = composePoses(pose.pose, Eigen::Translation3d(0, scale / 2.0, 0) *
Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX()));
setColor(m.color, GREEN);
container.push_back(m);
@ -96,4 +100,4 @@ void appendFrame(T& container, const geometry_msgs::PoseStamped& pose, double sc
container.push_back(m);
}
} // namespace rviz_marker_tools
} // namespace rviz_marker_tools

View File

@ -1,4 +1,4 @@
<package>
<package format="2">
<name>rviz_marker_tools</name>
<version>0.0.0</version>
<description>Tools for marker creation / handling</description>
@ -8,13 +8,8 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>visualization_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rviz</build_depend>
<run_depend>visualization_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rviz</run_depend>
<depend>visualization_msgs</depend>
<depend>geometry_msgs</depend>
<depend>roscpp</depend>
<depend>rviz</depend>
</package>

View File

@ -7,99 +7,98 @@ namespace vm = visualization_msgs;
namespace rviz_marker_tools {
std_msgs::ColorRGBA &setColor(std_msgs::ColorRGBA &color, Color color_id, double alpha)
{
std_msgs::ColorRGBA& setColor(std_msgs::ColorRGBA& color, Color color_id, double alpha) {
switch (color_id) {
case RED:
color.r = 0.8;
color.g = 0.1;
color.b = 0.1;
color.a = alpha;
break;
case GREEN:
color.r = 0.1;
color.g = 0.8;
color.b = 0.1;
color.a = alpha;
break;
case BLUE:
color.r = 0.1;
color.g = 0.1;
color.b = 0.8;
color.a = alpha;
break;
case WHITE:
color.r = 1.0;
color.g = 1.0;
color.b = 1.0;
color.a = alpha;
break;
case GREY:
color.r = 0.9;
color.g = 0.9;
color.b = 0.9;
color.a = alpha;
break;
case DARK_GREY:
color.r = 0.6;
color.g = 0.6;
color.b = 0.6;
color.a = alpha;
break;
case BLACK:
color.r = 0.0;
color.g = 0.0;
color.b = 0.0;
color.a = alpha;
break;
case YELLOW:
color.r = 1.0;
color.g = 1.0;
color.b = 0.0;
color.a = alpha;
break;
case ORANGE:
color.r = 1.0;
color.g = 0.5;
color.b = 0.0;
color.a = alpha;
break;
case BROWN:
color.r = 0.597;
color.g = 0.296;
color.b = 0.0;
color.a = alpha;
break;
case PINK:
color.r = 1.0;
color.g = 0.4;
color.b = 1;
color.a = alpha;
break;
case LIME_GREEN:
color.r = 0.6;
color.g = 1.0;
color.b = 0.2;
color.a = alpha;
break;
case PURPLE:
color.r = 0.597;
color.g = 0.0;
color.b = 0.597;
color.a = alpha;
break;
case CYAN:
color.r = 0.0;
color.g = 1.0;
color.b = 1.0;
color.a = alpha;
break;
case MAGENTA:
color.r = 1.0;
color.g = 0.0;
color.b = 1.0;
color.a = alpha;
break;
case RED:
color.r = 0.8;
color.g = 0.1;
color.b = 0.1;
color.a = alpha;
break;
case GREEN:
color.r = 0.1;
color.g = 0.8;
color.b = 0.1;
color.a = alpha;
break;
case BLUE:
color.r = 0.1;
color.g = 0.1;
color.b = 0.8;
color.a = alpha;
break;
case WHITE:
color.r = 1.0;
color.g = 1.0;
color.b = 1.0;
color.a = alpha;
break;
case GREY:
color.r = 0.9;
color.g = 0.9;
color.b = 0.9;
color.a = alpha;
break;
case DARK_GREY:
color.r = 0.6;
color.g = 0.6;
color.b = 0.6;
color.a = alpha;
break;
case BLACK:
color.r = 0.0;
color.g = 0.0;
color.b = 0.0;
color.a = alpha;
break;
case YELLOW:
color.r = 1.0;
color.g = 1.0;
color.b = 0.0;
color.a = alpha;
break;
case ORANGE:
color.r = 1.0;
color.g = 0.5;
color.b = 0.0;
color.a = alpha;
break;
case BROWN:
color.r = 0.597;
color.g = 0.296;
color.b = 0.0;
color.a = alpha;
break;
case PINK:
color.r = 1.0;
color.g = 0.4;
color.b = 1;
color.a = alpha;
break;
case LIME_GREEN:
color.r = 0.6;
color.g = 1.0;
color.b = 0.2;
color.a = alpha;
break;
case PURPLE:
color.r = 0.597;
color.g = 0.0;
color.b = 0.597;
color.a = alpha;
break;
case CYAN:
color.r = 0.0;
color.g = 1.0;
color.b = 1.0;
color.a = alpha;
break;
case MAGENTA:
color.r = 1.0;
color.g = 0.0;
color.b = 1.0;
color.a = alpha;
break;
}
return color;
}
@ -110,8 +109,10 @@ double interpolate(double start, double end, double fraction) {
}
std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& other, double fraction) {
if (fraction < 0.0) fraction = 0.0;
if (fraction > 1.0) fraction = 1.0;
if (fraction < 0.0)
fraction = 0.0;
if (fraction > 1.0)
fraction = 1.0;
color.r = interpolate(color.r, other.r, fraction);
color.g = interpolate(color.g, other.g, fraction);
color.b = interpolate(color.b, other.b, fraction);
@ -121,7 +122,8 @@ std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::Col
std_msgs::ColorRGBA& brighten(std_msgs::ColorRGBA& color, double fraction) {
static std_msgs::ColorRGBA white;
if (white.r == 0.0) setColor(white, WHITE);
if (white.r == 0.0)
setColor(white, WHITE);
return interpolate(color, white, fraction);
}
@ -274,7 +276,7 @@ vm::Marker& makeMesh(vm::Marker& m, const std::string& filename, double sx, doub
return m;
}
vm::Marker& makeArrow(vm::Marker &m, double scale, bool tip_at_origin) {
vm::Marker& makeArrow(vm::Marker& m, double scale, bool tip_at_origin) {
m.scale.y = m.scale.z = 0.1 * scale;
m.scale.x = scale;
prepareMarker(m, vm::Marker::ARROW);
@ -283,45 +285,40 @@ vm::Marker& makeArrow(vm::Marker &m, double scale, bool tip_at_origin) {
return m;
}
vm::Marker& makeText(vm::Marker& m, const std::string &text)
{
vm::Marker& makeText(vm::Marker& m, const std::string& text) {
prepareMarker(m, vm::Marker::TEXT_VIEW_FACING);
m.text = text;
return m;
}
vm::Marker& makeFromGeometry(vm::Marker &m, const urdf::Geometry& geom) {
vm::Marker& makeFromGeometry(vm::Marker& m, const urdf::Geometry& geom) {
switch (geom.type) {
case urdf::Geometry::SPHERE:
{
const urdf::Sphere& sphere = static_cast<const urdf::Sphere&>(geom);
makeSphere(m, sphere.radius);
break;
}
case urdf::Geometry::BOX:
{
const urdf::Box& box = static_cast<const urdf::Box&>(geom);
makeBox(m, box.dim.x, box.dim.y, box.dim.z);
break;
}
case urdf::Geometry::CYLINDER:
{
const urdf::Cylinder& cylinder = static_cast<const urdf::Cylinder&>(geom);
makeCylinder(m, 2.0*cylinder.radius, cylinder.length);
break;
}
case urdf::Geometry::MESH:
{
const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
makeMesh(m, mesh.filename, mesh.scale.x, mesh.scale.y, mesh.scale.z);
break;
}
default:
ROS_WARN("Unsupported geometry type: %d", geom.type);
break;
case urdf::Geometry::SPHERE: {
const urdf::Sphere& sphere = static_cast<const urdf::Sphere&>(geom);
makeSphere(m, sphere.radius);
break;
}
case urdf::Geometry::BOX: {
const urdf::Box& box = static_cast<const urdf::Box&>(geom);
makeBox(m, box.dim.x, box.dim.y, box.dim.z);
break;
}
case urdf::Geometry::CYLINDER: {
const urdf::Cylinder& cylinder = static_cast<const urdf::Cylinder&>(geom);
makeCylinder(m, 2.0 * cylinder.radius, cylinder.length);
break;
}
case urdf::Geometry::MESH: {
const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
makeMesh(m, mesh.filename, mesh.scale.x, mesh.scale.y, mesh.scale.z);
break;
}
default:
ROS_WARN("Unsupported geometry type: %d", geom.type);
break;
}
return m;
}
} // namespace rviz_marker_tools
} // namespace rviz_marker_tools

View File

@ -4,7 +4,7 @@ set(SOURCES
property_factory.cpp
)
include(FindPkgConfig)
find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML yaml-0.1)
if (YAML_FOUND)
# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually

View File

@ -49,58 +49,52 @@ namespace mtc = ::moveit::task_constructor;
namespace moveit_rviz_plugin {
static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop,
const planning_scene::PlanningScene*,
rviz::DisplayContext*) {
const planning_scene::PlanningScene*, rviz::DisplayContext*) {
std::string value;
if (!mtc_prop.value().empty())
value = boost::any_cast<std::string>(mtc_prop.value());
rviz::StringProperty* rviz_prop = new rviz::StringProperty(name, QString::fromStdString(value),
QString::fromStdString(mtc_prop.description()));
rviz::StringProperty* rviz_prop =
new rviz::StringProperty(name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description()));
QObject::connect(rviz_prop, &rviz::StringProperty::changed,
[rviz_prop, &mtc_prop]() {mtc_prop.setValue(rviz_prop->getStdString());});
[rviz_prop, &mtc_prop]() { mtc_prop.setValue(rviz_prop->getStdString()); });
return rviz_prop;
}
template <typename T>
static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop,
const planning_scene::PlanningScene*,
rviz::DisplayContext*) {
const planning_scene::PlanningScene*, rviz::DisplayContext*) {
T value = !mtc_prop.value().empty() ? boost::any_cast<T>(mtc_prop.value()) : T();
rviz::FloatProperty* rviz_prop = new rviz::FloatProperty(name, value, QString::fromStdString(mtc_prop.description()));
rviz::FloatProperty* rviz_prop =
new rviz::FloatProperty(name, value, QString::fromStdString(mtc_prop.description()));
QObject::connect(rviz_prop, &rviz::FloatProperty::changed,
[rviz_prop, &mtc_prop]() {mtc_prop.setValue(T(rviz_prop->getFloat()));});
[rviz_prop, &mtc_prop]() { mtc_prop.setValue(T(rviz_prop->getFloat())); });
return rviz_prop;
}
PropertyFactory::PropertyFactory()
{
PropertyFactory::PropertyFactory() {
// register some standard types
registerType<float>(&floatFactory<float>);
registerType<double>(&floatFactory<double>);
registerType<std::string>(&stringFactory);
}
PropertyFactory& PropertyFactory::instance()
{
PropertyFactory& PropertyFactory::instance() {
static PropertyFactory instance_;
return instance_;
}
void PropertyFactory::registerType(const std::string &type_name, const PropertyFactoryFunction &f)
{
void PropertyFactory::registerType(const std::string& type_name, const PropertyFactoryFunction& f) {
if (type_name.empty())
return;
property_registry_.insert(std::make_pair(type_name, f));
}
void PropertyFactory::registerStage(const std::type_index &type_index, const PropertyFactory::TreeFactoryFunction &f)
{
void PropertyFactory::registerStage(const std::type_index& type_index, const PropertyFactory::TreeFactoryFunction& f) {
stage_registry_.insert(std::make_pair(type_index, f));
}
rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context) const
{
rviz::DisplayContext* display_context) const {
auto it = property_registry_.find(prop.typeName());
if (it == property_registry_.end())
return createDefault(prop_name, prop.typeName(), prop.description(), prop.serialize());
@ -109,8 +103,7 @@ rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Prope
rviz::PropertyTreeModel* PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context)
{
rviz::DisplayContext* display_context) {
auto it = stage_registry_.find(typeid(stage));
if (it == stage_registry_.end())
return defaultPropertyTreeModel(stage.properties(), scene, display_context);
@ -142,7 +135,8 @@ void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::Property
continue;
rviz::Property* rviz_prop = create(prop.first, prop.second, scene, display_context);
if (!rviz_prop) rviz_prop = new rviz::Property(name);
if (!rviz_prop)
rviz_prop = new rviz::Property(name);
root->addChild(rviz_prop);
}
@ -154,21 +148,18 @@ void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::Property
#ifndef HAVE_YAML
rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type,
const std::string& description, const std::string& value,
rviz::Property* old)
{
rviz::Property* old) {
if (old) { // reuse existing Property?
assert(old->getNameStd() == name);
old->setDescription(QString::fromStdString(description));
old->setValue(QString::fromStdString(value));
return old;
} else { // create new Property?
rviz::Property *result = new rviz::StringProperty(QString::fromStdString(name),
QString::fromStdString(value),
rviz::Property* result = new rviz::StringProperty(QString::fromStdString(name), QString::fromStdString(value),
QString::fromStdString(description));
result->setReadOnly(true);
return result;
}
}
#endif
}

View File

@ -52,9 +52,11 @@ class DisplayContext;
namespace planning_scene {
class PlanningScene;
}
namespace moveit { namespace task_constructor {
namespace moveit {
namespace task_constructor {
class Stage;
} }
}
}
namespace moveit_rviz_plugin {
@ -75,10 +77,12 @@ public:
typedef std::function<rviz::Property*(const QString& name, moveit::task_constructor::Property&,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context)> PropertyFactoryFunction;
rviz::DisplayContext* display_context)>
PropertyFactoryFunction;
typedef std::function<rviz::PropertyTreeModel*(moveit::task_constructor::PropertyMap&,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context)> TreeFactoryFunction;
rviz::DisplayContext* display_context)>
TreeFactoryFunction;
/// register a factory function for type T
template <typename T>
@ -89,30 +93,31 @@ public:
/// register a factory function for stage T
template <typename T>
inline void registerStage(const TreeFactoryFunction& f) { registerStage(typeid(T), f); }
inline void registerStage(const TreeFactoryFunction& f) {
registerStage(typeid(T), f);
}
/// create rviz::Property for given MTC Property
rviz::Property* create(const std::string &prop_name, moveit::task_constructor::Property &prop,
const planning_scene::PlanningScene *scene, rviz::DisplayContext *display_context) const;
rviz::Property* create(const std::string& prop_name, moveit::task_constructor::Property& prop,
const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context) const;
/// create rviz::Property for property of given name, type, description, and value
static rviz::Property* createDefault(const std::string& name, const std::string& type,
const std::string& description, const std::string& value,
rviz::Property* old=nullptr);
rviz::Property* old = nullptr);
/// create PropertyTreeModel for given Stage
rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage &stage,
rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage& stage,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context);
/// turn a PropertyMap into an rviz::PropertyTreeModel
rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap &properties,
rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap& properties,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context);
/// add all properties from map that are not yet in root
void addRemainingProperties(rviz::Property *root, moveit::task_constructor::PropertyMap &properties,
const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context);
void addRemainingProperties(rviz::Property* root, moveit::task_constructor::PropertyMap& properties,
const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context);
private:
std::map<std::string, PropertyFactoryFunction> property_registry_;
@ -126,5 +131,4 @@ private:
void registerType(const std::string& type_name, const PropertyFactoryFunction& f);
void registerStage(const std::type_index& type_index, const TreeFactoryFunction& f);
};
}

View File

@ -51,16 +51,17 @@ namespace {
// Event-based YAML parser, creating an rviz::Property tree
// https://www.wpsoftware.net/andrew/pages/libyaml.html
class Parser {
class Parser
{
public:
static const int YAML_ERROR_EVENT = 255;
Parser(const std::string& value);
~Parser();
rviz::Property* process(const QString& name, const QString& description, rviz::Property *old) const;
static rviz::Property* createScalar(const QString& name, const QString& description,
const QByteArray& value, rviz::Property *old);
rviz::Property* process(const QString& name, const QString& description, rviz::Property* old) const;
static rviz::Property* createScalar(const QString& name, const QString& description, const QByteArray& value,
rviz::Property* old);
private:
// return true if there was no error so far
@ -68,25 +69,25 @@ private:
// parse a single event and return it's type, YAML_ERROR_EVENT on parsing error
int parse(yaml_event_t& event) const;
// process events: scalar, start mapping, start sequence
rviz::Property *process(const yaml_event_t &event, const QString &name, const QString &description, rviz::Property *old) const;
rviz::Property* process(const yaml_event_t& event, const QString& name, const QString& description,
rviz::Property* old) const;
inline static QByteArray byteArray(const yaml_event_t& event) {
assert(event.type == YAML_SCALAR_EVENT);
return QByteArray::fromRawData(reinterpret_cast<const char*>(event.data.scalar.value),
event.data.scalar.length);
return QByteArray::fromRawData(reinterpret_cast<const char*>(event.data.scalar.value), event.data.scalar.length);
}
// Try to set value of existing rviz::Property (expecting matching types). Return false on error.
static bool setValue(rviz::Property *old, const QByteArray &value);
static bool setValue(rviz::Property* old, const QByteArray& value);
static rviz::Property *createParent(const QString &name, const QString &description, rviz::Property *old);
rviz::Property* processMapping(const QString& name, const QString& description, rviz::Property *old) const;
rviz::Property* processSequence(const QString& name, const QString& description, rviz::Property *old) const;
static rviz::Property* createParent(const QString& name, const QString& description, rviz::Property* old);
rviz::Property* processMapping(const QString& name, const QString& description, rviz::Property* old) const;
rviz::Property* processSequence(const QString& name, const QString& description, rviz::Property* old) const;
private:
mutable yaml_parser_t parser_;
};
Parser::Parser(const std::string &value) {
Parser::Parser(const std::string& value) {
yaml_parser_initialize(&parser_);
yaml_parser_set_input_string(&parser_, reinterpret_cast<const yaml_char_t*>(value.c_str()), value.size());
}
@ -95,7 +96,7 @@ Parser::~Parser() {
yaml_parser_delete(&parser_);
}
int Parser::parse(yaml_event_t &event) const {
int Parser::parse(yaml_event_t& event) const {
if (!yaml_parser_parse(&parser_, &event)) {
yaml_event_delete(&event);
return YAML_ERROR_EVENT;
@ -104,22 +105,24 @@ int Parser::parse(yaml_event_t &event) const {
}
// main processing function
rviz::Property *Parser::process(const QString &name, const QString &description, rviz::Property *old) const {
rviz::Property* Parser::process(const QString& name, const QString& description, rviz::Property* old) const {
bool stop = false;
while (!stop) {
yaml_event_t event;
switch (parse(event)) {
case YAML_ERROR_EVENT: return Parser::createScalar(name, description, "YAML error", old);
case YAML_STREAM_END_EVENT:
stop = true;
break;
case YAML_ERROR_EVENT:
return Parser::createScalar(name, description, "YAML error", old);
case YAML_STREAM_END_EVENT:
stop = true;
break;
case YAML_SEQUENCE_START_EVENT:
case YAML_MAPPING_START_EVENT:
case YAML_SCALAR_EVENT:
return process(event, name, description, old);
case YAML_SEQUENCE_START_EVENT:
case YAML_MAPPING_START_EVENT:
case YAML_SCALAR_EVENT:
return process(event, name, description, old);
default: break;
default:
break;
}
}
// if we get here, there was no content in the yaml stream
@ -127,24 +130,28 @@ rviz::Property *Parser::process(const QString &name, const QString &description,
}
// default processing for scalar, start mapping, start sequence events
rviz::Property* Parser::process(const yaml_event_t& event,
const QString& name, const QString& description, rviz::Property *old) const {
rviz::Property* Parser::process(const yaml_event_t& event, const QString& name, const QString& description,
rviz::Property* old) const {
switch (event.type) {
case YAML_SEQUENCE_START_EVENT: return processSequence(name, description, old);
case YAML_MAPPING_START_EVENT: return processMapping(name, description, old);
case YAML_SCALAR_EVENT: return createScalar(name, description, byteArray(event), old);
default: break;
case YAML_SEQUENCE_START_EVENT:
return processSequence(name, description, old);
case YAML_MAPPING_START_EVENT:
return processMapping(name, description, old);
case YAML_SCALAR_EVENT:
return createScalar(name, description, byteArray(event), old);
default:
break;
}
assert(false); // should not be reached
}
// Try to set numeric or arbitrary scalar value from YAML node. Needs to match old's type.
bool Parser::setValue(rviz::Property* old, const QByteArray& value)
{
bool Parser::setValue(rviz::Property* old, const QByteArray& value) {
if (rviz::FloatProperty* p = dynamic_cast<rviz::FloatProperty*>(old)) {
bool ok = true;
double v = value.toDouble(&ok);
if (ok) p->setValue(v);
if (ok)
p->setValue(v);
return ok;
}
if (rviz::StringProperty* p = dynamic_cast<rviz::StringProperty*>(old)) {
@ -156,9 +163,8 @@ bool Parser::setValue(rviz::Property* old, const QByteArray& value)
}
// Update existing old rviz:Property or create a new one from scalar YAML node
rviz::Property *Parser::createScalar(const QString &name, const QString &description,
const QByteArray &value, rviz::Property *old)
{
rviz::Property* Parser::createScalar(const QString& name, const QString& description, const QByteArray& value,
rviz::Property* old) {
// try to update value, expecting matching rviz::Property
if (old && setValue(old, value)) {
// only if setValue succeeded, also update the rest
@ -169,9 +175,9 @@ rviz::Property *Parser::createScalar(const QString &name, const QString &descrip
bool ok = true;
double v = value.toDouble(&ok);
if (ok) // if value is a number, create a FloatProperty
if (ok) // if value is a number, create a FloatProperty
old = new rviz::FloatProperty(name, v, description);
else // otherwise create a StringProperty
else // otherwise create a StringProperty
old = new rviz::StringProperty(name, value, description);
old->setReadOnly(true);
@ -179,11 +185,9 @@ rviz::Property *Parser::createScalar(const QString &name, const QString &descrip
}
// Reuse old property (or create new one) as parent for a sequence or map
rviz::Property* Parser::createParent(const QString& name, const QString& description, rviz::Property* old)
{
rviz::Property* Parser::createParent(const QString& name, const QString& description, rviz::Property* old) {
// don't reuse float or string properties (they are for scalars)
if (dynamic_cast<rviz::FloatProperty*>(old) ||
dynamic_cast<rviz::StringProperty*>(old))
if (dynamic_cast<rviz::FloatProperty*>(old) || dynamic_cast<rviz::StringProperty*>(old))
old = nullptr;
if (!old) {
old = new rviz::Property(name, QVariant(), description);
@ -196,112 +200,107 @@ rviz::Property* Parser::createParent(const QString& name, const QString& descrip
}
// Hierarchically create property from YAML map node
rviz::Property* Parser::processMapping(const QString& name, const QString& description, rviz::Property* root) const
{
rviz::Property* Parser::processMapping(const QString& name, const QString& description, rviz::Property* root) const {
root = createParent(name, description, root);
int index = 0; // current child index in root
bool stop = false;
while (!stop && noError()) { // parse all map items
yaml_event_t event;
switch (parse(event)) { // parse key
case YAML_MAPPING_END_EVENT: // all fine, reached end of mapping
stop = true;
break;
case YAML_SCALAR_EVENT: { // key
QByteArray key = byteArray(event);
int num = root->numChildren();
// find first child with name >= it->name
int next = index;
while (next < num && root->childAt(next)->getName() < key)
++next;
// and remove all children in range [index, next) at once
root->removeChildren(index, next-index);
num = root->numChildren();
// if names differ, insert a new child, otherwise reuse existing
rviz::Property *old_child = index < num ? root->childAt(index) : nullptr;
if (old_child && old_child->getName() != key)
old_child = nullptr;
rviz::Property *new_child = nullptr;
switch (parse(event)) { // parse value
case YAML_MAPPING_START_EVENT:
case YAML_SEQUENCE_START_EVENT:
case YAML_SCALAR_EVENT:
new_child = process(event, key, "", old_child);
case YAML_MAPPING_END_EVENT: // all fine, reached end of mapping
stop = true;
break;
default: // all other events are an error
new_child = createScalar(key, "", parser_.problem, old_child);
root->setValue("YAML error");
case YAML_SCALAR_EVENT: { // key
QByteArray key = byteArray(event);
int num = root->numChildren();
// find first child with name >= it->name
int next = index;
while (next < num && root->childAt(next)->getName() < key)
++next;
// and remove all children in range [index, next) at once
root->removeChildren(index, next - index);
num = root->numChildren();
// if names differ, insert a new child, otherwise reuse existing
rviz::Property* old_child = index < num ? root->childAt(index) : nullptr;
if (old_child && old_child->getName() != key)
old_child = nullptr;
rviz::Property* new_child = nullptr;
switch (parse(event)) { // parse value
case YAML_MAPPING_START_EVENT:
case YAML_SEQUENCE_START_EVENT:
case YAML_SCALAR_EVENT:
new_child = process(event, key, "", old_child);
break;
default: // all other events are an error
new_child = createScalar(key, "", parser_.problem, old_child);
root->setValue("YAML error");
break;
}
if (new_child != old_child)
root->addChild(new_child, index);
++index;
break;
}
if (new_child != old_child)
root->addChild(new_child, index);
++index;
break;
}
default: // unexpected event
root->setValue("YAML error");
stop = true;
break;
default: // unexpected event
root->setValue("YAML error");
stop = true;
break;
}
}
// remove remaining children
root->removeChildren(index, root->numChildren()-index);
root->removeChildren(index, root->numChildren() - index);
return root;
}
// Hierarchically create property from YAML sequence node. Items are named [#].
rviz::Property* Parser::processSequence(const QString& name, const QString& description, rviz::Property* root) const
{
rviz::Property* Parser::processSequence(const QString& name, const QString& description, rviz::Property* root) const {
root = createParent(name, description, root);
int index = 0; // current child index in root
bool stop = false;
while (!stop && noError()) { // parse all map items
yaml_event_t event;
switch (parse(event)) {
case YAML_SEQUENCE_END_EVENT: // all fine, reached end of sequence
stop = true;
break;
case YAML_SEQUENCE_END_EVENT: // all fine, reached end of sequence
stop = true;
break;
case YAML_MAPPING_START_EVENT:
case YAML_SEQUENCE_START_EVENT:
case YAML_SCALAR_EVENT: {
rviz::Property *old_child = root->childAt(index); // nullptr for invalid index
rviz::Property *new_child = process(event, QString("[%1]").arg(index), "", old_child);
if (new_child != old_child)
root->addChild(new_child, index);
if (++index >= 10)
stop = true; // limit number of shown entries
break;
}
case YAML_MAPPING_START_EVENT:
case YAML_SEQUENCE_START_EVENT:
case YAML_SCALAR_EVENT: {
rviz::Property* old_child = root->childAt(index); // nullptr for invalid index
rviz::Property* new_child = process(event, QString("[%1]").arg(index), "", old_child);
if (new_child != old_child)
root->addChild(new_child, index);
if (++index >= 10)
stop = true; // limit number of shown entries
break;
}
default: // unexpected event
root->setValue("YAML error");
stop = true;
break;
default: // unexpected event
root->setValue("YAML error");
stop = true;
break;
}
}
// remove remaining children
root->removeChildren(index, root->numChildren()-index);
root->removeChildren(index, root->numChildren() - index);
return root;
}
}
namespace moveit_rviz_plugin {
rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type,
const std::string& description, const std::string& value,
rviz::Property* old)
{
rviz::Property* old) {
QString qname = QString::fromStdString(name);
QString qdesc = QString::fromStdString(description);
Parser parser(value);
return parser.process(qname, qdesc, old);
}
}

View File

@ -41,17 +41,14 @@
namespace moveit_rviz_plugin {
FactoryModel::FactoryModel(rviz::Factory &factory, const QString& mime_type, QObject *parent)
: QStandardItemModel(parent)
, mime_type_(mime_type)
{
setHorizontalHeaderLabels({tr("Name")});
FactoryModel::FactoryModel(rviz::Factory& factory, const QString& mime_type, QObject* parent)
: QStandardItemModel(parent), mime_type_(mime_type) {
setHorizontalHeaderLabels({ tr("Name") });
fillTree(factory);
}
void FactoryModel::fillTree(rviz::Factory &factory)
{
QIcon default_package_icon = rviz::loadPixmap( "package://rviz/icons/default_package_icon.png" );
void FactoryModel::fillTree(rviz::Factory& factory) {
QIcon default_package_icon = rviz::loadPixmap("package://rviz/icons/default_package_icon.png");
QStringList classes = factory.getDeclaredClassIds();
classes.sort();
@ -59,24 +56,19 @@ void FactoryModel::fillTree(rviz::Factory &factory)
// Map from package names to the corresponding top-level tree widget items.
std::map<QString, QStandardItem*> package_items;
for(const QString& lookup_name : classes)
{
for (const QString& lookup_name : classes) {
QString package = factory.getClassPackage(lookup_name);
QStandardItem* package_item;
auto mi = package_items.find(package);
if(mi == package_items.end())
{
if (mi == package_items.end()) {
package_item = new QStandardItem(default_package_icon, package);
package_items[package] = package_item;
appendRow(package_item);
}
else
{
} else {
package_item = mi->second;
}
QStandardItem* class_item = new QStandardItem(factory.getIcon(lookup_name),
factory.getClassName(lookup_name));
QStandardItem* class_item = new QStandardItem(factory.getIcon(lookup_name), factory.getClassName(lookup_name));
class_item->setWhatsThis(factory.getClassDescription(lookup_name));
class_item->setData(lookup_name, Qt::UserRole);
class_item->setDragEnabled(true);
@ -84,16 +76,14 @@ void FactoryModel::fillTree(rviz::Factory &factory)
}
}
QStringList FactoryModel::mimeTypes() const
{
QStringList FactoryModel::mimeTypes() const {
return { mime_type_ };
}
QMimeData *FactoryModel::mimeData(const QModelIndexList &indexes) const
{
QMimeData* FactoryModel::mimeData(const QModelIndexList& indexes) const {
QSet<int> rows_considered;
QMimeData* mime_data = new QMimeData();
for (const auto &index : indexes) {
for (const auto& index : indexes) {
if (rows_considered.contains(index.row()))
continue;
// mime data is lookup_name
@ -101,5 +91,4 @@ QMimeData *FactoryModel::mimeData(const QModelIndexList &indexes) const
}
return mime_data;
}
}

View File

@ -50,10 +50,9 @@ class FactoryModel : public QStandardItemModel
void fillTree(rviz::Factory& factory);
public:
FactoryModel(rviz::Factory& factory, const QString &mime_type, QObject *parent = nullptr);
FactoryModel(rviz::Factory& factory, const QString& mime_type, QObject* parent = nullptr);
QStringList mimeTypes() const override;
QMimeData* mimeData(const QModelIndexList &indexes) const override;
QMimeData* mimeData(const QModelIndexList& indexes) const override;
};
}

View File

@ -3,17 +3,13 @@
using namespace moveit_rviz_plugin::utils;
namespace moveit_rviz_plugin { namespace icons {
namespace moveit_rviz_plugin {
namespace icons {
const Icon CONNECT({
{QLatin1String(":icons/connectarrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint);
const Icon FORWARD({
{QLatin1String(":icons/downarrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint);
const Icon BACKWARD({
{QLatin1String(":icons/uparrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint);
const Icon BOTHWAY({
{QLatin1String(":icons/updownarrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint);
const Icon GENERATE({
{QLatin1String(":icons/generatearrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint);
} }
const Icon CONNECT({ { QLatin1String(":icons/connectarrow.png"), QColor::fromRgba(0xff000000) } }, Icon::Tint);
const Icon FORWARD({ { QLatin1String(":icons/downarrow.png"), QColor::fromRgba(0xff000000) } }, Icon::Tint);
const Icon BACKWARD({ { QLatin1String(":icons/uparrow.png"), QColor::fromRgba(0xff000000) } }, Icon::Tint);
const Icon BOTHWAY({ { QLatin1String(":icons/updownarrow.png"), QColor::fromRgba(0xff000000) } }, Icon::Tint);
const Icon GENERATE({ { QLatin1String(":icons/generatearrow.png"), QColor::fromRgba(0xff000000) } }, Icon::Tint);
}
}

View File

@ -2,12 +2,13 @@
#include <utils/icon.h>
namespace moveit_rviz_plugin { namespace icons {
namespace moveit_rviz_plugin {
namespace icons {
extern const moveit_rviz_plugin::utils::Icon CONNECT;
extern const moveit_rviz_plugin::utils::Icon FORWARD;
extern const moveit_rviz_plugin::utils::Icon BACKWARD;
extern const moveit_rviz_plugin::utils::Icon BOTHWAY;
extern const moveit_rviz_plugin::utils::Icon GENERATE;
} }
}
}

View File

@ -37,55 +37,45 @@
#include "job_queue.h"
#include <ros/console.h>
namespace moveit { namespace tools {
namespace moveit {
namespace tools {
JobQueue::JobQueue(QObject *parent) : QObject(parent)
{
}
JobQueue::JobQueue(QObject* parent) : QObject(parent) {}
void JobQueue::addJob(const std::function<void()>& job)
{
void JobQueue::addJob(const std::function<void()>& job) {
boost::unique_lock<boost::mutex> ulock(jobs_mutex_);
jobs_.push_back(job);
}
void JobQueue::clear()
{
void JobQueue::clear() {
jobs_.clear();
}
size_t JobQueue::numPending()
{
size_t JobQueue::numPending() {
boost::unique_lock<boost::mutex> ulock(jobs_mutex_);
return jobs_.size();
}
void JobQueue::waitForAllJobs()
{
void JobQueue::waitForAllJobs() {
boost::unique_lock<boost::mutex> ulock(jobs_mutex_);
while (!jobs_.empty())
idle_condition_.wait(ulock);
}
void JobQueue::executeJobs()
{
void JobQueue::executeJobs() {
boost::unique_lock<boost::mutex> ulock(jobs_mutex_);
while (!jobs_.empty())
{
while (!jobs_.empty()) {
std::function<void()> fn = jobs_.front();
jobs_.pop_front();
ulock.unlock();
try
{
try {
fn();
}
catch (std::exception& ex)
{
} catch (std::exception& ex) {
ROS_ERROR("Exception caught executing main loop job: %s", ex.what());
}
ulock.lock();
}
idle_condition_.notify_all();
}
} }
}
}

View File

@ -43,7 +43,8 @@
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>
namespace moveit { namespace tools {
namespace moveit {
namespace tools {
/** Job Queue (of std::functions) */
class JobQueue : public QObject
@ -54,13 +55,13 @@ class JobQueue : public QObject
boost::condition_variable idle_condition_;
public:
explicit JobQueue(QObject *parent = 0);
void addJob(const std::function<void()> &job);
explicit JobQueue(QObject* parent = 0);
void addJob(const std::function<void()>& job);
void clear();
size_t numPending();
void waitForAllJobs();
void executeJobs();
};
} }
}
}

View File

@ -49,8 +49,7 @@ using namespace moveit::task_constructor;
namespace moveit_rviz_plugin {
// return Node* corresponding to index
LocalTaskModel::Node* LocalTaskModel::node(const QModelIndex &index) const
{
LocalTaskModel::Node* LocalTaskModel::node(const QModelIndex& index) const {
if (!index.isValid())
return root_;
@ -60,16 +59,15 @@ LocalTaskModel::Node* LocalTaskModel::node(const QModelIndex &index) const
}
// return QModelIndex corresponding to Node*
QModelIndex LocalTaskModel::index(Node *n) const
{
QModelIndex LocalTaskModel::index(Node* n) const {
if (n == root_)
return QModelIndex();
const ContainerBasePrivate *parent = n->parent()->pimpl();
const ContainerBasePrivate* parent = n->parent()->pimpl();
// the internal pointer refers to n
int row = 0;
for (const auto& child : parent->children()) {
for (const auto& child : parent->children()) {
if (child->pimpl() == n)
return createIndex(row, 0, n);
++row;
@ -78,33 +76,30 @@ QModelIndex LocalTaskModel::index(Node *n) const
return QModelIndex();
}
LocalTaskModel::LocalTaskModel(ContainerBase::pointer &&container, const planning_scene::PlanningSceneConstPtr& scene,
LocalTaskModel::LocalTaskModel(ContainerBase::pointer&& container, const planning_scene::PlanningSceneConstPtr& scene,
rviz::DisplayContext* display_context, QObject* parent)
: BaseTaskModel(scene, display_context, parent)
, Task("", std::move(container))
{
: BaseTaskModel(scene, display_context, parent), Task("", std::move(container)) {
root_ = pimpl();
flags_ |= LOCAL_MODEL;
}
int LocalTaskModel::rowCount(const QModelIndex &parent) const
{
int LocalTaskModel::rowCount(const QModelIndex& parent) const {
if (parent.column() > 0)
return 0;
ContainerBasePrivate *c = dynamic_cast<ContainerBasePrivate*>(node(parent));
if (!c) return 0;
ContainerBasePrivate* c = dynamic_cast<ContainerBasePrivate*>(node(parent));
if (!c)
return 0;
return c->children().size();
}
QModelIndex LocalTaskModel::index(int row, int column, const QModelIndex &parent) const
{
QModelIndex LocalTaskModel::index(int row, int column, const QModelIndex& parent) const {
if (column < 0 || column >= columnCount())
return QModelIndex();
Q_ASSERT(dynamic_cast<ContainerBasePrivate*>(node(parent)));
ContainerBasePrivate *p = static_cast<ContainerBasePrivate*>(node(parent));
ContainerBasePrivate* p = static_cast<ContainerBasePrivate*>(node(parent));
if (!p || row < 0 || (size_t)row >= p->children().size())
return QModelIndex();
@ -113,48 +108,48 @@ QModelIndex LocalTaskModel::index(int row, int column, const QModelIndex &parent
return createIndex(row, column, it->get()->pimpl());
}
QModelIndex LocalTaskModel::parent(const QModelIndex &index) const
{
QModelIndex LocalTaskModel::parent(const QModelIndex& index) const {
if (index.model() != this)
return QModelIndex();
ContainerBasePrivate *parent = node(index)->parent()->pimpl();
ContainerBasePrivate* parent = node(index)->parent()->pimpl();
Q_ASSERT(parent);
return this->index(parent);
}
Qt::ItemFlags LocalTaskModel::flags(const QModelIndex &index) const
{
Qt::ItemFlags LocalTaskModel::flags(const QModelIndex& index) const {
Qt::ItemFlags flags = BaseTaskModel::flags(index);
ContainerBasePrivate *c = dynamic_cast<ContainerBasePrivate*>(node(index));
ContainerBasePrivate* c = dynamic_cast<ContainerBasePrivate*>(node(index));
// dropping into containers is enabled
if (c && stage_factory_)
flags |= Qt::ItemIsDropEnabled;
return flags;
}
QVariant LocalTaskModel::data(const QModelIndex &index, int role) const
{
Node *n = node(index);
if (!n) return QVariant();
QVariant LocalTaskModel::data(const QModelIndex& index, int role) const {
Node* n = node(index);
if (!n)
return QVariant();
switch (role) {
case Qt::EditRole:
case Qt::DisplayRole:
switch (index.column()) {
case 0: return QString::fromStdString(n->name());
case 1: return (uint)n->me()->solutions().size();
case 2: return 0;
}
break;
case Qt::EditRole:
case Qt::DisplayRole:
switch (index.column()) {
case 0:
return QString::fromStdString(n->name());
case 1:
return (uint)n->me()->solutions().size();
case 2:
return 0;
}
break;
}
return BaseTaskModel::data(index, role);
}
bool LocalTaskModel::setData(const QModelIndex &index, const QVariant &value, int role)
{
Node *n = node(index);
bool LocalTaskModel::setData(const QModelIndex& index, const QVariant& value, int role) {
Node* n = node(index);
if (!n || index.column() != 0 || role != Qt::EditRole)
return false;
@ -167,33 +162,31 @@ bool LocalTaskModel::setData(const QModelIndex &index, const QVariant &value, in
return true;
}
bool LocalTaskModel::removeRows(int row, int count, const QModelIndex &parent)
{
bool LocalTaskModel::removeRows(int row, int count, const QModelIndex& parent) {
if (!parent.isValid())
return false; // cannot remove top-level container
return false; // cannot remove top-level container
if (flags_ & IS_RUNNING)
return false; // cannot modify running task
return false; // cannot modify running task
Q_ASSERT(dynamic_cast<ContainerBase*>(node(parent)->me()));
ContainerBasePrivate *cp = static_cast<ContainerBasePrivate*>(node(parent));
ContainerBase *c = static_cast<ContainerBase*>(cp->me());
ContainerBasePrivate* cp = static_cast<ContainerBasePrivate*>(node(parent));
ContainerBase* c = static_cast<ContainerBase*>(cp->me());
if (row < 0 || (size_t)row + count > cp->children().size())
return false;
beginRemoveRows(parent, row, row+count-1);
beginRemoveRows(parent, row, row + count - 1);
for (; count > 0; --count)
c->remove(row);
endRemoveRows();
return true;
}
void LocalTaskModel::setStageFactory(const StageFactoryPtr &factory)
{
void LocalTaskModel::setStageFactory(const StageFactoryPtr& factory) {
stage_factory_ = factory;
}
bool LocalTaskModel::dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent)
{
bool LocalTaskModel::dropMimeData(const QMimeData* mime, Qt::DropAction action, int row, int column,
const QModelIndex& parent) {
Q_UNUSED(column);
if (!stage_factory_ || (flags_ & IS_RUNNING))
@ -202,12 +195,11 @@ bool LocalTaskModel::dropMimeData(const QMimeData *mime, Qt::DropAction action,
if (!mime->hasFormat(mime_type))
return false;
ContainerBasePrivate *c = dynamic_cast<ContainerBasePrivate*>(node(parent));
ContainerBasePrivate* c = dynamic_cast<ContainerBasePrivate*>(node(parent));
Q_ASSERT(c);
QString error;
moveit::task_constructor::Stage* stage
= stage_factory_->makeRaw(mime->data(mime_type), &error);
moveit::task_constructor::Stage* stage = stage_factory_->makeRaw(mime->data(mime_type), &error);
if (!stage)
return false;
@ -217,34 +209,31 @@ bool LocalTaskModel::dropMimeData(const QMimeData *mime, Qt::DropAction action,
return true;
}
QModelIndex LocalTaskModel::indexFromStageId(size_t id) const
{
QModelIndex LocalTaskModel::indexFromStageId(size_t id) const {
// TODO implement
return QModelIndex();
}
QAbstractItemModel *LocalTaskModel::getSolutionModel(const QModelIndex &index)
{
QAbstractItemModel* LocalTaskModel::getSolutionModel(const QModelIndex& index) {
// TODO implement
return nullptr;
}
DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex &index)
{
DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex& index) {
// TODO implement
return DisplaySolutionPtr();
}
rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex &index)
{
Node *n = node(index);
if (!n) return nullptr;
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 = PropertyFactory::instance().createPropertyTreeModel(*n->me(), scene_.get(), display_context_);
it_inserted.first->second =
PropertyFactory::instance().createPropertyTreeModel(*n->me(), scene_.get(), display_context_);
it_inserted.first->second->setParent(this);
}
return it_inserted.first->second;
}
}

View File

@ -41,43 +41,41 @@
namespace moveit_rviz_plugin {
class LocalTaskModel
: public BaseTaskModel
, public moveit::task_constructor::Task
class LocalTaskModel : public BaseTaskModel, public moveit::task_constructor::Task
{
Q_OBJECT
typedef moveit::task_constructor::StagePrivate Node;
Node *root_;
Node* root_;
StageFactoryPtr stage_factory_;
std::map<Node*, rviz::PropertyTreeModel*> properties_;
inline Node* node(const QModelIndex &index) const;
QModelIndex index(Node *n) const;
inline Node* node(const QModelIndex& index) const;
QModelIndex index(Node* n) const;
public:
LocalTaskModel(ContainerBase::pointer &&container, const planning_scene::PlanningSceneConstPtr &scene,
rviz::DisplayContext* display_context, QObject *parent = nullptr);
int rowCount(const QModelIndex &parent = QModelIndex()) const override;
LocalTaskModel(ContainerBase::pointer&& container, const planning_scene::PlanningSceneConstPtr& scene,
rviz::DisplayContext* display_context, QObject* parent = nullptr);
int rowCount(const QModelIndex& parent = QModelIndex()) const override;
QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override;
QModelIndex parent(const QModelIndex &index) const override;
QModelIndex index(int row, int column, const QModelIndex& parent = QModelIndex()) const override;
QModelIndex parent(const QModelIndex& index) const override;
Qt::ItemFlags flags(const QModelIndex & index) const override;
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override;
Qt::ItemFlags flags(const QModelIndex& index) const override;
QVariant data(const QModelIndex& index, int role = Qt::DisplayRole) const override;
bool setData(const QModelIndex& index, const QVariant& value, int role = Qt::EditRole) override;
bool removeRows(int row, int count, const QModelIndex &parent) override;
bool removeRows(int row, int count, const QModelIndex& parent) override;
/// providing a StageFactory makes the model accepting drops
void setStageFactory(const StageFactoryPtr &factory) override;
bool dropMimeData(const QMimeData *data, Qt::DropAction action, int row, int column, const QModelIndex &parent) override;
void setStageFactory(const StageFactoryPtr& factory) override;
bool dropMimeData(const QMimeData* data, Qt::DropAction action, int row, int column,
const QModelIndex& parent) override;
QModelIndex indexFromStageId(size_t id) const override;
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
DisplaySolutionPtr getSolution(const QModelIndex& index) override;
rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
};
}

View File

@ -40,20 +40,16 @@
namespace moveit_rviz_plugin {
MetaTaskListModel::MetaTaskListModel()
{
connect(this, SIGNAL(rowsRemoved(QModelIndex,int,int)),
this, SLOT(onRowsRemoved(QModelIndex,int,int)));
MetaTaskListModel::MetaTaskListModel() {
connect(this, SIGNAL(rowsRemoved(QModelIndex, int, int)), this, SLOT(onRowsRemoved(QModelIndex, int, int)));
}
MetaTaskListModel &MetaTaskListModel::instance()
{
MetaTaskListModel& MetaTaskListModel::instance() {
static MetaTaskListModel instance_;
return instance_;
}
bool MetaTaskListModel::insertModel(TaskListModel *model, TaskDisplay *display)
{
bool MetaTaskListModel::insertModel(TaskListModel* model, TaskDisplay* display) {
if (!model || !display)
return false;
if (display_.contains(display))
@ -67,17 +63,16 @@ bool MetaTaskListModel::insertModel(TaskListModel *model, TaskDisplay *display)
return true;
}
void MetaTaskListModel::onRowsRemoved(const QModelIndex &parent, int first, int last)
{
void MetaTaskListModel::onRowsRemoved(const QModelIndex& parent, int first, int last) {
if (!parent.isValid()) {
display_.remove(first, last-first+1);
display_.remove(first, last - first + 1);
}
}
void MetaTaskListModel::onDisplayNameChanged(const QString &name)
{
void MetaTaskListModel::onDisplayNameChanged(const QString& name) {
int row = display_.indexOf(static_cast<TaskDisplay*>(sender()));
if (row < 0) return;
if (row < 0)
return;
QModelIndex idx = index(row, 0);
if (idx.data() == name)
@ -86,8 +81,7 @@ void MetaTaskListModel::onDisplayNameChanged(const QString &name)
setData(idx, name, Qt::EditRole);
}
bool MetaTaskListModel::setData(const QModelIndex &index, const QVariant &value, int role)
{
bool MetaTaskListModel::setData(const QModelIndex& index, const QVariant& value, int role) {
bool result = TreeMergeProxyModel::setData(index, value, role);
if (result && isGroupItem(index)) {
display_.at(index.row())->setName(value.toString());
@ -95,26 +89,22 @@ bool MetaTaskListModel::setData(const QModelIndex &index, const QVariant &value,
return result;
}
bool MetaTaskListModel::removeRows(int row, int count, const QModelIndex &parent)
{
bool MetaTaskListModel::removeRows(int row, int count, const QModelIndex& parent) {
if (!parent.isValid()) // forbid removal of top-level items (displays)
return false;
return TreeMergeProxyModel::removeRows(row, count, parent);
}
std::pair<TaskListModel*, TaskDisplay*>
MetaTaskListModel::getTaskListModel(const QModelIndex &index) const
{
QAbstractItemModel *m = getModel(index).first;
if (!m) return std::make_pair(nullptr, nullptr);
std::pair<TaskListModel*, TaskDisplay*> MetaTaskListModel::getTaskListModel(const QModelIndex& index) const {
QAbstractItemModel* m = getModel(index).first;
if (!m)
return std::make_pair(nullptr, nullptr);
Q_ASSERT(dynamic_cast<TaskListModel*>(m));
return std::make_pair(static_cast<TaskListModel*>(m), display_.at(getRow(m)));
}
std::pair<BaseTaskModel*, QModelIndex>
MetaTaskListModel::getTaskModel(const QModelIndex &index) const
{
std::pair<BaseTaskModel*, QModelIndex> MetaTaskListModel::getTaskModel(const QModelIndex& index) const {
if (!index.isValid())
return std::make_pair(nullptr, QModelIndex());
@ -128,5 +118,4 @@ MetaTaskListModel::getTaskModel(const QModelIndex &index) const
Q_ASSERT(dynamic_cast<BaseTaskModel*>(m.first));
return std::make_pair(static_cast<BaseTaskModel*>(m.first), m.second);
}
}

View File

@ -55,7 +55,8 @@ MOVEIT_CLASS_FORWARD(TaskDisplay)
*
* This is a singleton instance.
*/
class MetaTaskListModel : public utils::TreeMergeProxyModel {
class MetaTaskListModel : public utils::TreeMergeProxyModel
{
Q_OBJECT
// 1:1 correspondence of displays to models
@ -70,8 +71,8 @@ class MetaTaskListModel : public utils::TreeMergeProxyModel {
using utils::TreeMergeProxyModel::insertModel;
private Q_SLOTS:
void onRowsRemoved(const QModelIndex &parent, int first, int last);
void onDisplayNameChanged(const QString &name);
void onRowsRemoved(const QModelIndex& parent, int first, int last);
void onDisplayNameChanged(const QString& name);
public:
static MetaTaskListModel& instance();
@ -79,13 +80,12 @@ public:
/// insert a new TaskListModel together with it's associated display
bool insertModel(TaskListModel* model, TaskDisplay* display);
bool setData(const QModelIndex &index, const QVariant &value, int role) override;
bool removeRows(int row, int count, const QModelIndex &parent) override;
bool setData(const QModelIndex& index, const QVariant& value, int role) override;
bool removeRows(int row, int count, const QModelIndex& parent) override;
/// retrieve TaskListModel and TaskDisplay corresponding to given index
std::pair<TaskListModel*, TaskDisplay*> getTaskListModel(const QModelIndex &index) const;
std::pair<TaskListModel*, TaskDisplay*> getTaskListModel(const QModelIndex& index) const;
/// retrieve TaskModel and its source index corresponding to given proxy index
std::pair<BaseTaskModel*, QModelIndex> getTaskModel(const QModelIndex& index) const;
};
}

View File

@ -57,163 +57,136 @@ namespace moveit_rviz_plugin {
/** Templated factory to create objects of a given pluginlib base class type.
* This is a slightly modified version of rviz::PluginlibFactory, providing a custom mime type.
*/
template<class Type>
class PluginlibFactory: public rviz::Factory
template <class Type>
class PluginlibFactory : public rviz::Factory
{
private:
struct BuiltInClassRecord
{
QString class_id_;
QString package_;
QString name_;
QString description_;
std::function<Type*()> factory_function_;
};
struct BuiltInClassRecord
{
QString class_id_;
QString package_;
QString name_;
QString description_;
std::function<Type*()> factory_function_;
};
public:
PluginlibFactory( const QString& package, const QString& base_class_type )
: mime_type_(QString("application/%1/%2").arg(package, base_class_type))
{
class_loader_ = new pluginlib::ClassLoader<Type>( package.toStdString(), base_class_type.toStdString() );
}
virtual ~PluginlibFactory()
{
delete class_loader_;
}
PluginlibFactory(const QString& package, const QString& base_class_type)
: mime_type_(QString("application/%1/%2").arg(package, base_class_type)) {
class_loader_ = new pluginlib::ClassLoader<Type>(package.toStdString(), base_class_type.toStdString());
}
virtual ~PluginlibFactory() { delete class_loader_; }
/// retrieve mime type used for given factory
QString mimeType() const {
return mime_type_;
}
/// retrieve mime type used for given factory
QString mimeType() const { return mime_type_; }
virtual QStringList getDeclaredClassIds()
{
QStringList ids;
for(const auto& record : built_ins_)
ids.push_back(record.class_id_);
for (const auto& id : class_loader_->getDeclaredClasses()) {
QString sid = QString::fromStdString(id);
if (ids.contains(sid)) continue; // built_in take precedence
ids.push_back(sid);
}
return ids;
}
virtual QStringList getDeclaredClassIds() {
QStringList ids;
for (const auto& record : built_ins_)
ids.push_back(record.class_id_);
for (const auto& id : class_loader_->getDeclaredClasses()) {
QString sid = QString::fromStdString(id);
if (ids.contains(sid))
continue; // built_in take precedence
ids.push_back(sid);
}
return ids;
}
virtual QString getClassDescription( const QString& class_id ) const
{
auto it = built_ins_.find( class_id );
if( it != built_ins_.end() )
{
return it->description_;
}
return QString::fromStdString( class_loader_->getClassDescription( class_id.toStdString() ));
}
virtual QString getClassDescription(const QString& class_id) const {
auto it = built_ins_.find(class_id);
if (it != built_ins_.end()) {
return it->description_;
}
return QString::fromStdString(class_loader_->getClassDescription(class_id.toStdString()));
}
virtual QString getClassName( const QString& class_id ) const
{
auto it = built_ins_.find( class_id );
if( it != built_ins_.end() )
{
return it->name_;
}
return QString::fromStdString( class_loader_->getName( class_id.toStdString() ));
}
virtual QString getClassName(const QString& class_id) const {
auto it = built_ins_.find(class_id);
if (it != built_ins_.end()) {
return it->name_;
}
return QString::fromStdString(class_loader_->getName(class_id.toStdString()));
}
virtual QString getClassPackage( const QString& class_id ) const
{
auto it = built_ins_.find( class_id );
if( it != built_ins_.end() )
{
return it->package_;
}
return QString::fromStdString( class_loader_->getClassPackage( class_id.toStdString() ));
}
virtual QString getClassPackage(const QString& class_id) const {
auto it = built_ins_.find(class_id);
if (it != built_ins_.end()) {
return it->package_;
}
return QString::fromStdString(class_loader_->getClassPackage(class_id.toStdString()));
}
virtual QString getPluginManifestPath( const QString& class_id ) const
{
auto it = built_ins_.find( class_id );
if( it != built_ins_.end() )
{
return "";
}
return QString::fromStdString( class_loader_->getPluginManifestPath( class_id.toStdString() ));
}
virtual QString getPluginManifestPath(const QString& class_id) const {
auto it = built_ins_.find(class_id);
if (it != built_ins_.end()) {
return "";
}
return QString::fromStdString(class_loader_->getPluginManifestPath(class_id.toStdString()));
}
virtual QIcon getIcon( const QString& class_id ) const
{
QString package = getClassPackage( class_id );
QString class_name = getClassName( class_id );
QIcon icon = rviz::loadPixmap( "package://"+package+"/icons/classes/"+class_name+".svg" );
if ( icon.isNull() )
{
icon = rviz::loadPixmap( "package://"+package+"/icons/classes/"+class_name+".png" );
if ( icon.isNull() )
{
icon = rviz::loadPixmap( "package://rviz/icons/default_class_icon.png");
}
}
return icon;
}
virtual QIcon getIcon(const QString& class_id) const {
QString package = getClassPackage(class_id);
QString class_name = getClassName(class_id);
QIcon icon = rviz::loadPixmap("package://" + package + "/icons/classes/" + class_name + ".svg");
if (icon.isNull()) {
icon = rviz::loadPixmap("package://" + package + "/icons/classes/" + class_name + ".png");
if (icon.isNull()) {
icon = rviz::loadPixmap("package://rviz/icons/default_class_icon.png");
}
}
return icon;
}
void addBuiltInClass(const QString& package, const QString& name, const QString& description,
const std::function<Type*()>& factory_function)
{
BuiltInClassRecord record;
record.class_id_ = package + "/" + name;
record.package_ = package;
record.name_ = name;
record.description_ = description;
record.factory_function_ = factory_function;
built_ins_[ record.class_id_ ] = record;
}
template <class Derived>
void addBuiltInClass(const QString& name, const QString& description)
{
addBuiltInClass("Built Ins", name, description, [](){return new Derived();});
}
void addBuiltInClass(const QString& package, const QString& name, const QString& description,
const std::function<Type*()>& factory_function) {
BuiltInClassRecord record;
record.class_id_ = package + "/" + name;
record.package_ = package;
record.name_ = name;
record.description_ = description;
record.factory_function_ = factory_function;
built_ins_[record.class_id_] = record;
}
template <class Derived>
void addBuiltInClass(const QString& name, const QString& description) {
addBuiltInClass("Built Ins", name, description, []() { return new Derived(); });
}
/** @brief Instantiate and return a instance of a subclass of Type using our
* pluginlib::ClassLoader.
* @param class_id A string identifying the class uniquely among
* classes of its parent class. rviz::GridDisplay might be
* rviz/Grid, for example.
* @param error_return If non-NULL and there is an error, *error_return is set to a description of the problem.
* @return A new instance of the class identified by class_id, or NULL if there was an error.
*
* If makeRaw() returns NULL and error_return is not NULL, *error_return will be set.
* On success, *error_return will not be changed. */
virtual Type* makeRaw( const QString& class_id, QString* error_return = NULL )
{
typename QHash<QString, BuiltInClassRecord>::const_iterator iter = built_ins_.find( class_id );
if( iter != built_ins_.end() )
{
Type* instance = iter->factory_function_();
if( instance == NULL && error_return != NULL )
{
*error_return = "Factory function for built-in class '" + class_id + "' returned NULL.";
}
return instance;
}
try
{
return class_loader_->createUnmanagedInstance( class_id.toStdString() );
}
catch( pluginlib::PluginlibException& ex )
{
ROS_ERROR( "PluginlibFactory: The plugin for class '%s' failed to load. Error: %s",
qPrintable( class_id ), ex.what() );
if( error_return )
{
*error_return = QString::fromStdString( ex.what() );
}
return NULL;
}
}
/** @brief Instantiate and return a instance of a subclass of Type using our
* pluginlib::ClassLoader.
* @param class_id A string identifying the class uniquely among
* classes of its parent class. rviz::GridDisplay might be
* rviz/Grid, for example.
* @param error_return If non-NULL and there is an error, *error_return is set to a description of the problem.
* @return A new instance of the class identified by class_id, or NULL if there was an error.
*
* If makeRaw() returns NULL and error_return is not NULL, *error_return will be set.
* On success, *error_return will not be changed. */
virtual Type* makeRaw(const QString& class_id, QString* error_return = NULL) {
typename QHash<QString, BuiltInClassRecord>::const_iterator iter = built_ins_.find(class_id);
if (iter != built_ins_.end()) {
Type* instance = iter->factory_function_();
if (instance == NULL && error_return != NULL) {
*error_return = "Factory function for built-in class '" + class_id + "' returned NULL.";
}
return instance;
}
try {
return class_loader_->createUnmanagedInstance(class_id.toStdString());
} catch (pluginlib::PluginlibException& ex) {
ROS_ERROR("PluginlibFactory: The plugin for class '%s' failed to load. Error: %s", qPrintable(class_id),
ex.what());
if (error_return) {
*error_return = QString::fromStdString(ex.what());
}
return NULL;
}
}
private:
const QString mime_type_;
pluginlib::ClassLoader<Type>* class_loader_;
QHash<QString, BuiltInClassRecord> built_ins_;
const QString mime_type_;
pluginlib::ClassLoader<Type>* class_loader_;
QHash<QString, BuiltInClassRecord> built_ins_;
};
}

View File

@ -55,14 +55,16 @@ using namespace moveit::task_constructor;
namespace moveit_rviz_plugin {
enum NodeFlag {
WAS_VISITED = 0x01, // indicate that model should emit change notifications
NAME_CHANGED = 0x02, // indicate that name was manually changed
enum NodeFlag
{
WAS_VISITED = 0x01, // indicate that model should emit change notifications
NAME_CHANGED = 0x02, // indicate that name was manually changed
};
typedef QFlags<NodeFlag> NodeFlags;
struct RemoteTaskModel::Node {
Node *parent_;
struct RemoteTaskModel::Node
{
Node* parent_;
std::vector<std::unique_ptr<Node>> children_;
QString name_;
InterfaceFlags interface_flags_;
@ -71,31 +73,30 @@ struct RemoteTaskModel::Node {
std::unique_ptr<rviz::PropertyTreeModel> property_tree_;
std::map<std::string, Property> properties_;
inline Node(Node *parent) : parent_(parent) {
inline Node(Node* parent) : parent_(parent) {
solutions_.reset(new RemoteSolutionModel());
property_tree_.reset(new rviz::PropertyTreeModel(new rviz::Property()));
}
bool setName(const QString& name) {
if (name == name_) return false;
if (name == name_)
return false;
name_ = name;
return true;
}
void setProperties(const std::vector<moveit_task_constructor_msgs::Property>& props,
const planning_scene::PlanningSceneConstPtr& scene_,
rviz::DisplayContext* display_context_);
rviz::Property *createProperty(const moveit_task_constructor_msgs::Property &prop, rviz::Property *old,
const planning_scene::PlanningSceneConstPtr& scene_, rviz::DisplayContext* display_context_);
rviz::Property* createProperty(const moveit_task_constructor_msgs::Property& prop, rviz::Property* old,
const planning_scene::PlanningSceneConstPtr& scene_,
rviz::DisplayContext* display_context_);
};
void RemoteTaskModel::Node::setProperties(const std::vector<moveit_task_constructor_msgs::Property> &props,
void RemoteTaskModel::Node::setProperties(const std::vector<moveit_task_constructor_msgs::Property>& props,
const planning_scene::PlanningSceneConstPtr& scene_,
rviz::DisplayContext* display_context_)
{
rviz::DisplayContext* display_context_) {
// insert properties in same order as reported in description
rviz::Property *root = property_tree_->getRoot();
rviz::Property* root = property_tree_->getRoot();
int index = 0; // current child index in root
for (auto it = props.begin(); it != props.end(); ++it) {
int num = root->numChildren();
@ -104,28 +105,27 @@ void RemoteTaskModel::Node::setProperties(const std::vector<moveit_task_construc
while (next < num && root->childAt(next)->getName().toStdString() < it->name)
++next;
// and remove all children in range [index, next) at once
root->removeChildren(index, next-index);
root->removeChildren(index, next - index);
num = root->numChildren();
// if names differ, insert a new child, otherwise reuse existing
rviz::Property *old_child = index < num ? root->childAt(index) : nullptr;
rviz::Property* old_child = index < num ? root->childAt(index) : nullptr;
if (old_child && old_child->getName().toStdString() != it->name)
old_child = nullptr;
rviz::Property *new_child = createProperty(*it, old_child, scene_, display_context_);
rviz::Property* new_child = createProperty(*it, old_child, scene_, display_context_);
if (new_child != old_child)
root->addChild(new_child, index);
++index;
}
// remove remaining children
root->removeChildren(index, root->numChildren()-index);
root->removeChildren(index, root->numChildren() - index);
}
rviz::Property*
RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Property& prop, rviz::Property* old,
const planning_scene::PlanningSceneConstPtr& scene_,
rviz::DisplayContext* display_context_)
{
rviz::Property* RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Property& prop,
rviz::Property* old,
const planning_scene::PlanningSceneConstPtr& scene_,
rviz::DisplayContext* display_context_) {
auto& factory = PropertyFactory::instance();
// try to deserialize from msg (using registered functions)
boost::any value = Property::deserialize(prop.type, prop.value);
@ -145,8 +145,7 @@ RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Proper
}
// return Node* corresponding to index
RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex &index) const
{
RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex& index) const {
if (!index.isValid())
return root_;
@ -156,25 +155,23 @@ RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex &index) const
}
// internal pointer refers to parent node
Node *parent = static_cast<Node*>(index.internalPointer());
Node* parent = static_cast<Node*>(index.internalPointer());
Q_ASSERT(index.row() >= 0 && (size_t)index.row() < parent->children_.size());
return parent->children_.at(index.row()).get();
}
// return Node* corresponding to stage_id
RemoteTaskModel::Node* RemoteTaskModel::node(uint32_t stage_id) const
{
RemoteTaskModel::Node* RemoteTaskModel::node(uint32_t stage_id) const {
auto it = id_to_stage_.find(stage_id);
return (it == id_to_stage_.end()) ? nullptr : it->second;
}
// return QModelIndex corresponding to Node*
QModelIndex RemoteTaskModel::index(const Node *n) const
{
QModelIndex RemoteTaskModel::index(const Node* n) const {
if (n == root_)
return QModelIndex();
Node *parent = n->parent_;
Node* parent = n->parent_;
// the internal pointer refers to the parent node of n
for (int row = 0, end = parent->children_.size(); row != end; ++row)
@ -184,41 +181,36 @@ QModelIndex RemoteTaskModel::index(const Node *n) const
return QModelIndex();
}
RemoteTaskModel::RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene,
rviz::DisplayContext* display_context, QObject *parent)
: BaseTaskModel(scene, display_context, parent)
, root_(new Node(nullptr))
{
id_to_stage_[0] = root_; // root node has ID 0
RemoteTaskModel::RemoteTaskModel(const planning_scene::PlanningSceneConstPtr& scene,
rviz::DisplayContext* display_context, QObject* parent)
: BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) {
id_to_stage_[0] = root_; // root node has ID 0
}
RemoteTaskModel::~RemoteTaskModel()
{
RemoteTaskModel::~RemoteTaskModel() {
delete root_;
}
void RemoteTaskModel::setSolutionClient(ros::ServiceClient *client)
{
void RemoteTaskModel::setSolutionClient(ros::ServiceClient* client) {
get_solution_client_ = client;
}
int RemoteTaskModel::rowCount(const QModelIndex &parent) const
{
int RemoteTaskModel::rowCount(const QModelIndex& parent) const {
if (parent.column() > 0)
return 0;
Node *n = node(parent);
if (!n) return 0; // invalid model in parent
Node* n = node(parent);
if (!n)
return 0; // invalid model in parent
return n->children_.size();
}
QModelIndex RemoteTaskModel::index(int row, int column, const QModelIndex &parent) const
{
QModelIndex RemoteTaskModel::index(int row, int column, const QModelIndex& parent) const {
if (column < 0 || column >= columnCount())
return QModelIndex();
Node *p = node(parent);
Node* p = node(parent);
if (!p || row < 0 || (size_t)row >= p->children_.size())
return QModelIndex();
@ -227,13 +219,12 @@ QModelIndex RemoteTaskModel::index(int row, int column, const QModelIndex &paren
return createIndex(row, column, p);
}
QModelIndex RemoteTaskModel::parent(const QModelIndex &child) const
{
QModelIndex RemoteTaskModel::parent(const QModelIndex& child) const {
if (!child.isValid())
return QModelIndex();
// the internal pointer refers to the parent node
Node *p = static_cast<Node*>(child.internalPointer());
Node* p = static_cast<Node*>(child.internalPointer());
Q_ASSERT(p);
if (child.model() != this || p == root_)
return QModelIndex();
@ -241,44 +232,45 @@ QModelIndex RemoteTaskModel::parent(const QModelIndex &child) const
return this->index(p);
}
Qt::ItemFlags RemoteTaskModel::flags(const QModelIndex &index) const
{
Qt::ItemFlags RemoteTaskModel::flags(const QModelIndex& index) const {
Qt::ItemFlags flags = BaseTaskModel::flags(index);
if (index.column() == 0)
flags |= Qt::ItemIsEditable; // name is editable
flags |= Qt::ItemIsEditable; // name is editable
return flags;
}
QVariant RemoteTaskModel::data(const QModelIndex &index, int role) const
{
Node *n = node(index);
if (!n) return QVariant(); // invalid model in index
QVariant RemoteTaskModel::data(const QModelIndex& index, int role) const {
Node* n = node(index);
if (!n)
return QVariant(); // invalid model in index
switch (role) {
case Qt::EditRole:
case Qt::DisplayRole:
switch (index.column()) {
case 0: return n->name_;
case 1: return n->solutions_->numSuccessful();
case 2: return n->solutions_->numFailed();
}
break;
case Qt::ForegroundRole:
if (index.column() == 0 && !index.parent().isValid())
return (flags_ & IS_DESTROYED) ? QColor(Qt::red) : QApplication::palette().text().color();
break;
case Qt::DecorationRole:
if (index.column() == 0 && index.parent().isValid())
return flowIcon(n->interface_flags_);
break;
case Qt::EditRole:
case Qt::DisplayRole:
switch (index.column()) {
case 0:
return n->name_;
case 1:
return n->solutions_->numSuccessful();
case 2:
return n->solutions_->numFailed();
}
break;
case Qt::ForegroundRole:
if (index.column() == 0 && !index.parent().isValid())
return (flags_ & IS_DESTROYED) ? QColor(Qt::red) : QApplication::palette().text().color();
break;
case Qt::DecorationRole:
if (index.column() == 0 && index.parent().isValid())
return flowIcon(n->interface_flags_);
break;
}
return BaseTaskModel::data(index, role);
}
bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, int role)
{
Node *n = node(index);
bool RemoteTaskModel::setData(const QModelIndex& index, const QVariant& value, int role) {
Node* n = node(index);
if (!n || index.column() != 0 || role != Qt::EditRole)
return false;
n->setName(value.toString());
@ -287,34 +279,34 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i
return true;
}
QModelIndex RemoteTaskModel::indexFromStageId(size_t id) const
{
Node *n = node(id);
QModelIndex RemoteTaskModel::indexFromStageId(size_t id) const {
Node* n = node(id);
return n ? index(n) : QModelIndex();
}
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg)
{
void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type& msg) {
// iterate over descriptions and create new / update existing nodes where needed
for (const auto &s : msg) {
for (const auto& s : msg) {
// find parent node for stage s, this should always exist
auto parent_it = id_to_stage_.find(s.parent_id);
if (parent_it == id_to_stage_.end()) {
ROS_ERROR_NAMED("TaskListModel", "No parent found for stage %d (%s)", s.id, s.name.c_str());
continue;
}
Node *parent = parent_it->second;
Node* parent = parent_it->second;
Node*& n = id_to_stage_[s.id];
if (!n) { // create a new Node if neccessary
if (!n) { // create a new Node if neccessary
// only emit notify signal if parent node was ever visited
bool notify = parent->node_flags_ & WAS_VISITED;
QModelIndex parentIdx = index(parent);
int row = parent->children_.size();
if (notify) beginInsertRows(parentIdx, row, row);
if (notify)
beginInsertRows(parentIdx, row, row);
parent->children_.push_back(std::make_unique<Node>(parent));
if (notify) endInsertRows();
if (notify)
endInsertRows();
// store Node* in id_to_stage_
n = parent->children_.back().get();
@ -323,16 +315,18 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg
// set content of stage
bool changed = false;
if (!(n->node_flags_ & NAME_CHANGED)) // avoid overwriting a manually changed name
if (!(n->node_flags_ & NAME_CHANGED)) // avoid overwriting a manually changed name
changed |= n->setName(QString::fromStdString(s.name));
n->setProperties(s.properties, scene_, display_context_);
InterfaceFlags old_flags = n->interface_flags_;
n->interface_flags_ = InterfaceFlags();
for (auto f : {READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END}) {
if (s.flags & f) n->interface_flags_ |= f;
else n->interface_flags_ &= ~f;
for (auto f : { READS_START, READS_END, WRITES_NEXT_START, WRITES_PREV_END }) {
if (s.flags & f)
n->interface_flags_ |= f;
else
n->interface_flags_ &= ~f;
}
changed |= (n->interface_flags_ != old_flags);
@ -349,17 +343,16 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg
}
}
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg)
{
void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type& msg) {
// iterate over statistics and update node's solutions where needed
for (const auto &s : msg) {
for (const auto& s : msg) {
// find node for stage s, this should always exist
auto it = id_to_stage_.find(s.id);
if (it == id_to_stage_.end()) {
ROS_ERROR_NAMED("TaskListModel", "No stage %d", s.id);
continue;
}
Node *n = it->second;
Node* n = it->second;
n->solutions_->processSolutionIDs(s.solved, s.failed, s.num_failed);
// emit notify about model changes when node was already visited
@ -370,15 +363,14 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs:
}
}
void RemoteTaskModel::setSolutionData(const moveit_task_constructor_msgs::SolutionInfo &info)
{
if (info.id == 0) return;
if (RemoteSolutionModel *m = getSolutionModel(info.stage_id))
void RemoteTaskModel::setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info) {
if (info.id == 0)
return;
if (RemoteSolutionModel* m = getSolutionModel(info.stage_id))
m->setSolutionData(info.id, info.cost, QString::fromStdString(info.comment));
}
DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg)
{
DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) {
DisplaySolutionPtr s(new DisplaySolution);
s->setFromMessage(scene_->diff(), msg);
@ -390,18 +382,20 @@ DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_con
// caching is only enabled for top-level solutions (stage_id == 1)
// otherwise we would store PlanningScenes over and over
if (!msg.sub_solution.empty() &&
msg.sub_solution.front().info.stage_id == 1 &&
if (!msg.sub_solution.empty() && msg.sub_solution.front().info.stage_id == 1 &&
msg.sub_solution.front().info.id != 0) {
// cache solution for future use
id_to_solution_[msg.sub_solution.front().info.id] = s;
// cache DisplaySolutions for all individual sub trajectories
uint i=0;
for (const auto &t : msg.sub_trajectory) {
if (t.info.id == 0) continue; // invalid id
DisplaySolutionPtr &sub = id_to_solution_.insert(std::make_pair(t.info.id, DisplaySolutionPtr())).first->second;
if (!sub) sub.reset(new DisplaySolution(*s, i));
uint i = 0;
for (const auto& t : msg.sub_trajectory) {
if (t.info.id == 0)
continue; // invalid id
DisplaySolutionPtr& sub =
id_to_solution_.insert(std::make_pair(t.info.id, DisplaySolutionPtr())).first->second;
if (!sub)
sub.reset(new DisplaySolution(*s, i));
i++;
}
}
@ -409,21 +403,19 @@ DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_con
return s;
}
RemoteSolutionModel* RemoteTaskModel::getSolutionModel(uint32_t stage_id) const
{
Node *n = node(stage_id);
RemoteSolutionModel* RemoteTaskModel::getSolutionModel(uint32_t stage_id) const {
Node* n = node(stage_id);
return n ? n->solutions_.get() : nullptr;
}
QAbstractItemModel* RemoteTaskModel::getSolutionModel(const QModelIndex &index)
{
Node *n = node(index);
if (!n) return nullptr;
QAbstractItemModel* RemoteTaskModel::getSolutionModel(const QModelIndex& index) {
Node* n = node(index);
if (!n)
return nullptr;
return n->solutions_.get();
}
DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
{
DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) {
Q_ASSERT(index.isValid());
uint32_t id = index.sibling(index.row(), 0).data(Qt::UserRole).toUInt();
@ -441,7 +433,7 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
if (get_solution_client_->call(srv)) {
id_to_solution_[id] = result = processSolutionMessage(srv.response.solution);
return result;
} else { // on failure mark remote task as destroyed: don't retrieve more solutions
} else { // on failure mark remote task as destroyed: don't retrieve more solutions
flags_ |= IS_DESTROYED;
}
} catch (const std::exception& e) {
@ -453,27 +445,23 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
return it->second;
}
rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex &index)
{
Node *n = node(index);
if (!n) return nullptr;
rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex& index) {
Node* n = node(index);
if (!n)
return nullptr;
return n->property_tree_.get();
}
namespace detail {
// method used by sorted_ container (requiring an additional dereference to access id)
template <class T>
typename T::iterator findById(T& c, decltype((*c.cbegin())->id) id)
{
return std::find_if(c.begin(), c.end(), [id](const typename T::value_type& item) {
return item->id == id;
});
typename T::iterator findById(T& c, decltype((*c.cbegin())->id) id) {
return std::find_if(c.begin(), c.end(), [id](const typename T::value_type& item) { return item->id == id; });
}
// insert new Data item into data_ container
template <class T>
typename T::iterator insert(T& c, typename T::value_type&& item)
{
typename T::iterator insert(T& c, typename T::value_type&& item) {
auto p = std::equal_range(c.begin(), c.end(), item);
if (p.first == p.second) // new item
return c.insert(p.second, std::move(item));
@ -482,80 +470,79 @@ typename T::iterator insert(T& c, typename T::value_type&& item)
}
}
RemoteSolutionModel::RemoteSolutionModel(QObject *parent)
: QAbstractTableModel(parent)
{
}
RemoteSolutionModel::RemoteSolutionModel(QObject* parent) : QAbstractTableModel(parent) {}
int RemoteSolutionModel::rowCount(const QModelIndex &parent) const
{
int RemoteSolutionModel::rowCount(const QModelIndex& parent) const {
return sorted_.size();
}
int RemoteSolutionModel::columnCount(const QModelIndex &parent) const
{
int RemoteSolutionModel::columnCount(const QModelIndex& parent) const {
return 3;
}
QVariant RemoteSolutionModel::headerData(int section, Qt::Orientation orientation, int role) const
{
QVariant RemoteSolutionModel::headerData(int section, Qt::Orientation orientation, int role) const {
if (orientation == Qt::Horizontal) {
switch (role) {
case Qt::DisplayRole:
switch (section) {
case 0: return tr("#");
case 1: return tr("cost");
case 2: return tr("comment");
}
case Qt::TextAlignmentRole:
return section == 2 ? Qt::AlignLeft : Qt::AlignRight;
case Qt::DisplayRole:
switch (section) {
case 0:
return tr("#");
case 1:
return tr("cost");
case 2:
return tr("comment");
}
case Qt::TextAlignmentRole:
return section == 2 ? Qt::AlignLeft : Qt::AlignRight;
}
}
return QAbstractItemModel::headerData(section, orientation, role);
}
QVariant RemoteSolutionModel::data(const QModelIndex &index, int role) const
{
QVariant RemoteSolutionModel::data(const QModelIndex& index, int role) const {
Q_ASSERT(index.isValid());
Q_ASSERT(!index.parent().isValid());
const Data &item = *sorted_[index.row()];
const Data& item = *sorted_[index.row()];
switch (role) {
case Qt::UserRole:
case Qt::ToolTipRole:
return item.id;
case Qt::UserRole:
case Qt::ToolTipRole:
return item.id;
case Qt::DisplayRole:
switch(index.column()) {
case 0: return item.creation_rank;
case 1:
if (std::isinf(item.cost)) return tr(u8"");
if (std::isnan(item.cost)) return QVariant();
return item.cost;
case 2: return item.comment;
}
case Qt::DisplayRole:
switch (index.column()) {
case 0:
return item.creation_rank;
case 1:
if (std::isinf(item.cost))
return tr(u8"");
if (std::isnan(item.cost))
return QVariant();
return item.cost;
case 2:
return item.comment;
}
case Qt::ForegroundRole:
if (std::isinf(item.cost))
return QColor(Qt::red);
break;
case Qt::ForegroundRole:
if (std::isinf(item.cost))
return QColor(Qt::red);
break;
case Qt::TextAlignmentRole:
return index.column() == 2 ? Qt::AlignLeft : Qt::AlignRight;
case Qt::TextAlignmentRole:
return index.column() == 2 ? Qt::AlignLeft : Qt::AlignRight;
}
return QVariant();
}
void RemoteSolutionModel::setSolutionData(uint32_t id, float cost, const QString &comment)
{
void RemoteSolutionModel::setSolutionData(uint32_t id, float cost, const QString& comment) {
// retrieve iterator and row corresponding to id
auto sit = detail::findById(sorted_, id);
int row = (sit != sorted_.end()) ? sit - sorted_.begin() : -1;
auto it = (sit != sorted_.end()) ? *sit : detail::insert(data_, Data(id, cost, 0, comment));
QModelIndex tl, br;
Data &item = *it;
Data& item = *it;
if (item.cost != cost) {
item.cost = cost;
tl = br = index(row, 1);
@ -569,14 +556,13 @@ void RemoteSolutionModel::setSolutionData(uint32_t id, float cost, const QString
if (tl.isValid())
Q_EMIT dataChanged(tl, br);
if (row < 0 && isVisible(*it)) // item was newly created: inform views
if (row < 0 && isVisible(*it)) // item was newly created: inform views
sortInternal();
}
void RemoteSolutionModel::sort(int column, Qt::SortOrder order)
{
void RemoteSolutionModel::sort(int column, Qt::SortOrder order) {
if (sort_column_ == column && sort_order_ == order)
return; // nothing to do
return; // nothing to do
sort_column_ = column;
sort_order_ = order;
@ -584,32 +570,36 @@ void RemoteSolutionModel::sort(int column, Qt::SortOrder order)
sortInternal();
}
void RemoteSolutionModel::sortInternal()
{
void RemoteSolutionModel::sortInternal() {
Q_EMIT layoutAboutToBeChanged();
QModelIndexList old_indexes = persistentIndexList();
std::vector<DataList::iterator> old_sorted_; std::swap(sorted_, old_sorted_);
std::vector<DataList::iterator> old_sorted_;
std::swap(sorted_, old_sorted_);
// create new order in sorted_
for (auto it = data_.begin(), end = data_.end(); it != end; ++it)
if (isVisible(*it)) sorted_.push_back(it);
if (isVisible(*it))
sorted_.push_back(it);
if (sort_column_ >= 0) {
std::sort(sorted_.begin(), sorted_.end(), [this](const DataList::iterator& left, const DataList::iterator& right) {
int comp = 0;
switch (sort_column_) {
case 1: // cost order
if (left->cost_rank < right->cost_rank) comp = -1;
else if (left->cost_rank > right->cost_rank) comp = 1;
break;
case 2: // comment
comp = left->comment.compare(right->comment);
break;
}
if (comp == 0) // if still undecided, id decides
comp = (left->id < right->id ? -1 : 1);
return (sort_order_ == Qt::AscendingOrder) ? (comp < 0) : (comp >= 0);
});
std::sort(sorted_.begin(), sorted_.end(),
[this](const DataList::iterator& left, const DataList::iterator& right) {
int comp = 0;
switch (sort_column_) {
case 1: // cost order
if (left->cost_rank < right->cost_rank)
comp = -1;
else if (left->cost_rank > right->cost_rank)
comp = 1;
break;
case 2: // comment
comp = left->comment.compare(right->comment);
break;
}
if (comp == 0) // if still undecided, id decides
comp = (left->id < right->id ? -1 : 1);
return (sort_order_ == Qt::AscendingOrder) ? (comp < 0) : (comp >= 0);
});
}
// map old indexes to new ones
@ -618,7 +608,7 @@ void RemoteSolutionModel::sortInternal()
for (int i = 0, end = old_indexes.count(); i != end; ++i) {
int old_row = old_indexes[i].row();
auto it_inserted = old_to_new_row.insert(std::make_pair(old_row, -1));
if (it_inserted.second) { // newly inserted: find new row index
if (it_inserted.second) { // newly inserted: find new row index
auto it = detail::findById(sorted_, old_sorted_[old_row]->id);
if (it != sorted_.cend())
it_inserted.first->second = it - sorted_.begin();
@ -631,32 +621,29 @@ void RemoteSolutionModel::sortInternal()
}
// process solution ids received in stage statistics
void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &successful,
const std::vector<uint32_t> &failed,
size_t num_failed)
{
void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t>& successful,
const std::vector<uint32_t>& failed, size_t num_failed) {
// append new items to the end of data_
processSolutionIDs(successful, true);
processSolutionIDs(failed, false);
// assign consecutive creation ranks
uint32_t rank = 0;
for (auto& item: data_)
for (auto& item : data_)
item.creation_rank = ++rank;
// the task may not report failure ids (in failed),
// but it may report the overall number of failures
num_failed_data_ = failed.size(); // needed to compute number of successes
num_failed_data_ = failed.size(); // needed to compute number of successes
num_failed_ = std::max(num_failed, num_failed_data_);
sortInternal();
}
void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &ids, bool successful)
{
void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t>& ids, bool successful) {
// ids are ordered by cost, insert them into data_ list sorted by id
double default_cost = successful ? std::numeric_limits<double>::quiet_NaN()
: std::numeric_limits<double>::infinity();
double default_cost =
successful ? std::numeric_limits<double>::quiet_NaN() : std::numeric_limits<double>::infinity();
uint32_t cost_rank = 0;
for (const uint32_t id : ids) {
uint32_t rank = successful ? ++cost_rank : std::numeric_limits<uint32_t>::max();
@ -666,9 +653,7 @@ void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &ids, b
}
}
bool RemoteSolutionModel::isVisible(const RemoteSolutionModel::Data &item) const
{
bool RemoteSolutionModel::isVisible(const RemoteSolutionModel::Data& item) const {
return std::isnan(item.cost) || item.cost <= max_cost_;
}
}

View File

@ -41,7 +41,9 @@
#include <memory>
#include <limits>
namespace ros { class ServiceClient; }
namespace ros {
class ServiceClient;
}
namespace moveit_rviz_plugin {
@ -50,7 +52,8 @@ class RemoteSolutionModel;
*
* filled via TaskDescription + TaskStatistics messages
*/
class RemoteTaskModel : public BaseTaskModel {
class RemoteTaskModel : public BaseTaskModel
{
Q_OBJECT
struct Node;
Node* const root_;
@ -59,56 +62,56 @@ class RemoteTaskModel : public BaseTaskModel {
std::map<uint32_t, Node*> id_to_stage_;
std::map<uint32_t, DisplaySolutionPtr> id_to_solution_;
inline Node* node(const QModelIndex &index) const;
inline Node* node(const QModelIndex& index) const;
QModelIndex index(const Node* n) const;
Node* node(uint32_t stage_id) const;
inline RemoteSolutionModel* getSolutionModel(uint32_t stage_id) const;
void setSolutionData(const moveit_task_constructor_msgs::SolutionInfo &info);
void setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info);
public:
RemoteTaskModel(const planning_scene::PlanningSceneConstPtr &scene, rviz::DisplayContext *display_context, QObject *parent = nullptr);
RemoteTaskModel(const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context,
QObject* parent = nullptr);
~RemoteTaskModel();
void setSolutionClient(ros::ServiceClient *client);
void setSolutionClient(ros::ServiceClient* client);
int rowCount(const QModelIndex &parent = QModelIndex()) const override;
int rowCount(const QModelIndex& parent = QModelIndex()) const override;
QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override;
QModelIndex parent(const QModelIndex &index) const override;
QModelIndex index(int row, int column, const QModelIndex& parent = QModelIndex()) const override;
QModelIndex parent(const QModelIndex& index) const override;
Qt::ItemFlags flags(const QModelIndex & index) const override;
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override;
Qt::ItemFlags flags(const QModelIndex& index) const override;
QVariant data(const QModelIndex& index, int role = Qt::DisplayRole) const override;
bool setData(const QModelIndex& index, const QVariant& value, int role = Qt::EditRole) override;
QModelIndex indexFromStageId(size_t id) const override;
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg);
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg);
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type& msg);
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type& msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg);
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
DisplaySolutionPtr getSolution(const QModelIndex &index) override;
DisplaySolutionPtr getSolution(const QModelIndex& index) override;
rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
};
/** Model representing solutions of a remote task */
class RemoteSolutionModel : public QAbstractTableModel {
class RemoteSolutionModel : public QAbstractTableModel
{
Q_OBJECT
struct Data {
struct Data
{
uint32_t id;
double cost; // nan if unknown, inf if failed
QString comment;
uint32_t creation_rank; // rank, ordered by creation
uint32_t cost_rank; // rank, ordering by cost
Data(uint32_t id, float cost, uint32_t cost_rank, const QString& name=QString())
: id(id), cost(cost), comment(name), creation_rank(0), cost_rank(cost_rank) {}
Data(uint32_t id, float cost, uint32_t cost_rank, const QString& name = QString())
: id(id), cost(cost), comment(name), creation_rank(0), cost_rank(cost_rank) {}
inline bool operator<(const Data& other) const {
return this->id < other.id;
}
inline bool operator<(const Data& other) const { return this->id < other.id; }
};
// successful and failed solutions ordered by id / creation
typedef std::list<Data> DataList;
@ -122,24 +125,24 @@ class RemoteSolutionModel : public QAbstractTableModel {
double max_cost_ = std::numeric_limits<double>::infinity();
std::vector<DataList::iterator> sorted_;
inline bool isVisible (const Data& item) const;
inline bool isVisible(const Data& item) const;
void processSolutionIDs(const std::vector<uint32_t>& ids, bool successful);
void sortInternal();
public:
RemoteSolutionModel(QObject *parent = nullptr);
RemoteSolutionModel(QObject* parent = nullptr);
uint numSuccessful() const { return data_.size() - num_failed_data_; }
uint numFailed() const { return num_failed_; }
int rowCount(const QModelIndex &parent = QModelIndex()) const override;
int columnCount(const QModelIndex &parent = QModelIndex()) const override;
int rowCount(const QModelIndex& parent = QModelIndex()) const override;
int columnCount(const QModelIndex& parent = QModelIndex()) const override;
QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
QVariant data(const QModelIndex& index, int role = Qt::DisplayRole) const override;
void sort(int column, Qt::SortOrder order) override;
void setSolutionData(uint32_t id, float cost, const QString &comment);
void processSolutionIDs(const std::vector<uint32_t> &successful, const std::vector<uint32_t> &failed, size_t num_failed);
void setSolutionData(uint32_t id, float cost, const QString& comment);
void processSolutionIDs(const std::vector<uint32_t>& successful, const std::vector<uint32_t>& failed,
size_t num_failed);
};
}

View File

@ -57,48 +57,43 @@
#include <OgreSceneNode.h>
#include <QTimer>
namespace moveit_rviz_plugin
{
namespace moveit_rviz_plugin {
TaskDisplay::TaskDisplay() : Display()
{
TaskDisplay::TaskDisplay() : Display() {
task_list_model_.reset(new TaskListModel);
task_list_model_->setSolutionClient(&get_solution_client);
MetaTaskListModel::instance().insertModel(task_list_model_.get(), this);
connect(task_list_model_.get(), SIGNAL(rowsInserted(QModelIndex,int,int)),
this, SLOT(onTasksInserted(QModelIndex,int,int)));
connect(task_list_model_.get(), SIGNAL(rowsAboutToBeRemoved(QModelIndex,int,int)),
this, SLOT(onTasksRemoved(QModelIndex,int,int)));
connect(task_list_model_.get(), SIGNAL(dataChanged(QModelIndex,QModelIndex)),
this, SLOT(onTaskDataChanged(QModelIndex,QModelIndex)));
connect(task_list_model_.get(), SIGNAL(rowsInserted(QModelIndex, int, int)), this,
SLOT(onTasksInserted(QModelIndex, int, int)));
connect(task_list_model_.get(), SIGNAL(rowsAboutToBeRemoved(QModelIndex, int, int)), this,
SLOT(onTasksRemoved(QModelIndex, int, int)));
connect(task_list_model_.get(), SIGNAL(dataChanged(QModelIndex, QModelIndex)), this,
SLOT(onTaskDataChanged(QModelIndex, QModelIndex)));
robot_description_property_ =
new rviz::StringProperty("Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
this, SLOT(changedRobotDescription()), this);
robot_description_property_ = new rviz::StringProperty(
"Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
this, SLOT(changedRobotDescription()), this);
task_solution_topic_property_ =
new rviz::RosTopicProperty("Task Solution Topic", "",
ros::message_traits::datatype<moveit_task_constructor_msgs::Solution>(),
"The topic on which task solutions (moveit_msgs::Solution messages) are received",
this, SLOT(changedTaskSolutionTopic()), this);
task_solution_topic_property_ = new rviz::RosTopicProperty(
"Task Solution Topic", "", ros::message_traits::datatype<moveit_task_constructor_msgs::Solution>(),
"The topic on which task solutions (moveit_msgs::Solution messages) are received", this,
SLOT(changedTaskSolutionTopic()), this);
trajectory_visual_.reset(new TaskSolutionVisualization(this, this));
connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)),
task_list_model_.get(), SLOT(highlightStage(size_t)));
connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)), task_list_model_.get(),
SLOT(highlightStage(size_t)));
tasks_property_ =
new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this);
tasks_property_ = new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this);
}
TaskDisplay::~TaskDisplay()
{
if (context_) TaskPanel::decDisplayCount();
TaskDisplay::~TaskDisplay() {
if (context_)
TaskPanel::decDisplayCount();
}
void TaskDisplay::onInitialize()
{
void TaskDisplay::onInitialize() {
Display::onInitialize();
trajectory_visual_->onInitialize(scene_node_, context_);
task_list_model_->setDisplayContext(context_);
@ -107,12 +102,10 @@ void TaskDisplay::onInitialize()
mainloop_jobs_.addJob([this]() { TaskPanel::incDisplayCount(context_->getWindowManager()); });
}
void TaskDisplay::loadRobotModel()
{
void TaskDisplay::loadRobotModel() {
rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString()));
if (!rdf_loader_->getURDF())
{
if (!rdf_loader_->getURDF()) {
this->setStatus(rviz::StatusProperty::Error, "Robot Model",
"Failed to load from parameter " + robot_description_property_->getString());
return;
@ -120,7 +113,7 @@ void TaskDisplay::loadRobotModel()
this->setStatus(rviz::StatusProperty::Ok, "Robot Model", "Successfully loaded");
const srdf::ModelSharedPtr& srdf =
rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model());
rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model());
robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf));
// Send to child class
@ -134,44 +127,37 @@ void TaskDisplay::loadRobotModel()
changedTaskSolutionTopic();
}
void TaskDisplay::reset()
{
void TaskDisplay::reset() {
Display::reset();
loadRobotModel();
trajectory_visual_->reset();
}
void TaskDisplay::save(rviz::Config config) const
{
void TaskDisplay::save(rviz::Config config) const {
Display::save(config);
}
void TaskDisplay::load(const rviz::Config &config)
{
void TaskDisplay::load(const rviz::Config& config) {
Display::load(config);
}
void TaskDisplay::onEnable()
{
void TaskDisplay::onEnable() {
Display::onEnable();
loadRobotModel();
calculateOffsetPosition();
}
void TaskDisplay::onDisable()
{
void TaskDisplay::onDisable() {
Display::onDisable();
trajectory_visual_->onDisable();
}
void TaskDisplay::fixedFrameChanged()
{
void TaskDisplay::fixedFrameChanged() {
Display::fixedFrameChanged();
calculateOffsetPosition();
}
void TaskDisplay::calculateOffsetPosition()
{
void TaskDisplay::calculateOffsetPosition() {
if (!robot_model_)
return;
@ -184,34 +170,29 @@ void TaskDisplay::calculateOffsetPosition()
scene_node_->setOrientation(orientation);
}
void TaskDisplay::update(float wall_dt, float ros_dt)
{
void TaskDisplay::update(float wall_dt, float ros_dt) {
Display::update(wall_dt, ros_dt);
mainloop_jobs_.executeJobs();
trajectory_visual_->update(wall_dt, ros_dt);
}
void TaskDisplay::setName(const QString& name)
{
void TaskDisplay::setName(const QString& name) {
BoolProperty::setName(name);
trajectory_visual_->setName(name);
}
void TaskDisplay::changedRobotDescription()
{
void TaskDisplay::changedRobotDescription() {
if (isEnabled())
reset();
else
loadRobotModel();
}
inline std::string getUniqueId(const std::string& process_id, const std::string& task_id)
{
inline std::string getUniqueId(const std::string& process_id, const std::string& task_id) {
return process_id + "/" + task_id;
}
void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg)
{
void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) {
const std::string id = getUniqueId(msg->process_id, msg->id);
mainloop_jobs_.addJob([this, id, msg]() {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
@ -219,8 +200,7 @@ void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDesc
});
}
void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg)
{
void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg) {
const std::string id = getUniqueId(msg->process_id, msg->id);
mainloop_jobs_.addJob([this, id, msg]() {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
@ -228,20 +208,18 @@ void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStati
});
}
void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg)
{
void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg) {
const std::string id = getUniqueId(msg->process_id, msg->task_id);
mainloop_jobs_.addJob([this, id, msg]() {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(id, *msg);
if (s) trajectory_visual_->showTrajectory(s, false);
if (s)
trajectory_visual_->showTrajectory(s, false);
return;
});
}
void TaskDisplay::changedTaskSolutionTopic()
{
void TaskDisplay::changedTaskSolutionTopic() {
// postpone setup until scene is well-defined
if (!trajectory_visual_->getScene())
return;
@ -271,20 +249,22 @@ void TaskDisplay::changedTaskSolutionTopic()
task_solution_sub = update_nh_.subscribe(base_ns + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this);
// service to request solutions
get_solution_client = update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(base_ns + GET_SOLUTION_SERVICE);
get_solution_client =
update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(base_ns + GET_SOLUTION_SERVICE);
setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received");
}
void TaskDisplay::setSolutionStatus(bool ok)
{
if (ok) setStatus(rviz::StatusProperty::Ok, "Solution", "Ok");
else setStatus(rviz::StatusProperty::Warn, "Solution", "Retrieval failed");
void TaskDisplay::setSolutionStatus(bool ok) {
if (ok)
setStatus(rviz::StatusProperty::Ok, "Solution", "Ok");
else
setStatus(rviz::StatusProperty::Warn, "Solution", "Retrieval failed");
}
void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last)
{
if (parent.isValid()) return; // only handle top-level items
void TaskDisplay::onTasksInserted(const QModelIndex& parent, int first, int last) {
if (parent.isValid())
return; // only handle top-level items
TaskListModel* m = static_cast<TaskListModel*>(sender());
for (; first <= last; ++first) {
@ -293,25 +273,25 @@ void TaskDisplay::onTasksInserted(const QModelIndex &parent, int first, int last
}
}
void TaskDisplay::onTasksRemoved(const QModelIndex &parent, int first, int last)
{
if (parent.isValid()) return; // only handle top-level items
void TaskDisplay::onTasksRemoved(const QModelIndex& parent, int first, int last) {
if (parent.isValid())
return; // only handle top-level items
for (; first <= last; ++first)
delete tasks_property_->takeChildAt(first);
}
void TaskDisplay::onTaskDataChanged(const QModelIndex &topLeft, const QModelIndex &bottomRight)
{
if (topLeft.parent().isValid()) return; // only handle top-level items
void TaskDisplay::onTaskDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight) {
if (topLeft.parent().isValid())
return; // only handle top-level items
for (int row = topLeft.row(); row <= bottomRight.row(); ++row) {
rviz::Property *child = tasks_property_->childAt(row);
rviz::Property* child = tasks_property_->childAt(row);
assert(child);
if (topLeft.column() <= 0 && 0 <= bottomRight.column()) // name changed
if (topLeft.column() <= 0 && 0 <= bottomRight.column()) // name changed
child->setName(topLeft.sibling(row, 0).data().toString());
if (topLeft.column() <= 1 && 1 <= bottomRight.column()) // #solutions changed
if (topLeft.column() <= 1 && 1 <= bottomRight.column()) // #solutions changed
child->setValue(topLeft.sibling(row, 1).data());
}
}

Some files were not shown because too many files have changed in this diff Show More