Merge branch 'master' into wip-python-api

This commit is contained in:
Robert Haschke 2020-11-26 12:05:03 +01:00
commit 8433e460e4
53 changed files with 1541 additions and 357 deletions

View File

@ -1,14 +1,10 @@
coverage:
precision: 2
round: up
range: "45...70"
status:
project:
default:
target: auto
# allow coverage to drop by this amount and still post success
threshold: 0.5%
base: auto
branches:
- master
if_not_found: success
if_ci_failed: error
only_pulls: false
informational: true
patch: off

View File

@ -65,8 +65,8 @@ public:
void add(Stage::pointer&& stage);
virtual bool insert(Stage::pointer&& stage, int before = -1);
virtual bool remove(int pos);
virtual bool remove(Stage* child);
virtual Stage::pointer remove(int pos);
virtual Stage::pointer remove(Stage* child);
virtual void clear();
void reset() override;

View File

@ -89,7 +89,7 @@ public:
const_iterator childByIndex(int index, bool for_insert = false) const;
/// remove child at given iterator position, returns fals if pos is invalid
bool remove(ContainerBasePrivate::const_iterator pos);
Stage::pointer remove(ContainerBasePrivate::const_iterator pos);
/// traversing all stages up to max_depth
bool traverseStages(const ContainerBase::StageCallback& processor, unsigned int cur_depth,

View File

@ -0,0 +1,174 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Hamburg University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Hamburg University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael 'v4hn' Goerner
Desc: Define implementations for general CostTerm's to use with Stage::setCostTerm()
*/
#pragma once
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/utils.h>
namespace moveit {
namespace task_constructor {
/** basic interface to compute costs for solutions
*
* If your cost term will only work on SubTrajectory solution objects,
* inherit from TrajectoryCostTerm instead.
*/
MOVEIT_CLASS_FORWARD(CostTerm);
class CostTerm
{
public:
CostTerm() = default;
CostTerm(std::nullptr_t) : CostTerm{} {}
virtual double operator()(const SubTrajectory& s, std::string& comment) const;
virtual double operator()(const SolutionSequence& s, std::string& comment) const;
virtual double operator()(const WrappedSolution& s, std::string& comment) const;
};
/** base class for cost terms that only work on SubTrajectory solutions
*
*/
class TrajectoryCostTerm : public CostTerm
{
public:
double operator()(const WrappedSolution& s, std::string& comment) const override;
};
class LambdaCostTerm : public TrajectoryCostTerm
{
public:
using SubTrajectorySignature = std::function<double(const SubTrajectory&, std::string&)>;
using SubTrajectoryShortSignature = std::function<double(const SubTrajectory&)>;
// accept lambdas according to either signature above
template <typename Term, typename Signature = decltype(signatureMatcher(std::declval<Term>()))>
LambdaCostTerm(const Term& t) : LambdaCostTerm{ Signature{ t } } {}
LambdaCostTerm(const SubTrajectorySignature&);
LambdaCostTerm(const SubTrajectoryShortSignature&);
double operator()(const SubTrajectory& s, std::string& comment) const override;
protected:
SubTrajectorySignature term_;
private:
template <typename T>
static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSignature{});
template <typename T>
static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySignature{});
};
namespace cost {
/// add a constant cost to each solution
class Constant : public CostTerm
{
public:
Constant(double c) : cost{ c } {};
double operator()(const SubTrajectory&, std::string&) const override;
double operator()(const SolutionSequence& s, std::string& comment) const override;
double operator()(const WrappedSolution& s, std::string& comment) const override;
double cost;
};
/// trajectory length (interpolated between waypoints)
class PathLength : public TrajectoryCostTerm
{
// TODO(v4hn): allow to consider specific joints only
public:
double operator()(const SubTrajectory&, std::string&) const override;
};
/// execution duration of the whole trajectory
class TrajectoryDuration : public TrajectoryCostTerm
{
public:
double operator()(const SubTrajectory&, std::string&) const override;
};
/** length of Cartesian trajection of a link */
class LinkMotion : public TrajectoryCostTerm
{
public:
LinkMotion(std::string link_name);
std::string link_name;
double operator()(const SubTrajectory&, std::string&) const override;
};
/** inverse distance to collision
*
* \arg with_world check distances to world objects or look at self-collisions
* \arg cumulative if true, compute clearance as aggregated distance of all bodies
* \arg group_property the name of the property which defines the group to look at
* \arg interface compute distances using START or END interface of solution *only*, instead of averaging over
* trajectory
* */
class Clearance : public TrajectoryCostTerm
{
public:
enum class Mode
{
AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */,
START_INTERFACE,
END_INTERFACE,
TRAJECTORY
};
Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group",
Mode mode = Mode::AUTO);
bool with_world;
bool cumulative;
std::string group_property;
Mode mode;
std::function<double(double)> distance_to_cost;
double operator()(const SubTrajectory& s, std::string& comment) const override;
};
} // namespace cost
} // namespace task_constructor
} // namespace moveit

View File

@ -138,6 +138,8 @@ private:
};
std::ostream& operator<<(std::ostream& os, const InitStageException& e);
MOVEIT_CLASS_FORWARD(CostTerm);
class LambdaCostTerm;
class ContainerBase;
class StagePrivate;
class Stage
@ -159,6 +161,7 @@ public:
PARENT = 2,
INTERFACE = 4,
};
virtual ~Stage();
/// auto-convert Stage to StagePrivate* when needed
@ -212,6 +215,15 @@ public:
/// remove function callback
void removeSolutionCallback(SolutionCallbackList::const_iterator which);
/** set method to determine costs for solutions of this stage */
void setCostTerm(const CostTermConstPtr& term);
// overload to accept appropriate lambda expressions
template <typename T, typename = std::enable_if_t<std::is_constructible<LambdaCostTerm, T>::value>>
void setCostTerm(T term) {
setCostTerm(std::make_shared<LambdaCostTerm>(term));
}
const ordered<SolutionBaseConstPtr>& solutions() const;
const std::list<SolutionBaseConstPtr>& failures() const;
size_t numFailures() const;
@ -227,8 +239,10 @@ public:
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)); }
/// analyze source of error and report accordingly
[[noreturn]] void reportPropertyError(const Property::error& e);
double getTotalComputeTime() const;
protected:

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/cost_queue.h>
#include <ros/ros.h>
@ -154,8 +155,13 @@ public:
total_compute_time_ += compute_stop_time - compute_start_time;
}
/** compute cost for solution through configured CostTerm */
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
protected:
Stage* me_; // associated/owning Stage instance
// associated/owning Stage instance
Stage* me_;
std::string name_;
PropertyMap properties_;
@ -163,6 +169,9 @@ protected:
InterfacePtr starts_;
InterfacePtr ends_;
// user-configurable cost estimator
CostTermConstPtr cost_term_;
// The total compute time
std::chrono::duration<double> total_compute_time_;

View File

@ -0,0 +1,61 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Hamburg University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holders nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael 'v4hn' Goerner */
#pragma once
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry>
namespace moveit {
namespace task_constructor {
namespace stages {
/** Generic Wrapper that passes on a solution
*
* Useful to set a custom CostTransform via Stage::setCostTerm
* to change a solution's cost without loosing the original value
*/
class PassThrough : public WrapperBase
{
public:
PassThrough(const std::string& name = "PassThrough", Stage::pointer&& child = Stage::pointer());
void onNewSolution(const SolutionBase& s) override;
};
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -192,10 +192,15 @@ private:
using base_type::remove_if;
};
class CostTerm;
class StagePrivate;
class ContainerBasePrivate;
/// abstract base class for solutions (primitive and sequences)
class SolutionBase
{
friend StagePrivate; // for set[Start|End]StateUnsafe
friend ContainerBasePrivate;
public:
virtual ~SolutionBase() = default;
@ -206,18 +211,22 @@ public:
template <Interface::Direction dir>
inline const InterfaceState::Solutions& trajectories() const;
/** set the solution's start_state_
*
* Must not be used with different states because it registers the solution with the state as well.
*/
inline void setStartState(const InterfaceState& state) {
// only allow setting once (by Stage)
assert(start_ == nullptr || start_ == &state);
start_ = &state;
const_cast<InterfaceState&>(state).addOutgoing(this);
setStartStateUnsafe(state);
}
/** set the solution's end_state_
*
* Must not be used with different states because it registers the solution with the state as well.
*/
inline void setEndState(const InterfaceState& state) {
// only allow setting once (by Stage)
assert(end_ == nullptr || end_ == &state);
end_ = &state;
const_cast<InterfaceState&>(state).addIncoming(this);
setEndStateUnsafe(state);
}
inline const Stage* creator() const { return creator_; }
@ -239,6 +248,9 @@ public:
Introspection* introspection = nullptr) const = 0;
void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const;
/// required to dispatch to type-specific CostTerm methods via vtable
virtual double computeCost(const CostTerm& cost, std::string& comment) const = 0;
/// order solutions by their cost
bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
@ -246,6 +258,18 @@ protected:
SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "")
: creator_(creator), cost_(cost), comment_(std::move(comment)) {}
/** unsafe setter for start_state_
*
* must only be used if the previously set state removes its link to this solution
*/
void setStartStateUnsafe(const InterfaceState& state);
/** unsafe setter for end_state_
*
* must only be used if the previously set state removes its link to this solution
*/
void setEndStateUnsafe(const InterfaceState& state);
private:
// back-pointer to creating stage, allows to access sub-solutions
Stage* creator_;
@ -276,6 +300,8 @@ public:
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
private:
// actual trajectory, might be empty
robot_trajectory::RobotTrajectoryConstPtr trajectory_;
@ -301,6 +327,8 @@ public:
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
const container_type& solutions() const { return subsolutions_; }
inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); }
@ -331,6 +359,8 @@ public:
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
const SolutionBase* wrapped() const { return wrapped_; }
private:

View File

@ -78,13 +78,14 @@ public:
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 = "", bool introspection = true,
Task(const std::string& ns = "", bool introspection = true,
ContainerBase::pointer&& container = std::make_unique<SerialContainer>("task pipeline"));
Task(Task&& other); // NOLINT(performance-noexcept-move-constructor)
Task& operator=(Task&& other); // NOLINT(performance-noexcept-move-constructor)
~Task() override;
std::string id() const;
const std::string& name() const { return stages()->name(); }
void setName(const std::string& name) { stages()->setName(name); }
const moveit::core::RobotModelConstPtr& getRobotModel() const;
/// setting the robot model also resets the task
@ -112,6 +113,9 @@ public:
using WrapperBase::removeSolutionCallback;
using WrapperBase::SolutionCallback;
using WrapperBase::setTimeout;
using WrapperBase::timeout;
/// reset all stages
void reset() final;
/// initialize all stages with given scene

View File

@ -53,15 +53,15 @@ class TaskPrivate : public WrapperBasePrivate
friend class Task;
public:
TaskPrivate(Task* me, const std::string& id);
const std::string& id() const { return id_; }
TaskPrivate(Task* me, const std::string& ns);
const std::string& ns() const { return ns_; }
const ContainerBase* stages() const;
protected:
static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
private:
std::string id_;
std::string ns_;
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
moveit::core::RobotModelConstPtr robot_model_;
bool preempt_requested_;

View File

