mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branch 'master' into wip-python-api
This commit is contained in:
commit
8433e460e4
12
codecov.yaml
12
codecov.yaml
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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,
|
||||
|
||||
174
core/include/moveit/task_constructor/cost_terms.h
Normal file
174
core/include/moveit/task_constructor/cost_terms.h
Normal 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
|
||||
@ -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:
|
||||
|
||||
@ -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_;
|
||||
|
||||
|
||||
61
core/include/moveit/task_constructor/stages/passthrough.h
Normal file
61
core/include/moveit/task_constructor/stages/passthrough.h
Normal 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
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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<>()))
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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,18 +865,20 @@ void MergerPrivate::onNewPropagateSolution(const SolutionBase& s) {
|
||||
void MergerPrivate::sendForward(SubTrajectory&& t, const InterfaceState* from) {
|
||||
// generate target state
|
||||
planning_scene::PlanningScenePtr to = from->scene()->diff();
|
||||
to->setCurrentState(t.trajectory()->getLastWayPoint());
|
||||
if (t.trajectory() && !t.trajectory()->empty())
|
||||
to->setCurrentState(t.trajectory()->getLastWayPoint());
|
||||
StagePrivate::sendForward(*from, InterfaceState(to), std::make_shared<SubTrajectory>(std::move(t)));
|
||||
}
|
||||
|
||||
void MergerPrivate::sendBackward(SubTrajectory&& t, const InterfaceState* to) {
|
||||
// generate target state
|
||||
planning_scene::PlanningScenePtr from = to->scene()->diff();
|
||||
from->setCurrentState(t.trajectory()->getFirstWayPoint());
|
||||
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,39 +924,46 @@ 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())
|
||||
sub_trajectories.push_back(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);
|
||||
// accumulate costs and markers
|
||||
double costs = 0.0;
|
||||
for (const auto& sub : sub_solutions) {
|
||||
costs += sub->cost();
|
||||
t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end());
|
||||
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) {
|
||||
costs += sub->cost();
|
||||
t.markers().insert(t.markers().end(), sub->markers().begin(), sub->markers().end());
|
||||
}
|
||||
t.setCost(costs);
|
||||
}
|
||||
t.setCost(costs);
|
||||
spawner(std::move(t));
|
||||
}
|
||||
} // namespace task_constructor
|
||||
|
||||
250
core/src/cost_terms.cpp
Normal file
250
core/src/cost_terms.cpp
Normal 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
|
||||
@ -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
|
||||
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -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");
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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");
|
||||
}
|
||||
|
||||
@ -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)));
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
65
core/src/stages/passthrough.cpp
Normal file
65
core/src/stages/passthrough.cpp
Normal 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
|
||||
@ -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
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
351
core/test/test_cost_terms.cpp
Normal file
351
core/test/test_cost_terms.cpp
Normal 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";
|
||||
}
|
||||
@ -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; }
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||
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_solution_sub = update_nh_.subscribe(base_ns_ + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this);
|
||||
}
|
||||
});
|
||||
// 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_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);
|
||||
});
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||
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);
|
||||
if (s)
|
||||
trajectory_visual_->showTrajectory(s, false);
|
||||
return;
|
||||
});
|
||||
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
|
||||
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");
|
||||
}
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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)) {
|
||||
removeModel(remote_task);
|
||||
created = true; // re-create remote task after it was destroyed beforehand
|
||||
// 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);
|
||||
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
|
||||
|
||||
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);
|
||||
}
|
||||
} else // normal update
|
||||
remote_task->processStageDescriptions(msg.stages);
|
||||
}
|
||||
|
||||
// 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
|
||||
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
@ -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;
|
||||
view->setModel(m);
|
||||
view->sortByColumn(sort_column, sort_order);
|
||||
delete sm; // we don't store the selection model
|
||||
sm = view->selectionModel();
|
||||
if (view->model() != m) {
|
||||
view->setModel(m);
|
||||
view->sortByColumn(sort_column, sort_order);
|
||||
delete sm; // we don't store the selection model
|
||||
|
||||
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);
|
||||
delete sm; // we don't store the selection model
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user