@ -272,12 +272,6 @@ class TestTask(unittest.TestCase):
def test(self):
task = core.Task()
self.assertEqual(task.id, "")
task = core.Task("foo", True, core.SerialContainer())
self.assertEqual(task.id, "foo")
task = core.Task("task")
self.assertEqual(task.id, "task")
current = stages.CurrentState("current")
self.assertEqual(current.name, "current")
current.timeout = 1.23

View File

@ -89,8 +89,8 @@ void ContainerBase_insert(ContainerBase& self, std::auto_ptr<Stage> stage, int b
}
BOOST_PYTHON_FUNCTION_OVERLOADS(ContainerBase_insert_overloads, ContainerBase_insert, 2, 3)
Task* Task_init(const std::string& id, bool introspection, std::auto_ptr<ContainerBase> container) {
return new Task(id, introspection, std::unique_ptr<ContainerBase>{ container.release() });
Task* Task_init(const std::string& ns, bool introspection, std::auto_ptr<ContainerBase> container) {
return new Task(ns, introspection, std::unique_ptr<ContainerBase>{ container.release() });
}
void Task_add(Task& self, std::auto_ptr<Stage> stage) {
@ -209,7 +209,7 @@ void export_core() {
PropertyMap& (Task::*Task_getPropertyMap)() = &Task::properties;
bp::class_<Task, boost::noncopyable>("Task", bp::no_init)
.add_property("id", &Task::id)
.add_property("name", bp::make_function(&Task::name, bp::return_internal_reference<>()), &Task::setName)
// read-only access to properties + solutions, reference returned directly as pointer
.add_property("properties", bp::make_function(Task_getPropertyMap, bp::return_internal_reference<>()))
.add_property("solutions", bp::make_function(&Task::solutions, bp::return_internal_reference<>()))

View File

@ -19,6 +19,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/solvers/pipeline_planner.h
container.cpp
cost_terms.cpp
introspection.cpp
marker_tools.cpp
merge.cpp

View File

@ -120,6 +120,10 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to) {
solution->setCreator(me());
computeCost(*internal_from, *internal_to, *solution);
if (!storeSolution(solution))
return;
@ -139,8 +143,8 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
InterfaceState* external_to = find_or_create_external(internal_to, created_to);
// connect solution to start/end state
solution->setStartState(*external_from);
solution->setEndState(*external_to);
solution->setStartStateUnsafe(*external_from);
solution->setEndStateUnsafe(*external_to);
// spawn created states in external interfaces
if (created_from)
@ -200,20 +204,21 @@ bool ContainerBase::insert(Stage::pointer&& stage, int before) {
return true;
}
bool ContainerBasePrivate::remove(ContainerBasePrivate::const_iterator pos) {
Stage::pointer ContainerBasePrivate::remove(ContainerBasePrivate::const_iterator pos) {
if (pos == children_.end())
return false;
return Stage::pointer();
(*pos)->pimpl()->unparent();
children_.erase(pos);
return true;
Stage::pointer result = std::move(*children_.erase(pos, pos)); // stage from non-const iterator to pos
children_.erase(pos); // actually erase stage
return result;
}
bool ContainerBase::remove(int pos) {
Stage::pointer ContainerBase::remove(int pos) {
return pimpl()->remove(pimpl()->childByIndex(pos, false));
}
bool ContainerBase::remove(Stage* child) {
Stage::pointer ContainerBase::remove(Stage* child) {
auto it = pimpl()->children_.begin(), end = pimpl()->children_.end();
for (; it != end && it->get() != child; ++it)
;
@ -752,8 +757,8 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
ContainerBasePrivate::resolveInterface(expected);
switch (interfaceFlags()) {
ParallelContainerBasePrivate::resolveInterface(expected);
switch (requiredInterface()) {
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:
case UNKNOWN:
@ -801,6 +806,9 @@ void Merger::compute() {
}
void Merger::onNewSolution(const SolutionBase& s) {
if (s.isFailure()) // ignore failure solutions
return;
auto impl = pimpl();
switch (impl->interfaceFlags()) {
case PROPAGATE_FORWARDS:
@ -817,8 +825,8 @@ void Merger::onNewSolution(const SolutionBase& s) {
void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
const SubTrajectory* trajectory = dynamic_cast<const SubTrajectory*>(&s);
if (!trajectory) {
ROS_ERROR_NAMED("Merger", "Only simple trajectories are supported");
if (!trajectory || !trajectory->trajectory()) {
ROS_ERROR_NAMED("Merger", "Only simple, valid trajectories are supported");
return;
}
@ -857,6 +865,7 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) {
// generate target state
planning_scene::PlanningScenePtr to = from->scene()->diff();
if (t.trajectory() && !t.trajectory()->empty())
to->setCurrentState(t.trajectory()->getLastWayPoint());
StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared<SubTrajectory>(std::move(t)));
}
@ -864,11 +873,12 @@ void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) {
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) {
// generate target state
planning_scene::PlanningScenePtr from = to->scene()->diff();
if (t.trajectory() && !t.trajectory()->empty())
from->setCurrentState(t.trajectory()->getFirstWayPoint());
StagePrivate::sendBackward(InterfaceState(from), *to, std::make_shared<SubTrajectory>(std::move(t)));
}
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& s) {
void MergerPrivate::onNewGeneratorSolution(const SolutionBase& /* s */) {
// TODO: implement in similar fashion as onNewPropagateSolution(), but also merge start/end states
}
@ -914,32 +924,38 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
// transform vector of SubTrajectories into vector of RobotTrajectories
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
sub_trajectories.reserve(sub_solutions.size());
for (const auto& sub : sub_solutions) {
// TODO: directly skip failures in mergeAnyCombination() or even earlier
if (sub->isFailure())
return;
if (sub->trajectory())
for (const auto& sub : sub_solutions)
sub_trajectories.push_back(sub->trajectory());
}
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged;
try {
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
} catch (const std::runtime_error& e) {
ROS_INFO_STREAM_NAMED("Merger", this->name() << "Merging failed: " << e.what());
SubTrajectory t;
t.markAsFailure();
t.setComment(e.what());
spawner(std::move(t));
return;
}
if (jmg_merged_.get() != jmg)
jmg_merged_.reset(jmg);
if (!merged)
return;
assert(merged);
SubTrajectory t(merged);
// check merged trajectory for collisions
if (!start_scene->isPathValid(*merged))
return;
SubTrajectory t(merged);
std::vector<std::size_t> invalid_index;
if (!start_scene->isPathValid(*merged, "", true, &invalid_index)) {
t.markAsFailure();
std::ostringstream oss;
oss << "Invalid waypoint(s): ";
if (invalid_index.size() == merged->getWayPointCount())
oss << "all";
else for (size_t i : invalid_index)
oss << i << ", ";
t.setComment(oss.str());
} else {
// accumulate costs and markers
double costs = 0.0;
for (const auto& sub : sub_solutions) {
@ -947,6 +963,7 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end());
}
t.setCost(costs);
}
spawner(std::move(t));
}
} // namespace task_constructor

250
core/src/cost_terms.cpp Normal file
View File

@ -0,0 +1,250 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Hamburg University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holders nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner */
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/collision_detection/collision_common.h>
#include <Eigen/Geometry>
#include <boost/format.hpp>
namespace moveit {
namespace task_constructor {
double CostTerm::operator()(const SubTrajectory& s, std::string&) const {
return s.cost();
}
double CostTerm::operator()(const SolutionSequence& s, std::string& comment) const {
double cost{ 0.0 };
std::string subcomment;
for (auto& solution : s.solutions()) {
cost += solution->computeCost((*this), subcomment);
if (!subcomment.empty()) {
if (!comment.empty())
comment.append(", ");
comment.append(subcomment);
subcomment.clear();
}
}
return cost;
}
double CostTerm::operator()(const WrappedSolution& s, std::string& /*comment*/) const {
return s.cost();
}
double TrajectoryCostTerm::operator()(const WrappedSolution& s, std::string& comment) const {
return s.wrapped()->computeCost(*this, comment);
}
LambdaCostTerm::LambdaCostTerm(const SubTrajectorySignature& term)
: term_{ [term](const SolutionBase& s, std::string& c) { return term(static_cast<const SubTrajectory&>(s), c); } } {}
LambdaCostTerm::LambdaCostTerm(const SubTrajectoryShortSignature& term)
: term_{ [term](const SolutionBase& s, std::string&) { return term(static_cast<const SubTrajectory&>(s)); } } {}
double LambdaCostTerm::operator()(const SubTrajectory& s, std::string& comment) const {
assert(bool{ term_ });
return term_(s, comment);
}
namespace cost {
double Constant::operator()(const SubTrajectory&, std::string&) const {
return cost;
}
double Constant::operator()(const SolutionSequence&, std::string&) const {
return cost;
}
double Constant::operator()(const WrappedSolution&, std::string&) const {
return cost;
}
double PathLength::operator()(const SubTrajectory& s, std::string&) const {
const auto& traj = s.trajectory();
if (traj == nullptr)
return 0.0;
double path_length{ 0.0 };
for (size_t i = 1; i < traj->getWayPointCount(); ++i)
path_length += traj->getWayPoint(i - 1).distance(traj->getWayPoint(i));
return path_length;
}
double TrajectoryDuration::operator()(const SubTrajectory& s, std::string&) const {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
}
LinkMotion::LinkMotion(std::string link) : link_name{ std::move(link) } {}
double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const {
const auto& traj{ s.trajectory() };
if (traj == nullptr || traj->getWayPointCount() == 0)
return 0.0;
if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) {
boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory");
desc % link_name;
comment = desc.str();
return std::numeric_limits<double>::infinity();
}
double distance{ 0.0 };
Eigen::Vector3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() };
for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) {
const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() };
distance += (new_position - position).norm();
position = new_position;
}
return distance;
}
Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode)
: with_world{ with_world }
, cumulative{ cumulative }
, group_property{ group_property }
, mode{ mode }
, distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {}
double Clearance::operator()(const SubTrajectory& s, std::string& comment) const {
const std::string PREFIX{ "Clearance: " };
collision_detection::DistanceRequest request;
request.type =
cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL;
const auto& state{ (mode == Mode::END_INTERFACE) ? s.end() : s.start() };
// prefer interface state property over stage property to find group_name
// TODO: This pattern is general enough to justify its own interface (in the properties?).
auto& state_properties{ state->properties() };
auto& stage_properties{ s.creator()->properties() };
request.group_name = state_properties.hasProperty(group_property) ?
state_properties.get<std::string>(group_property) :
stage_properties.get<std::string>(group_property);
// look at all forbidden collisions involving group_name
request.enableGroup(state->scene()->getRobotModel());
request.acm = &state->scene()->getAllowedCollisionMatrix();
// compute relevant distance data for state & robot
auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) {
collision_detection::DistanceResult result;
if (with_world)
#if MOVEIT_MASTER
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
#else
state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(),
robot);
#endif
else
#if MOVEIT_MASTER
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
#else
state->scene()->getCollisionRobot()->distanceSelf(request, result, robot);
#endif
if (result.minimum_distance.distance <= 0) {
return result.minimum_distance;
}
if (cumulative) {
double distance{ 0.0 };
for (const auto& distance_of_pair : result.distances) {
assert(distance_of_pair.second.size() == 1);
distance += distance_of_pair.second[0].distance;
}
result.minimum_distance.distance = distance;
}
return result.minimum_distance;
} };
auto collision_comment{ [=](const auto& distance) {
boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" };
desc % distance.link_names[0] % distance.link_names[1];
return desc.str();
} };
double distance{ 0.0 };
if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE ||
(mode == Mode::AUTO && s.trajectory() == nullptr)) {
auto distance_data{ check_distance(state, state->scene()->getCurrentState()) };
if (distance_data.distance < 0) {
comment = collision_comment(distance_data);
return std::numeric_limits<double>::infinity();
}
distance = distance_data.distance;
if (!cumulative) {
boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" };
desc % distance % distance_data.link_names[0] % distance_data.link_names[1];
comment = desc.str();
} else {
comment = PREFIX + "cumulative distance " + std::to_string(distance);
}
} else { // check trajectory
for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) {
auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i));
if (distance_data.distance < 0) {
comment = collision_comment(distance_data);
return std::numeric_limits<double>::infinity();
}
distance += distance_data.distance;
}
distance /= s.trajectory()->getWayPointCount();
boost::format desc(PREFIX + "average%1% distance: %2%");
desc % (cumulative ? " cumulative" : "") % distance;
comment = desc.str();
}
return distance_to_cost(distance);
}
} // namespace cost
} // namespace task_constructor
} // namespace moveit

View File

@ -46,26 +46,29 @@
#include <ros/service.h>
#include <moveit/planning_scene/planning_scene.h>
#include <sstream>
#include <boost/bimap.hpp>
namespace moveit {
namespace task_constructor {
namespace {
std::string getProcessId() {
std::string getTaskId(const TaskPrivate* task) {
std::ostringstream oss;
char our_hostname[256] = { 0 };
gethostname(our_hostname, sizeof(our_hostname) - 1);
return std::to_string(getpid()) + "@" + our_hostname;
oss << our_hostname << "_" << getpid() << "_" << reinterpret_cast<std::size_t>(task);
return oss.str();
}
} // namespace
class IntrospectionPrivate
{
public:
IntrospectionPrivate(const TaskPrivate* task)
: nh_(std::string("~/") + task->id()) // topics + services are advertised in private namespace
IntrospectionPrivate(const TaskPrivate* task, Introspection* self)
: nh_(std::string("~/") + task->ns()) // topics + services are advertised in private namespace
, task_(task)
, process_id_(getProcessId()) {
, task_id_(getTaskId(task)) {
task_description_publisher_ =
nh_.advertise<moveit_task_constructor_msgs::TaskDescription>(DESCRIPTION_TOPIC, 2, true);
// send reset message as early as possible to give subscribers time to see it
@ -75,14 +78,17 @@ public:
nh_.advertise<moveit_task_constructor_msgs::TaskStatistics>(STATISTICS_TOPIC, 1, true);
solution_publisher_ = nh_.advertise<moveit_task_constructor_msgs::Solution>(SOLUTION_TOPIC, 1, true);
get_solution_service_ =
nh_.advertiseService(std::string(GET_SOLUTION_SERVICE "_") + task_id_, &Introspection::getSolution, self);
resetMaps();
}
~IntrospectionPrivate() { indicateReset(); }
void indicateReset() {
// send empty task description message to indicate reset
::moveit_task_constructor_msgs::TaskDescription msg;
msg.process_id = process_id_;
msg.id = task_->id();
msg.task_id = task_id_;
task_description_publisher_.publish(msg);
}
@ -97,7 +103,7 @@ public:
ros::NodeHandle nh_;
/// associated task
const TaskPrivate* task_;
const std::string process_id_;
const std::string task_id_;
/// publish task detailed description and current state
ros::Publisher task_description_publisher_;
@ -112,9 +118,7 @@ public:
boost::bimap<uint32_t, const SolutionBase*> id_solution_bimap_;
};
Introspection::Introspection(const TaskPrivate* task) : impl(new IntrospectionPrivate(task)) {
impl->get_solution_service_ = impl->nh_.advertiseService(GET_SOLUTION_SERVICE, &Introspection::getSolution, this);
}
Introspection::Introspection(const TaskPrivate* task) : impl(new IntrospectionPrivate(task, this)) {}
Introspection::~Introspection() {
delete impl;
@ -143,8 +147,7 @@ void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, co
s.fillMessage(msg, this);
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
msg.process_id = impl->process_id_;
msg.task_id = impl->task_->id();
msg.task_id = impl->task_id_;
}
void Introspection::publishSolution(const SolutionBase& s) {
@ -242,8 +245,7 @@ Introspection::fillTaskDescription(moveit_task_constructor_msgs::TaskDescription
msg.stages.clear();
impl->task_->stages()->traverseRecursively(stage_processor);
msg.id = impl->task_->id();
msg.process_id = impl->process_id_;
msg.task_id = impl->task_id_;
return msg;
}
@ -263,8 +265,7 @@ Introspection::fillTaskStatistics(moveit_task_constructor_msgs::TaskStatistics&
msg.stages.clear();
impl->task_->stages()->traverseRecursively(stage_processor);
msg.id = impl->task_->id();
msg.process_id = impl->process_id_;
msg.task_id = impl->task_id_;
return msg;
}
} // namespace task_constructor

View File

@ -97,7 +97,12 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) {
}
StagePrivate::StagePrivate(Stage* me, const std::string& name)
: me_(me), name_(name), total_compute_time_{}, parent_(nullptr), introspection_(nullptr) {}
: me_{ me }
, name_{ name }
, cost_term_{ std::make_unique<CostTerm>() }
, total_compute_time_{}
, parent_{ nullptr }
, introspection_{ nullptr } {}
InterfaceFlags StagePrivate::interfaceFlags() const {
InterfaceFlags f;
@ -125,7 +130,6 @@ void StagePrivate::validateConnectivity() const {
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
solution->setCreator(me());
if (introspection_)
introspection_->registerSolution(*solution);
@ -142,14 +146,21 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) {
assert(nextStarts());
solution->setCreator(me());
computeCost(from, to, *solution);
if (!storeSolution(solution))
return; // solution dropped
me()->forwardProperties(from, to);
auto to_it = states_.insert(states_.end(), std::move(to));
solution->setStartState(from);
solution->setEndState(*to_it);
// register stored interfaces with solution
solution->setStartStateUnsafe(from);
solution->setEndStateUnsafe(*to_it);
if (!solution->isFailure())
nextStarts()->add(*to_it);
@ -159,14 +170,20 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution) {
assert(prevEnds());
solution->setCreator(me());
computeCost(from, to, *solution);
if (!storeSolution(solution))
return; // solution dropped
me()->forwardProperties(to, from);
auto from_it = states_.insert(states_.end(), std::move(from));
solution->setStartState(*from_it);
solution->setEndState(to);
solution->setStartStateUnsafe(*from_it);
solution->setEndStateUnsafe(to);
if (!solution->isFailure())
prevEnds()->add(*from_it);
@ -176,14 +193,19 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
assert(prevEnds() && nextStarts());
solution->setCreator(me());
computeCost(state, state, *solution);
if (!storeSolution(solution))
return; // solution dropped
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
auto to = states_.insert(states_.end(), std::move(state));
solution->setStartState(*from);
solution->setEndState(*to);
solution->setStartStateUnsafe(*from);
solution->setEndStateUnsafe(*to);
if (!solution->isFailure()) {
prevEnds()->add(*from);
@ -194,11 +216,15 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
}
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
solution->setCreator(me());
computeCost(from, to, *solution);
if (!storeSolution(solution))
return; // solution dropped
solution->setStartState(from);
solution->setEndState(to);
solution->setStartStateUnsafe(from);
solution->setEndStateUnsafe(to);
newSolution(solution);
}
@ -212,6 +238,30 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) {
parent()->onNewSolution(*solution);
}
void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) {
// no reason to compute costs for a failed solution
if (solution.isFailure())
return;
// chicken-and-egg problem: we don't know whether/where we will store the solution yet,
// but need to store it properly to compute the cost with a clean interface:
// set state copies for solution before computing cost:
InterfaceState tmp_from{ from }, tmp_to{ to };
solution.setStartState(tmp_from);
solution.setEndState(tmp_to);
std::string comment;
assert(cost_term_);
solution.setCost(solution.computeCost(*cost_term_, comment));
// If a comment was specified, add it to the solution
if (!comment.empty() && !solution.comment().empty()) {
solution.setComment(solution.comment() + " (" + comment + ")");
} else if (!comment.empty()) {
solution.setComment(comment);
}
}
Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
assert(impl);
auto& p = properties();
@ -254,8 +304,9 @@ void Stage::reset() {
}
void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
// init properties once from parent
auto impl = pimpl();
// init properties once from parent
impl->properties_.reset();
if (impl->parent()) {
try {
@ -308,6 +359,13 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) {
pimpl()->solution_cbs_.erase(which);
}
void Stage::setCostTerm(const CostTermConstPtr& term) {
if (!term)
pimpl()->cost_term_ = std::make_unique<CostTerm>();
else
pimpl()->cost_term_ = std::move(term);
}
const ordered<SolutionBaseConstPtr>& Stage::solutions() const {
return pimpl()->solutions_;
}

View File

@ -9,6 +9,7 @@ add_library(${PROJECT_NAME}_stages
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
${PROJECT_INCLUDE}/stages/generate_place_pose.h
${PROJECT_INCLUDE}/stages/compute_ik.h
${PROJECT_INCLUDE}/stages/passthrough.h
${PROJECT_INCLUDE}/stages/predicate_filter.h
${PROJECT_INCLUDE}/stages/connect.h
@ -28,6 +29,7 @@ add_library(${PROJECT_NAME}_stages
generate_grasp_pose.cpp
generate_place_pose.cpp
compute_ik.cpp
passthrough.cpp
predicate_filter.cpp
connect.cpp

View File

@ -40,12 +40,16 @@
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/cost_terms.h>
namespace moveit {
namespace task_constructor {
namespace stages {
Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) {
setTimeout(1.0);
setCostTerm(std::make_unique<cost::PathLength>());
auto& p = properties();
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
@ -82,9 +86,8 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
}
}
if (!errors && groups.size() >= 2) { // enable merging?
if (!errors && groups.size() >= 2 && !merged_jmg_) { // enable merging?
try {
merged_jmg_.reset();
merged_jmg_.reset(task_constructor::merge(groups));
} catch (const std::runtime_error& e) {
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging.");
@ -181,15 +184,6 @@ Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConst
planning_scene::PlanningSceneConstPtr start_ps = *scene_it;
const InterfaceState* state = &from;
// calculate cost
double cost = 0;
for (const auto& trajectory : sub_trajectories) {
if (!trajectory)
continue;
for (const double& distance : trajectory->getWayPointDurations())
cost += distance;
}
SolutionSequence::container_type sub_solutions;
for (const auto& sub : sub_trajectories) {
planning_scene::PlanningSceneConstPtr end_ps = *++scene_it;
@ -212,24 +206,15 @@ Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConst
start_ps = end_ps;
}
return std::make_shared<SolutionSequence>(std::move(sub_solutions), cost);
return std::make_shared<SolutionSequence>(std::move(sub_solutions));
}
SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const moveit::core::RobotState& state) {
// calculate cost
double cost = 0;
for (const auto& trajectory : sub_trajectories) {
if (!trajectory)
continue;
for (const double& distance : trajectory->getWayPointDurations())
cost += distance;
}
// no need to merge if there is only a single sub trajectory
if (sub_trajectories.size() == 1)
return std::make_shared<SubTrajectory>(sub_trajectories[0], cost);
return std::make_shared<SubTrajectory>(sub_trajectories[0]);
auto jmg = merged_jmg_.get();
assert(jmg);
@ -242,7 +227,7 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
properties().get<moveit_msgs::Constraints>("path_constraints")))
return SubTrajectoryPtr();
return std::make_shared<SubTrajectory>(trajectory, cost);
return std::make_shared<SubTrajectory>(trajectory);
}
} // namespace stages
} // namespace task_constructor

View File

@ -40,6 +40,8 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/cost_terms.h>
#include <rviz_marker_tools/marker_creation.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
@ -53,6 +55,9 @@ namespace task_constructor {
namespace stages {
FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) {
// TODO: possibly weight solutions based on the required displacement?
setCostTerm(std::make_unique<cost::Constant>(0.0));
auto& p = properties();
p.declare<double>("max_penetration", "maximally corrected penetration depth");
p.declare<geometry_msgs::Vector3>("direction", "direction vector to use for corrections");

View File

@ -36,7 +36,9 @@
#include <moveit/task_constructor/stages/fixed_cartesian_poses.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
@ -47,6 +49,8 @@ namespace stages {
using PosesList = std::vector<geometry_msgs::PoseStamped>;
FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0));
auto& p = properties();
p.declare<PosesList>("poses", PosesList(), "target poses to spawn");
}

View File

@ -35,13 +35,17 @@
/* Authors: Robert Haschke */
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit {
namespace task_constructor {
namespace stages {
FixedState::FixedState(const std::string& name) : Generator(name) {}
FixedState::FixedState(const std::string& name) : Generator(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0));
}
void FixedState::setState(const planning_scene::PlanningScenePtr& scene) {
scene_ = scene;

View File

@ -36,6 +36,7 @@
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
@ -45,6 +46,8 @@ namespace task_constructor {
namespace stages {
GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0));
auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states");
}

View File

@ -38,13 +38,17 @@
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit {
namespace task_constructor {
namespace stages {
ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {}
ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0));
}
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)));

View File

@ -37,6 +37,8 @@
*/
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
@ -47,6 +49,8 @@ namespace stages {
MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name), planner_(planner) {
setCostTerm(std::make_unique<cost::PathLength>());
auto& p = properties();
p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("group", "name of planning group");
@ -291,13 +295,6 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
robot_trajectory->reverse();
solution.setTrajectory(robot_trajectory);
// set cost
double cost = 0;
for (const double& distance : robot_trajectory->getWayPointDurations()) {
cost += distance;
}
solution.setCost(cost);
if (!success)
solution.markAsFailure();
return true;

View File

@ -37,6 +37,8 @@
*/
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
@ -48,6 +50,8 @@ namespace stages {
MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name), planner_(planner) {
setCostTerm(std::make_unique<cost::PathLength>());
auto& p = properties();
p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("group", "name of planning group");
@ -248,19 +252,17 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
}
// store result
if (!robot_trajectory && storeFailures()) {
robot_trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, jmg);
robot_trajectory->addSuffixWayPoint(state.scene()->getCurrentState(), 0.0);
robot_trajectory->addSuffixWayPoint(scene->getCurrentState(), 1.0);
}
if (robot_trajectory) {
scene->setCurrentState(robot_trajectory->getLastWayPoint());
if (dir == BACKWARD)
robot_trajectory->reverse();
solution.setTrajectory(robot_trajectory);
// set cost
double cost = 0;
for (const double& distance : robot_trajectory->getWayPointDurations()) {
cost += distance;
}
solution.setCost(cost);
if (!success)
solution.markAsFailure();

View File

@ -0,0 +1,65 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Hamburg University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holders nor the names of their
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Michael 'v4hn' Goerner */
#include <moveit/task_constructor/stages/passthrough.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <chrono>
#include <functional>
#include <iterator>
#include <ros/console.h>
namespace moveit {
namespace task_constructor {
namespace stages {
PassThrough::PassThrough(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {}
void PassThrough::onNewSolution(const SolutionBase& s) {
this->liftSolution(s);
}
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -141,6 +141,16 @@ void SolutionBase::setCreator(Stage* creator) {
creator_ = creator;
}
void SolutionBase::setStartStateUnsafe(const InterfaceState& state) {
start_ = &state;
const_cast<InterfaceState&>(state).addOutgoing(this);
}
void SolutionBase::setEndStateUnsafe(const InterfaceState& state) {
end_ = &state;
const_cast<InterfaceState&>(state).addIncoming(this);
}
void SolutionBase::setCost(double cost) {
cost_ = cost;
}
@ -168,6 +178,10 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Int
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
}
double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {
return f(*this, comment);
}
void SolutionSequence::push_back(const SolutionBase& solution) {
subsolutions_.push_back(&solution);
}
@ -207,6 +221,10 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg,
}
}
double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) const {
return f(*this, comment);
}
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection) const {
wrapped_->fillMessage(solution, introspection);
@ -217,5 +235,10 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0);
solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg));
}
double WrappedSolution::computeCost(const CostTerm& f, std::string& comment) const {
return f(*this, comment);
}
} // namespace task_constructor
} // namespace moveit

View File

@ -71,8 +71,8 @@ std::string rosNormalizeName(const std::string& name) {
namespace moveit {
namespace task_constructor {
TaskPrivate::TaskPrivate(Task* me, const std::string& id)
: WrapperBasePrivate(me, std::string()), id_(rosNormalizeName(id)), preempt_requested_(false) {}
TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
: WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {}
void swap(StagePrivate*& lhs, StagePrivate*& rhs) {
// It only makes sense to swap pimpl instances of a Task!
@ -104,10 +104,9 @@ const ContainerBase* TaskPrivate::stages() const {
return children().empty() ? nullptr : static_cast<ContainerBase*>(children().front().get());
}
Task::Task(const std::string& id, bool introspection, ContainerBase::pointer&& container)
: WrapperBase(new TaskPrivate(this, id), std::move(container)) {
if (!id.empty())
stages()->setName(id);
Task::Task(const std::string& ns, bool introspection, ContainerBase::pointer&& container)
: WrapperBase(new TaskPrivate(this, ns), std::move(container)) {
setTimeout(std::numeric_limits<double>::max());
// monitor state on commandline
// addTaskCallback(std::bind(&Task::printState, this, std::ref(std::cout)));
@ -174,6 +173,7 @@ planning_pipeline::PlanningPipelinePtr Task::createPlanner(const robot_model::Ro
Task::~Task() {
auto impl = pimpl();
impl->introspection_.reset(); // stop introspection
clear(); // remove all stages
impl->robot_model_.reset();
// only destroy loader after all references to the model are gone!
@ -296,8 +296,10 @@ bool Task::plan(size_t max_solutions) {
init();
impl->preempt_requested_ = false;
while (ros::ok() && !impl->preempt_requested_ && canCompute() &&
(max_solutions == 0 || numSolutions() < max_solutions)) {
const double available_time = timeout();
const auto start_time = std::chrono::steady_clock::now();
while (!impl->preempt_requested_ && canCompute() && (max_solutions == 0 || numSolutions() < max_solutions) &&
std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() < available_time) {
compute();
for (const auto& cb : impl->task_cbs_)
cb(*this);
@ -355,10 +357,6 @@ void Task::setProperty(const std::string& name, const boost::any& value) {
wrapped()->setProperty(name, value);
}
std::string Task::id() const {
return pimpl()->id();
}
const core::RobotModelConstPtr& Task::getRobotModel() const {
auto impl = pimpl();
return impl->robot_model_;

View File

@ -24,6 +24,8 @@ if (CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp)
target_link_libraries(${PROJECT_NAME}-test-cost_terms ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
# building these integration tests works without moveit config packages
add_executable(pick_ur5 pick_ur5.cpp)

View File

@ -8,6 +8,8 @@
#include "gtest_value_printers.h"
#include <gtest/gtest.h>
#include <initializer_list>
#include <chrono>
#include <thread>
using namespace moveit::task_constructor;
@ -15,14 +17,18 @@ static unsigned int MOCK_ID = 0;
class GeneratorMockup : public Generator
{
moveit::core::RobotModelConstPtr robot;
int runs = 0;
public:
GeneratorMockup(int runs = 0) : Generator("generator " + std::to_string(++MOCK_ID)), runs(runs) {}
void init(const moveit::core::RobotModelConstPtr& robot_model) override { robot = robot_model; }
bool canCompute() const override { return runs > 0; }
void compute() override {
if (runs > 0)
if (runs > 0) {
--runs;
spawn(InterfaceState(std::make_shared<planning_scene::PlanningScene>(robot)), SubTrajectory());
}
}
};
@ -33,7 +39,9 @@ public:
: MonitoringGenerator("monitoring generator " + std::to_string(++MOCK_ID), monitored) {}
bool canCompute() const override { return false; }
void compute() override {}
void onNewSolution(const SolutionBase& /*solution*/) override {}
void onNewSolution(const SolutionBase& s) override {
spawn(InterfaceState(s.end()->scene()->diff()), SubTrajectory());
}
};
class PropagatorMockup : public PropagatingEitherWay
@ -44,13 +52,17 @@ class PropagatorMockup : public PropagatingEitherWay
public:
PropagatorMockup(int fw = 0, int bw = 0)
: PropagatingEitherWay("propagate " + std::to_string(++MOCK_ID)), fw_runs(fw), bw_runs(bw) {}
void computeForward(const InterfaceState& /* from */) override {
if (fw_runs > 0)
void computeForward(const InterfaceState& from) override {
if (fw_runs > 0) {
--fw_runs;
sendForward(from, InterfaceState(from.scene()->diff()), SubTrajectory());
}
void computeBackward(const InterfaceState& /* from */) override {
if (bw_runs > 0)
}
void computeBackward(const InterfaceState& to) override {
if (bw_runs > 0) {
--bw_runs;
sendBackward(InterfaceState(to.scene()->diff()), to, SubTrajectory());
}
}
};
class ForwardMockup : public PropagatorMockup
@ -70,15 +82,29 @@ public:
}
};
// ForwardMockup that takes a while for its computation
class TimedForwardMockup : public ForwardMockup
{
std::chrono::milliseconds duration_;
public:
TimedForwardMockup(std::chrono::milliseconds duration) : ForwardMockup(1000), duration_(duration) {}
void computeForward(const InterfaceState& from) override {
std::this_thread::sleep_for(duration_);
ForwardMockup::computeForward(from);
}
};
class ConnectMockup : public Connecting
{
int runs = 0;
public:
ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(++MOCK_ID)), runs(runs) {}
void compute(const InterfaceState& /* from */, const InterfaceState& /* to */) override {
void compute(const InterfaceState& from, const InterfaceState& to) override {
if (runs > 0)
--runs;
connect(from, to, std::make_shared<SubTrajectory>());
}
};
@ -673,13 +699,13 @@ TEST(Task, reuse) {
ref->setState(scene);
t.add(Stage::pointer(ref));
t.add(std::make_unique<ConnectMockup>());
t.add(std::make_unique<ConnectMockup>(2));
t.add(std::make_unique<MonitoringGeneratorMockup>(ref));
};
try {
configure(t);
t.plan(1);
EXPECT_TRUE(t.plan(1));
t = Task("second");
t.setRobotModel(robot_model);
@ -688,8 +714,43 @@ TEST(Task, reuse) {
EXPECT_EQ(static_cast<void*>(t.stages()->pimpl()->parent()), static_cast<void*>(&t));
configure(t);
t.plan(1);
EXPECT_TRUE(t.plan(1));
} catch (const InitStageException& e) {
ADD_FAILURE() << "InitStageException:" << std::endl << e << t;
}
}
TEST(Task, timeout) {
// create dummy robot model
moveit::core::RobotModelBuilder builder("robot", "base");
builder.addChain("base->a->b->c", "continuous");
builder.addGroupChain("base", "c", "group");
Task t;
t.setRobotModel(builder.build());
auto timeout = std::chrono::milliseconds(10);
t.add(std::make_unique<GeneratorMockup>(100)); // allow up to 100 solutions spawned
t.add(std::make_unique<TimedForwardMockup>(timeout));
// no timeout, but limited number of solutions
EXPECT_TRUE(t.plan(3));
EXPECT_EQ(t.solutions().size(), 3u);
// zero timeout fails
t.reset();
t.setTimeout(0.0);
EXPECT_FALSE(t.plan());
// time for 1 solution
t.reset();
t.setTimeout(std::chrono::duration<double>(timeout).count());
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.solutions().size(), 1u);
// time for 2 solutions
t.reset();
t.setTimeout(std::chrono::duration<double>(2 * timeout).count());
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.solutions().size(), 2u);
}

View File

@ -0,0 +1,351 @@
#include "models.h"
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/stages/passthrough.h>
#include <moveit/planning_scene/planning_scene.h>
#include <geometry_msgs/PoseStamped.h>
#include <gtest/gtest.h>
using namespace moveit::task_constructor;
using namespace planning_scene;
const double STAGE_COST{ 7.0 };
const double TRAJECTORY_DURATION{ 9.0 };
class GeneratorMockup : public Generator
{
PlanningScenePtr ps;
InterfacePtr prev;
InterfacePtr next;
public:
GeneratorMockup() : Generator("generator") {
prev.reset(new Interface);
next.reset(new Interface);
pimpl()->setPrevEnds(prev);
pimpl()->setNextStarts(next);
}
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
Generator::init(robot_model);
}
bool canCompute() const override { return true; }
void compute() override {
InterfaceState state(ps);
spawn(std::move(state), STAGE_COST);
}
};
class ConnectMockup : public Connecting
{
using Connecting::Connecting;
void compute(const InterfaceState& from, const InterfaceState& to) override {
auto solution{ std::make_shared<SubTrajectory>() };
solution->setCost(STAGE_COST);
connect(from, to, solution);
}
};
class ForwardMockup : public PropagatingForward
{
PlanningScenePtr ps;
public:
using PropagatingForward::PropagatingForward;
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
PropagatingForward::init(robot_model);
}
void computeForward(const InterfaceState& from) override {
SubTrajectory solution;
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(ps->getRobotModel(),
ps->getRobotModel()->getJointModelGroups()[0]) };
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
solution.setTrajectory(traj);
solution.setCost(STAGE_COST);
InterfaceState to(from);
sendForward(from, std::move(to), std::move(solution));
};
};
class BackwardMockup : public PropagatingBackward
{
using PropagatingBackward::PropagatingBackward;
void computeBackward(const InterfaceState& to) override {
SubTrajectory solution;
solution.setCost(STAGE_COST);
InterfaceState from(to);
sendBackward(std::move(from), to, std::move(solution));
};
};
template <typename T>
class Standalone : public T
{
moveit::core::RobotModelConstPtr robot;
InterfacePtr dummy;
planning_scene::PlanningSceneConstPtr ps;
InterfaceStatePtr state_start, state_end;
public:
Standalone(const moveit::core::RobotModelConstPtr& robot)
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {}
// reset and prepare for a compute step
void prepare() {
auto impl{ this->pimpl() };
this->reset();
state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
// infer and setup interface from children
Stage* s{ this };
ContainerBase* b;
while ((b = dynamic_cast<ContainerBase*>(s)))
s = &(*b->pimpl()->children().front());
InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK };
s = this;
while ((b = dynamic_cast<ContainerBase*>(s)))
s = &(*b->pimpl()->children().back());
InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK };
InterfaceFlags flags{ start_flags | end_flags };
impl->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr);
impl->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr);
T::init(robot);
impl->resolveInterface(flags);
// feed interfaces as required for one computation
if (flags & READS_START) {
impl->starts()->add(*state_start);
}
if (flags & READS_END) {
impl->ends()->add(*state_end);
}
}
void runCompute() { this->pimpl()->runCompute(); }
void computeWithStages(std::initializer_list<StageUniquePtr> stages) {
this->clear();
// initializer_list offers only const access (a known shortcoming in C++)
for (const auto& stage : stages) {
this->add(const_cast<StageUniquePtr&&>(stage));
}
prepare();
runCompute();
}
void computeWithStageCost(std::initializer_list<StageUniquePtr> stages, const CostTermConstPtr& cost_term) {
this->setCostTerm(nullptr);
for (const auto& stage : stages)
const_cast<StageUniquePtr&&>(stage)->setCostTerm(cost_term);
computeWithStages(stages);
}
void computeWithContainerCost(std::initializer_list<StageUniquePtr> stages, const CostTermPtr& cost_term) {
this->setCostTerm(cost_term);
computeWithStages(stages);
}
};
TEST(CostTerm, SetLambdaCostTerm) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
const moveit::core::RobotModelConstPtr robot{ getModel() };
Standalone<SerialContainer> container(robot);
auto stage{ std::make_unique<GeneratorMockup>() };
stage->setCostTerm([](auto&&) { return 1.0; });
container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use simple lambda signature";
stage = std::make_unique<GeneratorMockup>();
stage->setCostTerm([](auto&&, auto&&) { return 1.0; });
container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature";
stage = std::make_unique<GeneratorMockup>();
stage->setCostTerm([](auto&&, auto&& comment) {
comment = "I want the user to see this";
return 1.0;
});
container.computeWithStages({ std::move(stage) });
auto sol = std::dynamic_pointer_cast<const SolutionSequence>(container.solutions().front());
EXPECT_EQ(sol->cost(), 1.0);
EXPECT_EQ(sol->solutions()[0]->comment(), "I want the user to see this") << "can write to comment";
}
TEST(CostTerm, CostOverwrite) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
const moveit::core::RobotModelConstPtr robot{ getModel() };
Standalone<SerialContainer> container(robot);
container.computeWithStages({ std::make_unique<GeneratorMockup>() });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default";
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, nullptr);
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost";
auto constant_cost{ std::make_shared<cost::Constant>(1.0) };
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, constant_cost);
EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost";
}
TEST(CostTerm, StageTypes) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<SerialContainer> container(robot);
auto constant{ std::make_shared<cost::Constant>(1.0) };
// already tested above
// cont.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant);
// EXPECT_EQ(cont.solutions().front()->cost(), constant.cost);
container.computeWithStageCost({ std::make_unique<ConnectMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect";
container.computeWithStageCost({ std::make_unique<ForwardMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator";
container.computeWithStageCost({ std::make_unique<BackwardMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator";
}
TEST(CostTerm, PassThroughUsesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto constant{ std::make_shared<cost::Constant>(84.0) };
stage->setCostTerm(constant);
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant->cost) << "PassThrough forwards children's cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "Child cost equals PassThrough cost";
}
TEST(CostTerm, PassThroughOverwritesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto constant_inner{ std::make_shared<cost::Constant>(84.0) };
stage->setCostTerm(constant_inner);
auto constant_outer{ std::make_shared<cost::Constant>(72.0) };
container.setCostTerm(constant_outer);
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_outer->cost) << "PassThrough can apply custom cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant_inner->cost) << "child's cost is not affected";
}
TEST(CostTerm, PassThroughCanModifyCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto constant{ std::make_shared<cost::Constant>(8.0) };
stage->setCostTerm(constant);
container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant->cost * constant->cost) << "PassThrough can compute cost based on child";
EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "child's cost is not affected";
}
TEST(CostTerm, CompositeSolutions) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
Standalone<SerialContainer> container{ getModel() };
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
container.computeWithStages({ std::move(s1), std::move(s2) });
EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added";
}
{
auto s1{ std::make_unique<ForwardMockup>() };
auto constant{ std::make_shared<cost::Constant>(1.0) };
s1->setCostTerm(constant);
auto s2{ std::make_unique<ForwardMockup>() };
container.computeWithStages({ std::move(s1), std::move(s2) });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost)
<< "mix of explicit and implicit cost terms works";
}
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
auto c1{ std::make_unique<SerialContainer>() };
auto constant1{ std::make_shared<cost::Constant>(1.0) };
s1->setCostTerm(constant1);
s2->setCostTerm(constant1);
c1->add(std::move(s1));
c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() };
auto constant2{ std::make_shared<cost::Constant>(9.0) };
s3->setCostTerm(constant2);
container.computeWithStages({ std::move(c1), std::move(s3) });
EXPECT_EQ(container.solutions().front()->cost(), 2 * constant1->cost + constant2->cost)
<< "cost aggregation works across multiple levels";
}
}
TEST(CostTerm, CompositeSolutionsContainerCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
Standalone<SerialContainer> container{ getModel() };
auto s1{ std::make_unique<ForwardMockup>() };
auto s1_ptr{ s1.get() };
auto s2{ std::make_unique<ForwardMockup>() };
auto c1{ std::make_unique<SerialContainer>() };
c1->add(std::move(s1));
c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() };
container.setCostTerm(std::make_unique<cost::TrajectoryDuration>());
container.computeWithStages({ std::move(c1), std::move(s3) });
EXPECT_EQ(container.solutions().front()->cost(), 3 * TRAJECTORY_DURATION)
<< "container cost term overwrites stage costs";
EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected";
}

View File

@ -28,6 +28,7 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset((new PlanningScene(robot_model)));
Generator::init(robot_model);
}
bool canCompute() const override { return true; }

View File

@ -1,7 +1,4 @@
# id of generating process
string process_id
# id of associated task
# id of generating task
string task_id
# planning scene of start state

View File

@ -1,8 +1,5 @@
# id of generating process
string process_id
# unique ID of this task
string id
# unique id of this task
string task_id
# list of all stages, including the task stage itself
StageDescription[] stages

View File

@ -1,8 +1,5 @@
# id of generating process
string process_id
# unique of this task
string id
# unique id of generating task
string task_id
# list of all stages, including the task stage itself
StageStatistics[] stages

View File

@ -49,6 +49,17 @@ namespace mtc = ::moveit::task_constructor;
namespace {
class ScopedYamlEvent
{
public:
~ScopedYamlEvent() { yaml_event_delete(&event_); }
operator yaml_event_t const &() const { return event_; }
operator yaml_event_t&() { return event_; }
private:
yaml_event_t event_;
};
// Event-based YAML parser, creating an rviz::Property tree
// https://www.wpsoftware.net/andrew/pages/libyaml.html
class Parser
@ -60,10 +71,11 @@ public:
~Parser();
rviz::Property* process(const QString& name, const QString& description, rviz::Property* old) const;
private:
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
bool noError() const { return parser_.error == YAML_NO_ERROR; }
// parse a single event and return it's type, YAML_ERROR_EVENT on parsing error
@ -98,7 +110,6 @@ Parser::~Parser() {
int Parser::parse(yaml_event_t& event) const {
if (!yaml_parser_parse(&parser_, &event)) {
yaml_event_delete(&event);
return YAML_ERROR_EVENT;
}
return event.type;
@ -108,7 +119,7 @@ int Parser::parse(yaml_event_t& event) const {
rviz::Property* Parser::process(const QString& name, const QString& description, rviz::Property* old) const {
bool stop = false;
while (!stop) {
yaml_event_t event;
ScopedYamlEvent event;
switch (parse(event)) {
case YAML_ERROR_EVENT:
return Parser::createScalar(name, description, "YAML error", old);
@ -205,7 +216,7 @@ rviz::Property* Parser::processMapping(const QString& name, const QString& descr
int index = 0; // current child index in root
bool stop = false;
while (!stop && noError()) { // parse all map items
yaml_event_t event;
ScopedYamlEvent event;
switch (parse(event)) { // parse key
case YAML_MAPPING_END_EVENT: // all fine, reached end of mapping
stop = true;
@ -263,7 +274,7 @@ rviz::Property* Parser::processSequence(const QString& name, const QString& desc
int index = 0; // current child index in root
bool stop = false;
while (!stop && noError()) { // parse all map items
yaml_event_t event;
ScopedYamlEvent event;
switch (parse(event)) {
case YAML_SEQUENCE_END_EVENT: // all fine, reached end of sequence
stop = true;

View File

@ -45,7 +45,6 @@
#include <rviz/properties/property_tree_model.h>
#include <rviz/properties/string_property.h>
#include <ros/console.h>
#include <ros/service_client.h>
#include <QApplication>
#include <QPalette>
@ -181,20 +180,19 @@ QModelIndex RemoteTaskModel::index(const Node* n) const {
return QModelIndex();
}
RemoteTaskModel::RemoteTaskModel(const planning_scene::PlanningSceneConstPtr& scene,
RemoteTaskModel::RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name,
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
// service to request solutions
get_solution_client_ = nh.serviceClient<moveit_task_constructor_msgs::GetSolution>(service_name);
}
RemoteTaskModel::~RemoteTaskModel() {
delete root_;
}
void RemoteTaskModel::setSolutionClient(ros::ServiceClient* client) {
get_solution_client_ = client;
}
int RemoteTaskModel::rowCount(const QModelIndex& parent) const {
if (parent.column() > 0)
return 0;
@ -353,7 +351,7 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs:
// emit notify about model changes when node was already visited
if (n->node_flags_ & WAS_VISITED) {
QModelIndex idx = index(n);
dataChanged(idx.sibling(idx.row(), 1), idx.sibling(idx.row(), 2));
dataChanged(idx.sibling(idx.row(), 1), idx.sibling(idx.row(), 3));
}
}
}
@ -420,15 +418,16 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) {
// to avoid some communication overhead
DisplaySolutionPtr result;
if (!(flags_ & IS_DESTROYED) && get_solution_client_) {
if (!(flags_ & IS_DESTROYED)) {
// request solution via service
moveit_task_constructor_msgs::GetSolution srv;
srv.request.solution_id = id;
try {
if (get_solution_client_->call(srv)) {
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
get_solution_client_.shutdown();
flags_ |= IS_DESTROYED;
}
} catch (const std::exception& e) {

View File

@ -38,13 +38,10 @@
#include "task_list_model.h"
#include <moveit/visualization_tools/display_solution.h>
#include <ros/service_client.h>
#include <memory>
#include <limits>
namespace ros {
class ServiceClient;
}
namespace moveit_rviz_plugin {
class RemoteSolutionModel;
@ -57,7 +54,7 @@ class RemoteTaskModel : public BaseTaskModel
Q_OBJECT
struct Node;
Node* const root_;
ros::ServiceClient* get_solution_client_ = nullptr;
ros::ServiceClient get_solution_client_;
std::map<uint32_t, Node*> id_to_stage_;
std::map<uint32_t, DisplaySolutionPtr> id_to_solution_;
@ -70,12 +67,11 @@ class RemoteTaskModel : public BaseTaskModel
void setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info);
public:
RemoteTaskModel(const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context,
RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name,
const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context,
QObject* parent = nullptr);
~RemoteTaskModel() override;
void setSolutionClient(ros::ServiceClient* client);
int rowCount(const QModelIndex& parent = QModelIndex()) const override;
QModelIndex index(int row, int column, const QModelIndex& parent = QModelIndex()) const override;

View File

@ -59,9 +59,8 @@
namespace moveit_rviz_plugin {
TaskDisplay::TaskDisplay() : Display(), received_task_description_(false) {
TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_description_(false) {
task_list_model_.reset(new TaskListModel);
task_list_model_->setSolutionClient(&get_solution_client);
MetaTaskListModel::instance().insertModel(task_list_model_.get(), this);
@ -89,17 +88,24 @@ TaskDisplay::TaskDisplay() : Display(), received_task_description_(false) {
}
TaskDisplay::~TaskDisplay() {
if (context_)
TaskPanel::decDisplayCount();
if (panel_requested_)
TaskPanel::release(); // Indicate that we don't need a TaskPanel anymore
}
void TaskDisplay::onInitialize() {
Display::onInitialize();
trajectory_visual_->onInitialize(scene_node_, context_);
task_list_model_->setDisplayContext(context_);
// create a new TaskPanel by default
// by post-poning this to main loop, we can ensure that rviz has loaded everything before
mainloop_jobs_.addJob([this]() { TaskPanel::incDisplayCount(context_->getWindowManager()); });
}
inline void TaskDisplay::requestPanel() {
if (panel_requested_)
return; // already done
// Create a new TaskPanel if not yet done.
// This cannot be done in initialize(), because Panel loading follows Display loading in rviz.
panel_requested_ = true;
TaskPanel::request(context_->getWindowManager());
}
void TaskDisplay::loadRobotModel() {
@ -171,8 +177,8 @@ void TaskDisplay::calculateOffsetPosition() {
}
void TaskDisplay::update(float wall_dt, float ros_dt) {
requestPanel();
Display::update(wall_dt, ros_dt);
mainloop_jobs_.executeJobs();
trajectory_visual_->update(wall_dt, ros_dt);
}
@ -188,49 +194,32 @@ void TaskDisplay::changedRobotDescription() {
loadRobotModel();
}
inline std::string getUniqueId(const std::string& process_id, const std::string& task_id) {
std::string id{ process_id };
if (!task_id.empty()) {
id += "/";
id += task_id;
}
return id;
}
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");
task_list_model_->processTaskDescriptionMessage(id, *msg);
requestPanel();
task_list_model_->processTaskDescriptionMessage(*msg, update_nh_,
base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id);
// Start listening to other topics if this is the first description
// Waiting for the description ensures we do not receive data that cannot be interpreted yet
if (!received_task_description_ && !msg->stages.empty()) {
received_task_description_ = true;
task_statistics_sub =
update_nh_.subscribe(base_ns_ + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
task_statistics_sub = update_nh_.subscribe(base_ns_ + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this);
task_solution_sub = update_nh_.subscribe(base_ns_ + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this);
}
});
}
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");
task_list_model_->processTaskStatisticsMessage(id, *msg);
});
task_list_model_->processTaskStatisticsMessage(*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);
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg);
if (s)
trajectory_visual_->showTrajectory(s, false);
return;
});
}
void TaskDisplay::changedTaskSolutionTopic() {
@ -241,7 +230,6 @@ void TaskDisplay::changedTaskSolutionTopic() {
task_description_sub.shutdown();
task_statistics_sub.shutdown();
task_solution_sub.shutdown();
get_solution_client.shutdown();
received_task_description_ = false;
@ -256,11 +244,7 @@ void TaskDisplay::changedTaskSolutionTopic() {
base_ns_ = solution_topic.toStdString().substr(0, solution_topic.length() - strlen(SOLUTION_TOPIC));
// listen to task descriptions updates
task_description_sub = update_nh_.subscribe(base_ns_ + DESCRIPTION_TOPIC, 2, &TaskDisplay::taskDescriptionCB, this);
// service to request solutions
get_solution_client =
update_nh_.serviceClient<moveit_task_constructor_msgs::GetSolution>(base_ns_ + GET_SOLUTION_SERVICE);
task_description_sub = update_nh_.subscribe(base_ns_ + DESCRIPTION_TOPIC, 10, &TaskDisplay::taskDescriptionCB, this);
setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received");
}

View File

@ -45,7 +45,6 @@
#include "job_queue.h"
#include <moveit/macros/class_forward.h>
#include <ros/subscriber.h>
#include <ros/service_client.h>
#include <moveit_task_constructor_msgs/TaskDescription.h>
#include <moveit_task_constructor_msgs/TaskStatistics.h>
#include <moveit_task_constructor_msgs/Solution.h>
@ -101,6 +100,9 @@ protected:
void fixedFrameChanged() override;
void calculateOffsetPosition();
private:
inline void requestPanel();
private Q_SLOTS:
/**
* \brief Slot Event Functions
@ -119,15 +121,12 @@ protected:
ros::Subscriber task_solution_sub;
ros::Subscriber task_description_sub;
ros::Subscriber task_statistics_sub;
ros::ServiceClient get_solution_client;
// handle processing of task+solution messages in Qt mainloop
moveit::tools::JobQueue mainloop_jobs_;
// The trajectory playback component
std::unique_ptr<TaskSolutionVisualization> trajectory_visual_;
// The TaskListModel storing actual task and solution data
std::unique_ptr<TaskListModel> task_list_model_;
bool panel_requested_;
// Load robot model
rdf_loader::RDFLoaderPtr rdf_loader_;

View File

@ -38,11 +38,11 @@
#include "task_list_model.h"
#include "local_task_model.h"
#include "remote_task_model.h"
#include "task_panel.h"
#include "factory_model.h"
#include "icons.h"
#include <ros/console.h>
#include <ros/service_client.h>
#include <QMimeData>
#include <QHeaderView>
@ -162,7 +162,8 @@ void TaskListModel::onRemoveModel(QAbstractItemModel* model) {
it->second = nullptr;
}
TaskListModel::TaskListModel(QObject* parent) : FlatMergeProxyModel(parent) {
TaskListModel::TaskListModel(QObject* parent)
: FlatMergeProxyModel(parent), old_task_handling_(TaskView::OLD_TASK_REPLACE) {
ROS_DEBUG_NAMED(LOGNAME, "created TaskListModel: %p", this);
setStageFactory(getStageFactory());
}
@ -184,10 +185,6 @@ void TaskListModel::setDisplayContext(rviz::DisplayContext* display_context) {
display_context_ = display_context;
}
void TaskListModel::setSolutionClient(ros::ServiceClient* client) {
get_solution_client_ = client;
}
void TaskListModel::setStageFactory(const StageFactoryPtr& factory) {
stage_factory_ = factory;
if (stage_factory_)
@ -206,6 +203,10 @@ Qt::ItemFlags TaskListModel::flags(const QModelIndex& index) const {
return f;
}
void TaskListModel::setOldTaskHandling(int mode) {
old_task_handling_ = mode;
}
void TaskListModel::highlightStage(size_t id) {
if (!active_task_model_)
return;
@ -240,52 +241,49 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const {
// process a task description message:
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one
void TaskListModel::processTaskDescriptionMessage(const std::string& id,
const moveit_task_constructor_msgs::TaskDescription& msg) {
// retrieve existing or insert new remote task for given id
auto it_inserted = remote_tasks_.insert(std::make_pair(id, nullptr));
bool created = it_inserted.second;
void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg,
ros::NodeHandle& nh, const std::string& service_name) {
// retrieve existing or insert new remote task for given task id
auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr));
const auto& task_it = it_inserted.first;
RemoteTaskModel*& remote_task = task_it->second;
if (!msg.stages.empty() && remote_task && (remote_task->taskFlags() & BaseTaskModel::IS_DESTROYED)) {
// task overriding previous one that was already marked destroyed, but not yet removed from model
if (old_task_handling_ != TaskView::OLD_TASK_KEEP)
removeModel(remote_task);
created = true; // re-create remote task after it was destroyed beforehand
remote_task = nullptr; // resetting to nullptr will trigger creation of a new task
}
// empty list indicates, that this remote task is not available anymore
// empty list indicates, that this remote task was destroyed and we won't get updates for it
if (msg.stages.empty()) {
if (!remote_task) { // task was already deleted locally
// we can now remove it from remote_tasks_
// always remove destroyed RemoteTask?
if (old_task_handling_ == TaskView::OLD_TASK_REMOVE && remote_task) {
removeModel(remote_task);
remote_tasks_.erase(task_it);
return;
}
} else if (created) { // create new task model, if ID was not known before
if (remote_task) // keep the task, but mark it as destroyed
remote_task->processStageDescriptions(msg.stages);
} else if (!remote_task) { // create new task model, if ID was not known before
// the model is managed by this instance via Qt's parent-child mechanism
remote_task = new RemoteTaskModel(scene_, display_context_, this);
remote_task->setSolutionClient(get_solution_client_);
remote_task = new RemoteTaskModel(nh, service_name, scene_, display_context_, this);
remote_task->processStageDescriptions(msg.stages);
ROS_DEBUG_NAMED(LOGNAME, "received new task: %s (%s)", msg.stages[0].name.c_str(), msg.task_id.c_str());
// insert newly created model into this' model instance
insertModel(remote_task, -1);
// HACK: always use the last created model as active
active_task_model_ = remote_task;
}
if (!remote_task)
return; // task is not in use anymore
} else // normal update
remote_task->processStageDescriptions(msg.stages);
// insert newly created model into this' model instance
if (created) {
ROS_DEBUG_NAMED(LOGNAME, "received new task: %s", msg.id.c_str());
insertModel(remote_task, -1);
}
}
// process a task statistics message
void TaskListModel::processTaskStatisticsMessage(const std::string& id,
const moveit_task_constructor_msgs::TaskStatistics& msg) {
auto it = remote_tasks_.find(id);
void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg) {
auto it = remote_tasks_.find(msg.task_id);
if (it == remote_tasks_.cend()) {
ROS_WARN("unknown task: %s", id.c_str());
ROS_WARN("unknown task: %s", msg.task_id.c_str());
return;
}
@ -296,9 +294,8 @@ void TaskListModel::processTaskStatisticsMessage(const std::string& id,
remote_task->processStageStatistics(msg.stages);
}
DisplaySolutionPtr TaskListModel::processSolutionMessage(const std::string& id,
const moveit_task_constructor_msgs::Solution& msg) {
auto it = remote_tasks_.find(id);
DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) {
auto it = remote_tasks_.find(msg.task_id);
if (it == remote_tasks_.cend())
return DisplaySolutionPtr(); // unkown task

View File

@ -42,6 +42,7 @@
#include <utils/flat_merge_proxy_model.h>
#include <moveit/macros/class_forward.h>
#include <ros/node_handle.h>
#include <moveit_task_constructor_msgs/TaskDescription.h>
#include <moveit_task_constructor_msgs/TaskStatistics.h>
#include <moveit_task_constructor_msgs/Solution.h>
@ -52,9 +53,6 @@
#include <memory>
#include <QPointer>
namespace ros {
class ServiceClient;
}
namespace rviz {
class PropertyTreeModel;
class DisplayContext;
@ -128,11 +126,12 @@ class TaskListModel : public utils::FlatMergeProxyModel
// rviz::DisplayContext used to show (interactive) markers by the property models
rviz::DisplayContext* display_context_ = nullptr;
// map from remote task IDs to tasks
// map from remote task IDs to (active) tasks
// if task is destroyed remotely, it is marked with flag IS_DESTROYED
// if task is removed locally from tasks vector, it is marked with a nullptr
std::map<std::string, RemoteTaskModel*> remote_tasks_;
ros::ServiceClient* get_solution_client_ = nullptr;
// mode reflecting the "Old task handling" setting
int old_task_handling_;
// factory used to create stages
StageFactoryPtr stage_factory_;
@ -148,7 +147,6 @@ public:
void setScene(const planning_scene::PlanningSceneConstPtr& scene);
void setDisplayContext(rviz::DisplayContext* display_context);
void setSolutionClient(ros::ServiceClient* client);
void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; }
int columnCount(const QModelIndex& parent = QModelIndex()) const override { return 4; }
@ -157,11 +155,12 @@ public:
QVariant data(const QModelIndex& index, int role) const override;
/// process an incoming task description message - only call in Qt's main loop
void processTaskDescriptionMessage(const std::string& id, const moveit_task_constructor_msgs::TaskDescription& msg);
void processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg, ros::NodeHandle& nh,
const std::string& service_name);
/// process an incoming task description message - only call in Qt's main loop
void processTaskStatisticsMessage(const std::string& id, const moveit_task_constructor_msgs::TaskStatistics& msg);
void processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg);
/// process an incoming solution message - only call in Qt's main loop
DisplaySolutionPtr processSolutionMessage(const std::string& id, const moveit_task_constructor_msgs::Solution& msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg);
/// insert a TaskModel, pos is relative to modelCount()
bool insertModel(BaseTaskModel* model, int pos = -1);
@ -175,6 +174,9 @@ public:
Qt::DropActions supportedDropActions() const override;
Qt::ItemFlags flags(const QModelIndex& index) const override;
public Q_SLOTS:
void setOldTaskHandling(int mode);
protected Q_SLOTS:
void highlightStage(size_t id);
};

View File

@ -130,12 +130,30 @@ void TaskPanel::addSubPanel(SubPanel* w, const QString& title, const QIcon& icon
connect(w, SIGNAL(configChanged()), this, SIGNAL(configChanged()));
}
void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager) {
/* Realizing a singleton Panel is a nightmare with rviz...
* Formally, Panels (as a plugin class) cannot be singleton, because new instances are created on demand.
* Hence, we decided to use a true singleton for the underlying model only and a fake singleton for the panel.
* Thus, all panels (in case multiple were created) show the same content.
* The fake singleton shall ensure that only a single panel is created, even if several displays are created.
* To this end, the displays request() the need for a panel during their initialization and they release()
* this need during their destruction. This, in principle, allows to create a panel together with the first
* display and destroy it when the last display is gone.
* Obviously, the user can still decide to explicitly delete the panel (or create new ones).
* The nightmare arises from the order of loading of displays and panels: Displays are loaded first.
* However, directly creating a panel with the first loaded display doesn't work, because panel loading
* will create another panel instance later (because there is no singleton support).
* Hence, we need to postpone the actual panel creation from displays until panel loading is finished as well.
* This was initially done, by postponing panel creation to TaskDisplay::update(). However, update()
* will never be called if the display is disabled...
*/
void TaskPanel::request(rviz::WindowManagerInterface* window_manager) {
++DISPLAY_COUNT;
rviz::VisualizationFrame* vis_frame = dynamic_cast<rviz::VisualizationFrame*>(window_manager);
if (SINGLETON || !vis_frame)
return; // already define, nothing to do
return; // already defined, nothing to do
QDockWidget* dock =
vis_frame->addPanelByName("Motion Planning Tasks", "moveit_task_constructor/Motion Planning Tasks",
@ -143,15 +161,15 @@ void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager) {
assert(dock->widget() == SINGLETON);
}
void TaskPanel::decDisplayCount() {
void TaskPanel::release() {
Q_ASSERT(DISPLAY_COUNT > 0);
if (--DISPLAY_COUNT == 0 && SINGLETON)
SINGLETON->deleteLater();
}
TaskPanelPrivate::TaskPanelPrivate(TaskPanel* q_ptr) : q_ptr(q_ptr) {
setupUi(q_ptr);
tool_buttons_group = new QButtonGroup(q_ptr);
TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) {
setupUi(panel);
tool_buttons_group = new QButtonGroup(panel);
tool_buttons_group->setExclusive(true);
button_show_stage_dock_widget->setEnabled(bool(getStageFactory()));
button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages"));
@ -198,35 +216,16 @@ void setExpanded(QTreeView* view, const QModelIndex& index, bool expand, int dep
view->setExpanded(index, expand);
}
TaskViewPrivate::TaskViewPrivate(TaskView* q_ptr) : q_ptr(q_ptr), exec_action_client_("execute_task_solution") {
setupUi(q_ptr);
TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view), exec_action_client_("execute_task_solution") {
setupUi(view);
MetaTaskListModel* meta_model = &MetaTaskListModel::instance();
StageFactoryPtr factory = getStageFactory();
if (factory)
meta_model->setMimeTypes({ factory->mimeType() });
tasks_view->setModel(meta_model);
// auto-expand newly-inserted top-level items
QObject::connect(meta_model, &QAbstractItemModel::rowsInserted,
[this](const QModelIndex& parent, int first, int last) {
if (parent.isValid() && !parent.parent().isValid()) { // top-level task items inserted
int expand = this->q_ptr->initial_task_expand->getOptionInt();
for (int row = first; row <= last; ++row) {
QModelIndex child = parent.model()->index(row, 0, parent);
if (expand != TaskView::EXPAND_NONE) {
// recursively expand all inserted items
setExpanded(tasks_view, child, true);
}
if (expand == TaskView::EXPAND_TOP) {
// collapse up to first level
setExpanded(tasks_view, child, false, 1);
// expand inserted item
setExpanded(tasks_view, child, true, 0);
}
}
tasks_view->setExpanded(parent, true); // expand parent group item
}
});
QObject::connect(meta_model, SIGNAL(rowsInserted(QModelIndex, int, int)), q_ptr,
SLOT(_q_configureInsertedModels(QModelIndex, int, int)));
tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection);
tasks_view->setAcceptDrops(true);
@ -251,6 +250,40 @@ std::pair<BaseTaskModel*, QModelIndex> TaskViewPrivate::getTaskModel(const QMode
return meta_model->getTaskModel(index);
}
void TaskViewPrivate::configureTaskListModel(TaskListModel* model) {
QObject::connect(q_ptr, &TaskView::oldTaskHandlingChanged, model, &TaskListModel::setOldTaskHandling);
model->setOldTaskHandling(q_ptr->old_task_handling->getOptionInt());
}
void TaskViewPrivate::configureExistingModels() {
auto* meta_model = static_cast<MetaTaskListModel*>(tasks_view->model());
for (int row = meta_model->rowCount() - 1; row >= 0; --row)
configureTaskListModel(meta_model->getTaskListModel(meta_model->index(row, 0)).first);
}
void TaskViewPrivate::_q_configureInsertedModels(const QModelIndex& parent, int first, int last) {
if (parent.isValid() && !parent.parent().isValid()) { // top-level task items inserted
int expand = q_ptr->initial_task_expand->getOptionInt();
for (int row = first; row <= last; ++row) {
// set expanded state of items
QModelIndex child = parent.model()->index(row, 0, parent);
if (expand != TaskView::EXPAND_NONE) {
// recursively expand all inserted items
setExpanded(tasks_view, child, true);
}
if (expand == TaskView::EXPAND_TOP) {
// collapse up to first level
setExpanded(tasks_view, child, false, 1);
// expand inserted item
setExpanded(tasks_view, child, true, 0);
}
configureTaskListModel(getTaskListModel(child).first);
}
tasks_view->setExpanded(parent, true); // expand parent group item
}
}
void TaskViewPrivate::lock(TaskDisplay* display) {
if (locked_display_ && locked_display_ != display) {
locked_display_->clearMarkers();
@ -292,8 +325,18 @@ TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root)
initial_task_expand->addOption("All Expanded", EXPAND_ALL);
initial_task_expand->addOption("All Closed", EXPAND_NONE);
old_task_handling =
new rviz::EnumProperty("Old task handling", "Keep",
"Configure what to do with old tasks whose solutions cannot be queried anymore", configs);
old_task_handling->addOption("Keep", OLD_TASK_KEEP);
old_task_handling->addOption("Replace", OLD_TASK_REPLACE);
old_task_handling->addOption("Remove", OLD_TASK_REMOVE);
connect(old_task_handling, &rviz::Property::changed, this, &TaskView::onOldTaskHandlingChanged);
show_time_column = new rviz::BoolProperty("Show Computation Times", true, "Show the 'time' column", configs);
connect(show_time_column, SIGNAL(changed()), this, SLOT(onShowTimeChanged()));
connect(show_time_column, &rviz::Property::changed, this, &TaskView::onShowTimeChanged);
d_ptr->configureExistingModels();
}
TaskView::~TaskView() {
@ -407,12 +450,12 @@ void TaskView::onCurrentStageChanged(const QModelIndex& current, const QModelInd
QItemSelectionModel* sm = view->selectionModel();
QAbstractItemModel* m = task ? task->getSolutionModel(task_index) : nullptr;
if (view->model() != m) {
view->setModel(m);
view->sortByColumn(sort_column, sort_order);
delete sm; // we don't store the selection model
sm = view->selectionModel();
if (sm) {
sm = view->selectionModel();
connect(sm, SIGNAL(currentChanged(QModelIndex, QModelIndex)), this,
SLOT(onCurrentSolutionChanged(QModelIndex, QModelIndex)));
connect(sm, SIGNAL(selectionChanged(QItemSelection, QItemSelection)), this,
@ -420,11 +463,13 @@ void TaskView::onCurrentStageChanged(const QModelIndex& current, const QModelInd
}
// update the PropertyModel
QTreeView* pview = d_ptr->property_view;
sm = pview->selectionModel();
view = d_ptr->property_view;
sm = view->selectionModel();
m = task ? task->getPropertyModel(task_index) : nullptr;
pview->setModel(m);
if (view->model() != m) {
view->setModel(m);
delete sm; // we don't store the selection model
}
}
void TaskView::onCurrentSolutionChanged(const QModelIndex& current, const QModelIndex& previous) {
@ -489,10 +534,14 @@ void TaskView::onShowTimeChanged() {
d_ptr->actionShowTimeColumn->setChecked(show);
}
GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz::Property* root)
: q_ptr(q_ptr) {
setupUi(q_ptr);
properties = new rviz::PropertyTreeModel(root, q_ptr);
void TaskView::onOldTaskHandlingChanged() {
Q_EMIT oldTaskHandlingChanged(old_task_handling->getOptionInt());
}
GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* widget, rviz::Property* root)
: q_ptr(widget) {
setupUi(widget);
properties = new rviz::PropertyTreeModel(root, widget);
view->setModel(properties);
}

View File

@ -91,8 +91,8 @@ public:
* If not yet done, an instance is created. If use count drops to zero,
* the global instance is destroyed.
*/
static void incDisplayCount(rviz::WindowManagerInterface* window_manager);
static void decDisplayCount();
static void request(rviz::WindowManagerInterface* window_manager);
static void release();
void onInitialize() override;
void load(const rviz::Config& config) override;
@ -124,11 +124,19 @@ protected:
EXPAND_ALL,
EXPAND_NONE
};
rviz::EnumProperty* initial_task_expand;
rviz::EnumProperty* initial_task_expand;
rviz::EnumProperty* old_task_handling;
rviz::BoolProperty* show_time_column;
public:
enum OldTaskHandling
{
OLD_TASK_KEEP = 1,
OLD_TASK_REPLACE,
OLD_TASK_REMOVE
};
TaskView(TaskPanel* parent, rviz::Property* root);
~TaskView() override;
@ -145,6 +153,13 @@ protected Q_SLOTS:
void onSolutionSelectionChanged(const QItemSelection& selected, const QItemSelection& deselected);
void onExecCurrentSolution() const;
void onShowTimeChanged();
void onOldTaskHandlingChanged();
private:
Q_PRIVATE_SLOT(d_ptr, void _q_configureInsertedModels(QModelIndex, int, int));
Q_SIGNALS:
void oldTaskHandlingChanged(int);
};
class GlobalSettingsWidgetPrivate;

View File

@ -78,6 +78,13 @@ public:
/// retrieve TaskModel corresponding to given index
inline std::pair<BaseTaskModel*, QModelIndex> getTaskModel(const QModelIndex& index) const;
/// configure a (new) TaskListModel
void configureTaskListModel(TaskListModel* model);
/// configure all TaskListModels that were already created when TaskView gets instantiated
void configureExistingModels();
/// configure newly inserted models
void _q_configureInsertedModels(const QModelIndex& parent, int first, int last);
/// unlock locked_display_ if given display is different
void lock(TaskDisplay* display);

View File

@ -16,5 +16,5 @@ if (CATKIN_ENABLE_TESTING)
add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp)
target_link_libraries(${PROJECT_NAME}-test-task_model
motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest_main)
motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest)
endif()

View File

@ -50,15 +50,17 @@ using namespace moveit::task_constructor;
class TaskListModelTest : public ::testing::Test
{
protected:
ros::NodeHandle nh;
moveit_rviz_plugin::TaskListModel model;
int children = 0;
int num_inserts = 0;
int num_updates = 0;
moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name) {
moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name,
const std::string& task_id = std::string()) {
moveit_task_constructor_msgs::TaskDescription t;
t.id = name;
uint id = 0, root_id;
t.task_id = task_id.empty() ? name : task_id;
moveit_task_constructor_msgs::StageDescription desc;
desc.parent_id = id;
@ -127,7 +129,7 @@ protected:
SCOPED_TRACE("first i=" + std::to_string(i));
num_inserts = 0;
num_updates = 0;
model.processTaskDescriptionMessage("1", genMsg("first"));
model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution");
if (i == 0)
EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task
@ -141,7 +143,7 @@ protected:
SCOPED_TRACE("second i=" + std::to_string(i));
num_inserts = 0;
num_updates = 0;
model.processTaskDescriptionMessage("2", genMsg("second")); // 1 notify for inserted task
model.processTaskDescriptionMessage(genMsg("second"), nh, "get_solution"); // 1 notify for inserted task
if (i == 0)
EXPECT_EQ(num_inserts, 1);
@ -163,7 +165,7 @@ protected:
TEST_F(TaskListModelTest, remoteTaskModel) {
children = 3;
planning_scene::PlanningSceneConstPtr scene;
moveit_rviz_plugin::RemoteTaskModel m(scene, nullptr);
moveit_rviz_plugin::RemoteTaskModel m(nh, "get_solution", scene, nullptr);
m.processStageDescriptions(genMsg("first").stages);
SCOPED_TRACE("first");
validate(m, { "first" });
@ -200,13 +202,13 @@ TEST_F(TaskListModelTest, threeChildren) {
TEST_F(TaskListModelTest, visitedPopulate) {
// first population without children
children = 0;
model.processTaskDescriptionMessage("1", genMsg("first"));
model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution");
validate(model, { "first" }); // validation visits root node
EXPECT_EQ(num_inserts, 1);
children = 3;
num_inserts = 0;
model.processTaskDescriptionMessage("1", genMsg("first"));
model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution");
validate(model, { "first" });
// second population with children should emit insert notifies for them
EXPECT_EQ(num_inserts, 3);
@ -215,7 +217,7 @@ TEST_F(TaskListModelTest, visitedPopulate) {
TEST_F(TaskListModelTest, deletion) {
children = 3;
model.processTaskDescriptionMessage("1", genMsg("first"));
model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution");
auto m = model.getModel(model.index(0, 0)).first;
int num_deletes = 0;
QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; });
@ -227,3 +229,9 @@ TEST_F(TaskListModelTest, deletion) {
// EXPECT_EQ(num_deletes, 1); // TODO: event is not processed, missing event loop?
EXPECT_EQ(model.rowCount(), 0);
}
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_task_model");
return RUN_ALL_TESTS();
}

View File

@ -1,7 +1,4 @@
<launch>
<include file="$(find moveit_resources_fanuc_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<test pkg="moveit_task_constructor_visualization"
type="moveit_task_constructor_visualization-test-task_model" test-name="test_task_model" />
</launch>

View File

@ -107,7 +107,7 @@ public:
std::vector<ModelData> data_;
public:
FlatMergeProxyModelPrivate(FlatMergeProxyModel* q_ptr) : q_ptr(q_ptr) {}
FlatMergeProxyModelPrivate(FlatMergeProxyModel* model) : q_ptr(model) {}
std::vector<ModelData>::iterator find(const QObject* model) {
Q_ASSERT(model);

View File

@ -108,7 +108,7 @@ public:
std::vector<ModelData> data_;
public:
TreeMergeProxyModelPrivate(TreeMergeProxyModel* q_ptr) : q_ptr(q_ptr) {}
TreeMergeProxyModelPrivate(TreeMergeProxyModel* model) : q_ptr(model) {}
std::vector<ModelData>::iterator find(const QObject* model) {
Q_ASSERT(model);

View File

@ -2,6 +2,11 @@ set(MOVEIT_LIB_NAME moveit_task_visualization_tools)
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_tools)
# TODO: Remove when Kinetic support is dropped
if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2?
add_definitions(-DRVIZ_TF1)
endif()
set(HEADERS
${PROJECT_INCLUDE}/display_solution.h
${PROJECT_INCLUDE}/marker_visualization.h

View File

@ -7,6 +7,9 @@
#include <rviz/frame_manager.h>
#include <OgreSceneManager.h>
#include <OgreSceneNode.h>
#ifndef RVIZ_TF1
#include <tf/tf.h>
#endif
#include <tf2_msgs/TF2Error.h>
#include <ros/console.h>
#include <eigen_conversions/eigen_msg.h>
@ -69,18 +72,23 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce
Ogre::Vector3 pos;
try {
#ifdef RVIZ_TF1
tf::TransformListener* tf = context->getFrameManager()->getTFClient();
std::string error_msg;
if (!tf->canTransform(planning_frame_, fixed_frame, ros::Time(), &error_msg)) {
ROS_WARN_STREAM_NAMED("MarkerVisualization", error_msg);
return false; // frame transform not (yet) available
}
tf::StampedTransform tm;
tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm);
auto q = tm.getRotation();
auto p = tm.getOrigin();
quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z());
pos = Ogre::Vector3(p.x(), p.y(), p.z());
#else
std::shared_ptr<tf2_ros::Buffer> tf = context->getFrameManager()->getTF2BufferPtr();
geometry_msgs::TransformStamped tm;
tm = tf->lookupTransform(planning_frame_, fixed_frame, ros::Time());
auto q = tm.transform.rotation;
auto p = tm.transform.translation;
quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z);
pos = Ogre::Vector3(p.x, p.y, p.z);
#endif
} catch (const tf2::TransformException& e) {
ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what());
return false;