mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
simplify compute() API
- remove bool return value - always create a solution trajectory, also in case of failures - success/failure determined from solution.isFailure() minor adjustments during cherry-pick
This commit is contained in:
parent
eb50aaed0e
commit
52fdf30433
@ -74,7 +74,7 @@ public:
|
||||
virtual void validateConnectivity() const;
|
||||
|
||||
virtual bool canCompute() const = 0;
|
||||
virtual bool compute() = 0;
|
||||
virtual void compute() = 0;
|
||||
|
||||
size_t numFailures() const override { return 0; }
|
||||
void processFailures(const SolutionProcessor&) const override {}
|
||||
@ -103,7 +103,7 @@ public:
|
||||
void validateConnectivity() const override;
|
||||
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
size_t numSolutions() const override;
|
||||
void processSolutions(const SolutionProcessor &processor) const override;
|
||||
@ -192,7 +192,7 @@ public:
|
||||
Alternatives(const std::string &name = "alternatives") : ParallelContainerBase(name) {}
|
||||
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
};
|
||||
|
||||
|
||||
@ -212,7 +212,7 @@ public:
|
||||
void reset() override;
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
};
|
||||
|
||||
|
||||
@ -227,7 +227,7 @@ public:
|
||||
void reset() override;
|
||||
void init(const core::RobotModelConstPtr &robot_model) override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
protected:
|
||||
Merger(MergerPrivate* impl);
|
||||
@ -263,7 +263,7 @@ public:
|
||||
}
|
||||
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
protected:
|
||||
WrapperBase(WrapperBasePrivate* impl, Stage::pointer &&child = Stage::pointer());
|
||||
|
||||
@ -99,7 +99,7 @@ public:
|
||||
|
||||
// forward these methods to the public interface for containers
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
InterfacePtr pendingBackward() const { return pending_backward_; }
|
||||
InterfacePtr pendingForward() const { return pending_forward_; }
|
||||
|
||||
@ -158,8 +158,6 @@ public:
|
||||
virtual size_t numSolutions() const = 0;
|
||||
virtual size_t numFailures() const = 0;
|
||||
|
||||
bool storeFailures() const;
|
||||
|
||||
typedef std::function<bool(const SolutionBase&)> SolutionProcessor;
|
||||
/// process all solutions in cost order, calling the callback for each of them
|
||||
virtual void processSolutions(const SolutionProcessor &processor) const = 0;
|
||||
@ -232,8 +230,8 @@ public:
|
||||
void reset() override;
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
|
||||
virtual bool computeForward(const InterfaceState& from) = 0;
|
||||
virtual bool computeBackward(const InterfaceState& to) = 0;
|
||||
virtual void computeForward(const InterfaceState& from) = 0;
|
||||
virtual void computeBackward(const InterfaceState& to) = 0;
|
||||
|
||||
void sendForward(const InterfaceState& from,
|
||||
InterfaceState&& to,
|
||||
@ -256,7 +254,7 @@ public:
|
||||
|
||||
private:
|
||||
// restrict access to backward method to provide compile-time check
|
||||
bool computeBackward(const InterfaceState &to) override;
|
||||
void computeBackward(const InterfaceState &to) override;
|
||||
using PropagatingEitherWay::sendBackward;
|
||||
};
|
||||
|
||||
@ -269,7 +267,7 @@ public:
|
||||
|
||||
private:
|
||||
// restrict access to forward method to provide compile-time check
|
||||
bool computeForward(const InterfaceState &from) override;
|
||||
void computeForward(const InterfaceState &from) override;
|
||||
using PropagatingEitherWay::sendForward;
|
||||
};
|
||||
|
||||
@ -281,7 +279,7 @@ public:
|
||||
Generator(const std::string& name);
|
||||
|
||||
virtual bool canCompute() const = 0;
|
||||
virtual bool compute() = 0;
|
||||
virtual void compute() = 0;
|
||||
void spawn(InterfaceState &&state, SubTrajectory &&trajectory);
|
||||
void spawn(InterfaceState &&state, double cost) {
|
||||
SubTrajectory trajectory;
|
||||
@ -330,7 +328,7 @@ public:
|
||||
|
||||
void reset() override;
|
||||
|
||||
virtual bool compute(const InterfaceState& from, const InterfaceState& to) = 0;
|
||||
virtual void compute(const InterfaceState& from, const InterfaceState& to) = 0;
|
||||
void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory);
|
||||
void connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& trajectory, double cost) {
|
||||
trajectory.setCost(cost);
|
||||
|
||||
@ -56,6 +56,7 @@ class StagePrivate {
|
||||
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
|
||||
|
||||
public:
|
||||
// container type used to store children
|
||||
typedef std::list<Stage::pointer> container_type;
|
||||
StagePrivate(Stage* me, const std::string& name);
|
||||
virtual ~StagePrivate() = default;
|
||||
@ -68,7 +69,7 @@ public:
|
||||
virtual void pruneInterface(InterfaceFlags accepted) {}
|
||||
|
||||
virtual bool canCompute() const = 0;
|
||||
virtual bool compute() = 0;
|
||||
virtual void compute() = 0;
|
||||
|
||||
inline const Stage* me() const { return me_; }
|
||||
inline Stage* me() { return me_; }
|
||||
@ -104,6 +105,7 @@ public:
|
||||
void composePropertyErrorMsg(const std::string& name, std::ostream& os);
|
||||
|
||||
void newSolution(SolutionBase& current);
|
||||
bool storeFailures() const { return introspection_ != nullptr; }
|
||||
|
||||
protected:
|
||||
Stage* const me_; // associated/owning Stage instance
|
||||
@ -143,12 +145,6 @@ public:
|
||||
// store trajectory in internal trajectories_ list
|
||||
SubTrajectory& addTrajectory(SubTrajectory&& trajectory);
|
||||
|
||||
// countFailures() serves as a filter before returning the result of compute()
|
||||
inline bool countFailures(bool success) {
|
||||
if (!success) ++num_failures_;
|
||||
return success;
|
||||
}
|
||||
|
||||
private:
|
||||
ordered<SubTrajectory> solutions_;
|
||||
std::list<SubTrajectory> failures_;
|
||||
@ -174,7 +170,7 @@ public:
|
||||
void pruneInterface(InterfaceFlags accepted) override;
|
||||
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
bool hasStartState() const;
|
||||
const InterfaceState &fetchStartState();
|
||||
@ -210,7 +206,7 @@ public:
|
||||
|
||||
InterfaceFlags requiredInterface() const override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
};
|
||||
PIMPL_FUNCTIONS(Generator)
|
||||
|
||||
@ -244,7 +240,7 @@ public:
|
||||
|
||||
InterfaceFlags requiredInterface() const override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
void connect(const robot_trajectory::RobotTrajectoryPtr& t,
|
||||
const InterfaceStatePair& state_pair, double cost);
|
||||
|
||||
@ -67,7 +67,7 @@ public:
|
||||
|
||||
void reset() override;
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
bool compute(const InterfaceState &from, const InterfaceState &to) override;
|
||||
void compute(const InterfaceState &from, const InterfaceState &to) override;
|
||||
|
||||
size_t numSolutions() const override;
|
||||
void processSolutions(const SolutionProcessor &processor) const override;
|
||||
|
||||
@ -51,7 +51,7 @@ public:
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
void setTimeout(const ros::Duration& timeout) {
|
||||
setProperty("timeout", timeout);
|
||||
|
||||
@ -49,8 +49,8 @@ class FixCollisionObjects : public PropagatingEitherWay {
|
||||
public:
|
||||
FixCollisionObjects(const std::string& name = "fix collisions of objects");
|
||||
|
||||
bool computeForward(const InterfaceState& from) override;
|
||||
bool computeBackward(const InterfaceState& to) override;
|
||||
void computeForward(const InterfaceState& from) override;
|
||||
void computeBackward(const InterfaceState& to) override;
|
||||
|
||||
void setMaxPenetration(double penetration);
|
||||
|
||||
|
||||
@ -50,7 +50,7 @@ public:
|
||||
void setState(const planning_scene::PlanningScenePtr& scene);
|
||||
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
protected:
|
||||
planning_scene::PlanningScenePtr scene_;
|
||||
|
||||
@ -47,7 +47,7 @@ public:
|
||||
GenerateGraspPose(const std::string& name);
|
||||
|
||||
void init(const core::RobotModelConstPtr &robot_model) override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
void setEndEffector(const std::string &eef);
|
||||
void setNamedPose(const std::string &pose_name);
|
||||
|
||||
@ -49,7 +49,7 @@ public:
|
||||
|
||||
void reset() override;
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
|
||||
void setPose(const geometry_msgs::PoseStamped pose){
|
||||
setProperty("pose", std::move(pose));
|
||||
|
||||
@ -63,8 +63,8 @@ public:
|
||||
typedef std::function<void(const planning_scene::PlanningScenePtr& scene, const PropertyMap& properties)> ApplyCallback;
|
||||
ModifyPlanningScene(const std::string& name = "modify planning scene");
|
||||
|
||||
bool computeForward(const InterfaceState& from) override;
|
||||
bool computeBackward(const InterfaceState& to) override;
|
||||
void computeForward(const InterfaceState& from) override;
|
||||
void computeBackward(const InterfaceState& to) override;
|
||||
|
||||
/// call an arbitrary function
|
||||
void setCallback(const ApplyCallback& cb) { callback_ = cb; }
|
||||
|
||||
@ -52,8 +52,8 @@ public:
|
||||
MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner);
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
bool computeForward(const InterfaceState& from) override;
|
||||
bool computeBackward(const InterfaceState& to) override;
|
||||
void computeForward(const InterfaceState& from) override;
|
||||
void computeBackward(const InterfaceState& to) override;
|
||||
|
||||
void setGroup(const std::string& group) {
|
||||
setProperty("group", group);
|
||||
@ -103,7 +103,7 @@ public:
|
||||
|
||||
|
||||
protected:
|
||||
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
||||
void compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
||||
SubTrajectory &trajectory, Direction dir);
|
||||
|
||||
protected:
|
||||
|
||||
@ -54,8 +54,8 @@ public:
|
||||
MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner);
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||
bool computeForward(const InterfaceState& from) override;
|
||||
bool computeBackward(const InterfaceState& to) override;
|
||||
void computeForward(const InterfaceState& from) override;
|
||||
void computeBackward(const InterfaceState& to) override;
|
||||
|
||||
void setGroup(const std::string& group) {
|
||||
setProperty("group", group);
|
||||
@ -99,7 +99,7 @@ public:
|
||||
}
|
||||
|
||||
protected:
|
||||
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
||||
void compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
|
||||
SubTrajectory &trajectory, Direction dir);
|
||||
bool getJointStateGoal(moveit::core::RobotState& state);
|
||||
|
||||
|
||||
@ -195,6 +195,7 @@ public:
|
||||
|
||||
inline double cost() const { return cost_; }
|
||||
void setCost(double cost);
|
||||
void markAsFailure() { setCost(std::numeric_limits<double>::infinity()); }
|
||||
inline bool isFailure() const { return !std::isfinite(cost_); }
|
||||
|
||||
const std::string& comment() const { return comment_; }
|
||||
|
||||
@ -131,7 +131,7 @@ public:
|
||||
|
||||
protected:
|
||||
bool canCompute() const override;
|
||||
bool compute() override;
|
||||
void compute() override;
|
||||
void onNewSolution(const SolutionBase &s) override;
|
||||
|
||||
private:
|
||||
|
||||
@ -94,10 +94,10 @@ bool ContainerBasePrivate::canCompute() const
|
||||
return static_cast<ContainerBase*>(me_)->canCompute();
|
||||
}
|
||||
|
||||
bool ContainerBasePrivate::compute()
|
||||
void ContainerBasePrivate::compute()
|
||||
{
|
||||
// call the method of the public interface
|
||||
return static_cast<ContainerBase*>(me_)->compute();
|
||||
static_cast<ContainerBase*>(me_)->compute();
|
||||
}
|
||||
|
||||
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
|
||||
@ -619,31 +619,26 @@ void SerialContainer::validateConnectivity() const
|
||||
|
||||
bool SerialContainer::canCompute() const
|
||||
{
|
||||
size_t num_finished = 0;
|
||||
for(const auto& stage : pimpl()->children()) {
|
||||
if (!stage->pimpl()->canCompute())
|
||||
++num_finished;
|
||||
if (stage->pimpl()->canCompute())
|
||||
return true;
|
||||
}
|
||||
return num_finished < pimpl()->children().size();
|
||||
return false;
|
||||
}
|
||||
|
||||
bool SerialContainer::compute()
|
||||
void SerialContainer::compute()
|
||||
{
|
||||
bool computed = false;
|
||||
for(const auto& stage : pimpl()->children()) {
|
||||
try {
|
||||
if(!stage->pimpl()->canCompute())
|
||||
continue;
|
||||
|
||||
ROS_INFO("Computing stage '%s'", stage->name().c_str());
|
||||
bool success = stage->pimpl()->compute();
|
||||
computed = true;
|
||||
ROS_INFO("Stage '%s': %s", stage->name().c_str(), success ? "succeeded" : "failed");
|
||||
ROS_DEBUG("Computing stage '%s'", stage->name().c_str());
|
||||
stage->pimpl()->compute();
|
||||
} catch (const Property::error &e) {
|
||||
stage->reportPropertyError(e);
|
||||
}
|
||||
}
|
||||
return computed;
|
||||
}
|
||||
|
||||
size_t SerialContainer::numSolutions() const
|
||||
@ -954,16 +949,13 @@ bool WrapperBase::canCompute() const
|
||||
return wrapped()->pimpl()->canCompute();
|
||||
}
|
||||
|
||||
bool WrapperBase::compute()
|
||||
void WrapperBase::compute()
|
||||
{
|
||||
try {
|
||||
size_t num_before = numSolutions();
|
||||
wrapped()->pimpl()->compute();
|
||||
return numSolutions() > num_before;
|
||||
} catch (const Property::error &e) {
|
||||
wrapped()->reportPropertyError(e);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
@ -975,17 +967,15 @@ bool Alternatives::canCompute() const
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Alternatives::compute()
|
||||
void Alternatives::compute()
|
||||
{
|
||||
bool success = false;
|
||||
for (const auto& stage : pimpl()->children()) {
|
||||
try {
|
||||
success |= stage->pimpl()->compute();
|
||||
stage->pimpl()->compute();
|
||||
} catch (const Property::error &e) {
|
||||
stage->reportPropertyError(e);
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
|
||||
@ -1014,17 +1004,16 @@ bool Fallbacks::canCompute() const
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Fallbacks::compute()
|
||||
void Fallbacks::compute()
|
||||
{
|
||||
if (!active_child_)
|
||||
return false;
|
||||
return;
|
||||
|
||||
try {
|
||||
return active_child_->pimpl()->compute();
|
||||
active_child_->pimpl()->compute();
|
||||
} catch (const Property::error &e) {
|
||||
active_child_->reportPropertyError(e);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
@ -1092,17 +1081,15 @@ bool Merger::canCompute() const
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Merger::compute()
|
||||
void Merger::compute()
|
||||
{
|
||||
bool success = false;
|
||||
for (const auto& stage : pimpl()->children()) {
|
||||
try {
|
||||
success |= stage->pimpl()->compute();
|
||||
stage->pimpl()->compute();
|
||||
} catch (const Property::error &e) {
|
||||
stage->reportPropertyError(e);
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
void Merger::onNewSolution(const SolutionBase& s)
|
||||
|
||||
@ -161,11 +161,6 @@ void Stage::setName(const std::string& name)
|
||||
pimpl_->name_ = name;
|
||||
}
|
||||
|
||||
bool Stage::storeFailures() const
|
||||
{
|
||||
return pimpl_->introspection_ != nullptr;
|
||||
}
|
||||
|
||||
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb)
|
||||
{
|
||||
auto impl = pimpl();
|
||||
@ -251,14 +246,14 @@ std::ostream& operator<<(std::ostream& os, const StagePrivate& impl) {
|
||||
}
|
||||
|
||||
SubTrajectory& ComputeBasePrivate::addTrajectory(SubTrajectory&& trajectory) {
|
||||
if (!trajectory.isFailure()) {
|
||||
if (!trajectory.isFailure())
|
||||
return *solutions_.insert(std::move(trajectory));
|
||||
} else if (me()->storeFailures()) {
|
||||
// only store failures when introspection is enabled
|
||||
auto it = failures_.insert(failures_.end(), std::move(trajectory));
|
||||
return *it;
|
||||
} else
|
||||
return trajectory;
|
||||
|
||||
++num_failures_;
|
||||
if (storeFailures())
|
||||
return *failures_.insert(failures_.end(), std::move(trajectory));
|
||||
|
||||
return trajectory;
|
||||
}
|
||||
|
||||
|
||||
@ -382,26 +377,22 @@ bool PropagatingEitherWayPrivate::canCompute() const
|
||||
return hasStartState() || hasEndState();
|
||||
}
|
||||
|
||||
bool PropagatingEitherWayPrivate::compute()
|
||||
void PropagatingEitherWayPrivate::compute()
|
||||
{
|
||||
PropagatingEitherWay* me = static_cast<PropagatingEitherWay*>(me_);
|
||||
|
||||
bool result = false;
|
||||
if (hasStartState()) {
|
||||
const InterfaceState& state = fetchStartState();
|
||||
// enforce property initialization from INTERFACE
|
||||
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
|
||||
if (countFailures(me->computeForward(state)))
|
||||
result |= true;
|
||||
me->computeForward(state);
|
||||
}
|
||||
if (hasEndState()) {
|
||||
const InterfaceState& state = fetchEndState();
|
||||
// enforce property initialization from INTERFACE
|
||||
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
|
||||
if (countFailures(me->computeBackward(state)))
|
||||
result |= true;
|
||||
me->computeBackward(state);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
@ -479,7 +470,7 @@ PropagatingForward::PropagatingForward(const std::string& name)
|
||||
: PropagatingEitherWay(new PropagatingForwardPrivate(this, name))
|
||||
{}
|
||||
|
||||
bool PropagatingForward::computeBackward(const InterfaceState &to)
|
||||
void PropagatingForward::computeBackward(const InterfaceState &to)
|
||||
{
|
||||
assert(false); // This should never be called
|
||||
}
|
||||
@ -497,7 +488,7 @@ PropagatingBackward::PropagatingBackward(const std::string &name)
|
||||
: PropagatingEitherWay(new PropagatingBackwardPrivate(this, name))
|
||||
{}
|
||||
|
||||
bool PropagatingBackward::computeForward(const InterfaceState &from)
|
||||
void PropagatingBackward::computeForward(const InterfaceState &from)
|
||||
{
|
||||
assert(false); // This should never be called
|
||||
}
|
||||
@ -515,8 +506,8 @@ bool GeneratorPrivate::canCompute() const {
|
||||
return static_cast<Generator*>(me_)->canCompute();
|
||||
}
|
||||
|
||||
bool GeneratorPrivate::compute() {
|
||||
return countFailures(static_cast<Generator*>(me_)->compute());
|
||||
void GeneratorPrivate::compute() {
|
||||
static_cast<Generator*>(me_)->compute();
|
||||
}
|
||||
|
||||
|
||||
@ -629,11 +620,11 @@ bool ConnectingPrivate::canCompute() const{
|
||||
return !pending.empty();
|
||||
}
|
||||
|
||||
bool ConnectingPrivate::compute() {
|
||||
void ConnectingPrivate::compute() {
|
||||
const StatePair& top = pending.pop();
|
||||
const InterfaceState& from = *top.first;
|
||||
const InterfaceState& to = *top.second;
|
||||
return countFailures(static_cast<Connecting*>(me_)->compute(from, to));
|
||||
static_cast<Connecting*>(me_)->compute(from, to);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -277,10 +277,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
// validate placed link for collisions
|
||||
collision_detection::CollisionResult collisions;
|
||||
bool colliding = isTargetPoseColliding(sandbox_scene, target_pose, link, &collisions);
|
||||
if (colliding && !storeFailures()) {
|
||||
ROS_WARN_STREAM("eef in collision: " << listCollisionPairs(collisions.contacts, "\n"));
|
||||
return;
|
||||
}
|
||||
|
||||
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
|
||||
|
||||
// markers used for failures
|
||||
@ -300,7 +297,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
SubTrajectory solution;
|
||||
generateCollisionMarkers(sandbox_state, appender, link_to_visualize);
|
||||
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
|
||||
solution.setCost(std::numeric_limits<double>::infinity()); // mark solution as failure
|
||||
solution.markAsFailure();
|
||||
// TODO: visualize collisions
|
||||
solution.setComment("eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
|
||||
spawn(InterfaceState(sandbox_scene), std::move(solution));
|
||||
@ -351,7 +348,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
remaining_time -= std::chrono::duration<double>(now - start_time).count();
|
||||
start_time = now;
|
||||
|
||||
if (succeeded || (storeFailures() && ik_solutions.size() > previous)) {
|
||||
// for all new solutions (successes and failures)
|
||||
for (size_t i = previous; i != ik_solutions.size(); ++i) {
|
||||
// create a new scene for each solution as they will have different robot states
|
||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
||||
SubTrajectory solution;
|
||||
@ -360,11 +358,11 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
|
||||
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
|
||||
|
||||
if (succeeded)
|
||||
if (succeeded && i+1 == ik_solutions.size())
|
||||
// compute cost as distance to compare_pose
|
||||
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
|
||||
else // found an IK solution, but this was not valid
|
||||
solution.setCost(std::numeric_limits<double>::infinity());
|
||||
solution.markAsFailure();
|
||||
|
||||
// set scene's robot state
|
||||
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
|
||||
@ -388,12 +386,11 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
||||
break; // first and only attempt failed
|
||||
}
|
||||
|
||||
if (ik_solutions.empty() && storeFailures()) { // failed to find any solution
|
||||
if (ik_solutions.empty()) { // failed to find any solution
|
||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
||||
SubTrajectory solution;
|
||||
|
||||
// mark solution as invalid
|
||||
solution.setCost(std::numeric_limits<double>::infinity());
|
||||
solution.markAsFailure();
|
||||
solution.setComment("no IK found");
|
||||
|
||||
// ik target link placement
|
||||
|
||||
@ -136,7 +136,7 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
const auto& props = properties();
|
||||
double timeout = props.get<double>("timeout");
|
||||
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
||||
@ -167,26 +167,22 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
}
|
||||
|
||||
SolutionBase* solution = nullptr;
|
||||
if (sub_trajectories.size() != planner_.size()) { // error during sequential planning
|
||||
if (!storeFailures())
|
||||
return false;
|
||||
if (sub_trajectories.size() != planner_.size()) { // error during sequential planning
|
||||
// push back a dummy solution to also show the target scene of the failed attempt
|
||||
sub_trajectories.push_back(robot_trajectory::RobotTrajectoryPtr());
|
||||
solution = storeSequential(sub_trajectories, intermediate_scenes);
|
||||
// mark solution as failure
|
||||
solution->setCost(std::numeric_limits<double>::infinity());
|
||||
solution->markAsFailure();
|
||||
} else {
|
||||
auto t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
|
||||
if (t) {
|
||||
connect(from, to, SubTrajectory(t));
|
||||
return true;
|
||||
return;
|
||||
}
|
||||
// merging failed, store sequentially
|
||||
solution = storeSequential(sub_trajectories, intermediate_scenes);
|
||||
}
|
||||
|
||||
newSolution(from, to, *solution);
|
||||
return !solution->isFailure();
|
||||
}
|
||||
|
||||
SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||
|
||||
@ -64,7 +64,7 @@ bool CurrentState::canCompute() const{
|
||||
return !scene_;
|
||||
}
|
||||
|
||||
bool CurrentState::compute() {
|
||||
void CurrentState::compute() {
|
||||
scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
|
||||
|
||||
ros::NodeHandle h;
|
||||
@ -90,11 +90,10 @@ bool CurrentState::compute() {
|
||||
if (client.call(req, res)) {
|
||||
scene_->setPlanningSceneMsg(res.scene);
|
||||
spawn(InterfaceState(scene_), 0.0);
|
||||
return true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
ROS_WARN("failed to acquire current PlanningScene");
|
||||
return false;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -61,30 +61,29 @@ void FixCollisionObjects::setMaxPenetration(double penetration)
|
||||
setProperty("max_penetration", penetration);
|
||||
}
|
||||
|
||||
bool FixCollisionObjects::computeForward(const InterfaceState &from)
|
||||
void FixCollisionObjects::computeForward(const InterfaceState &from)
|
||||
{
|
||||
planning_scene::PlanningScenePtr to = from.scene()->diff();
|
||||
SubTrajectory solution;
|
||||
bool success = fixCollisions(*to, solution.markers());
|
||||
if (!success) solution.setCost(std::numeric_limits<double>::infinity());
|
||||
if (!success) solution.markAsFailure();
|
||||
sendForward(from, InterfaceState(to), std::move(solution));
|
||||
return success;
|
||||
}
|
||||
|
||||
bool FixCollisionObjects::computeBackward(const InterfaceState &to)
|
||||
void FixCollisionObjects::computeBackward(const InterfaceState &to)
|
||||
{
|
||||
planning_scene::PlanningScenePtr from = to.scene()->diff();
|
||||
SubTrajectory solution;
|
||||
bool success = fixCollisions(*from, solution.markers());
|
||||
if (!success) solution.setCost(std::numeric_limits<double>::infinity());
|
||||
if (!success) solution.markAsFailure();
|
||||
sendBackward(InterfaceState(from), to, std::move(solution));
|
||||
return success;
|
||||
}
|
||||
|
||||
bool FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene, std::deque<visualization_msgs::Marker> &markers) const
|
||||
{
|
||||
const auto& props = properties();
|
||||
double penetration = props.get<double>("max_penetration");
|
||||
(void) penetration;
|
||||
|
||||
collision_detection::CollisionRequest req;
|
||||
collision_detection::CollisionResult res;
|
||||
|
||||
@ -52,9 +52,9 @@ bool FixedState::canCompute() const{
|
||||
return !ran_ && scene_;
|
||||
}
|
||||
|
||||
bool FixedState::compute() {
|
||||
void FixedState::compute() {
|
||||
spawn(InterfaceState(scene_), 0.0);
|
||||
return ran_ = true;
|
||||
ran_ = true;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -121,9 +121,9 @@ void GenerateGraspPose::onNewSolution(const SolutionBase& s)
|
||||
scenes_.push_back(scene);
|
||||
}
|
||||
|
||||
bool GenerateGraspPose::compute() {
|
||||
void GenerateGraspPose::compute() {
|
||||
if (scenes_.empty())
|
||||
return false;
|
||||
return;
|
||||
planning_scene::PlanningSceneConstPtr scene = scenes_[0];
|
||||
scenes_.pop_front();
|
||||
|
||||
@ -151,7 +151,6 @@ bool GenerateGraspPose::compute() {
|
||||
|
||||
spawn(std::move(state), std::move(trajectory));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -64,9 +64,9 @@ bool GeneratePose::canCompute() const {
|
||||
return scenes_.size() > 0;
|
||||
}
|
||||
|
||||
bool GeneratePose::compute() {
|
||||
void GeneratePose::compute() {
|
||||
if (scenes_.empty())
|
||||
return false;
|
||||
return;
|
||||
planning_scene::PlanningSceneConstPtr scene = scenes_[0];
|
||||
scenes_.pop_front();
|
||||
|
||||
@ -75,7 +75,7 @@ bool GeneratePose::compute() {
|
||||
target_pose.header.frame_id = scene->getPlanningFrame();
|
||||
else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
|
||||
ROS_WARN_NAMED("GeneratePose", "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
InterfaceState state(scene);
|
||||
@ -87,7 +87,6 @@ bool GeneratePose::compute() {
|
||||
rviz_marker_tools::appendFrame(trajectory.markers(), target_pose, 0.1, "pose frame");
|
||||
|
||||
spawn(std::move(state), std::move(trajectory));
|
||||
return true;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -64,15 +64,13 @@ void ModifyPlanningScene::allowCollisions(const std::string &first, const moveit
|
||||
allowCollisions(Names({first}), links, allow);
|
||||
}
|
||||
|
||||
bool ModifyPlanningScene::computeForward(const InterfaceState &from){
|
||||
void ModifyPlanningScene::computeForward(const InterfaceState &from){
|
||||
sendForward(from, apply(from, false), SubTrajectory());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ModifyPlanningScene::computeBackward(const InterfaceState &to)
|
||||
void ModifyPlanningScene::computeBackward(const InterfaceState &to)
|
||||
{
|
||||
sendBackward(apply(to, true), to, SubTrajectory());
|
||||
return true;
|
||||
}
|
||||
|
||||
void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene,
|
||||
|
||||
@ -78,8 +78,8 @@ void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model)
|
||||
planner_->init(robot_model);
|
||||
}
|
||||
|
||||
bool MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||
SubTrajectory &trajectory, Direction dir) {
|
||||
void MoveRelative::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||
SubTrajectory &solution, Direction dir) {
|
||||
scene = state.scene()->diff();
|
||||
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
assert(robot_model);
|
||||
@ -90,7 +90,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
|
||||
if (!jmg) {
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group);
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
// only allow single target
|
||||
@ -98,7 +98,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
if (count_goals != 1) {
|
||||
if (count_goals == 0) ROS_WARN_NAMED("MoveRelative", "No goal defined");
|
||||
else ROS_WARN_NAMED("MoveRelative", "Cannot plan to multiple goals");
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
double max_distance = props.get<double>("max_distance");
|
||||
@ -118,7 +118,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
auto jm = robot_model->getJointModel(index);
|
||||
if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) {
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Cannot plan joint target for joint '" << j.first << "' that is not part of group '" << group << "'");
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second);
|
||||
robot_state.enforceBounds(jm);
|
||||
@ -134,7 +134,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
// determine IK link from group
|
||||
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Failed to derive IK target link");
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
ik_pose_msg.header.frame_id = link->getName();
|
||||
ik_pose_msg.pose.orientation.w = 1.0;
|
||||
@ -142,7 +142,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
|
||||
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
|
||||
ROS_WARN_STREAM_NAMED("MoveRelative", "Unknown link: " << ik_pose_msg.header.frame_id);
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
@ -239,7 +239,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
if (!success) {
|
||||
char msg[100];
|
||||
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
|
||||
trajectory.setComment(msg);
|
||||
solution.setComment(msg);
|
||||
}
|
||||
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
||||
success = true;
|
||||
@ -265,41 +265,37 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
||||
}
|
||||
tf::pointEigenToMsg(pos, m.pose.position);
|
||||
tf::quaternionEigenToMsg(quat, m.pose.orientation);
|
||||
trajectory.markers().push_back(m);
|
||||
solution.markers().push_back(m);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// store result
|
||||
if (robot_trajectory && (success || storeFailures())) {
|
||||
if (robot_trajectory) {
|
||||
scene->setCurrentState(robot_trajectory->getLastWayPoint());
|
||||
if (dir == BACKWARD) robot_trajectory->reverse();
|
||||
trajectory.setTrajectory(robot_trajectory);
|
||||
solution.setTrajectory(robot_trajectory);
|
||||
if (!success)
|
||||
solution.markAsFailure();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool MoveRelative::computeForward(const InterfaceState &from)
|
||||
void MoveRelative::computeForward(const InterfaceState &from)
|
||||
{
|
||||
planning_scene::PlanningScenePtr to;
|
||||
SubTrajectory trajectory;
|
||||
|
||||
bool success = compute(from, to, trajectory, FORWARD);
|
||||
if (!success) trajectory.setCost(std::numeric_limits<double>::infinity());
|
||||
compute(from, to, trajectory, FORWARD);
|
||||
sendForward(from, InterfaceState(to), std::move(trajectory));
|
||||
return success;
|
||||
}
|
||||
|
||||
bool MoveRelative::computeBackward(const InterfaceState &to)
|
||||
void MoveRelative::computeBackward(const InterfaceState &to)
|
||||
{
|
||||
planning_scene::PlanningScenePtr from;
|
||||
SubTrajectory trajectory;
|
||||
|
||||
bool success = compute(to, from, trajectory, BACKWARD);
|
||||
if (!success) trajectory.setCost(std::numeric_limits<double>::infinity());
|
||||
compute(to, from, trajectory, BACKWARD);
|
||||
sendBackward(InterfaceState(from), to, std::move(trajectory));
|
||||
return success;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -119,7 +119,7 @@ bool MoveTo::getJointStateGoal(moveit::core::RobotState& state) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||
void MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
|
||||
SubTrajectory &solution, Direction dir) {
|
||||
scene = state.scene()->diff();
|
||||
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
|
||||
@ -131,7 +131,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
||||
const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
|
||||
if (!jmg) {
|
||||
ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group);
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
|
||||
@ -143,24 +143,24 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
||||
has_joint_state_goal= getJointStateGoal(scene->getCurrentStateNonConst());
|
||||
} catch (const InitStageException &e) {
|
||||
ROS_WARN_STREAM_NAMED("MoveTo", e.what());
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
size_t cartesian_goals = props.countDefined({"pose", "point"});
|
||||
|
||||
if (cartesian_goals > 1){
|
||||
ROS_WARN_NAMED("MoveTo", "Ambiguous goals: Multiple Cartesian goals defined");
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (cartesian_goals > 0 && has_joint_state_goal){
|
||||
ROS_WARN_NAMED("MoveTo", "Ambiguous goals: Cartesian goals and joint state goals defined");
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (cartesian_goals == 0 && !has_joint_state_goal){
|
||||
ROS_WARN_NAMED("MoveTo", "No goal defined");
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
|
||||
if (has_joint_state_goal) {
|
||||
@ -177,7 +177,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
||||
// determine IK link from group
|
||||
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
|
||||
ROS_WARN_STREAM_NAMED("MoveTo", "Failed to derive IK target link");
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
ik_pose_msg.header.frame_id = link->getName();
|
||||
ik_pose_msg.pose.orientation.w = 1.0;
|
||||
@ -185,7 +185,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
||||
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
|
||||
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
|
||||
ROS_WARN_STREAM_NAMED("MoveTo", "Unknown link: " << ik_pose_msg.header.frame_id);
|
||||
return false;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
@ -230,36 +230,32 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
||||
}
|
||||
|
||||
// store result
|
||||
if (success || (robot_trajectory && storeFailures())) {
|
||||
if (robot_trajectory) {
|
||||
scene->setCurrentState(robot_trajectory->getLastWayPoint());
|
||||
if (dir == BACKWARD) robot_trajectory->reverse();
|
||||
solution.setTrajectory(robot_trajectory);
|
||||
if (!success)
|
||||
solution.markAsFailure();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
// -1 TODO: move these as default implementation to PropagateEitherWay?
|
||||
// Essentially, here compute() is a class-specific worker function
|
||||
bool MoveTo::computeForward(const InterfaceState &from){
|
||||
void MoveTo::computeForward(const InterfaceState &from){
|
||||
planning_scene::PlanningScenePtr to;
|
||||
SubTrajectory trajectory;
|
||||
|
||||
bool success = compute(from, to, trajectory, FORWARD);
|
||||
if (!success) trajectory.setCost(std::numeric_limits<double>::infinity());
|
||||
compute(from, to, trajectory, FORWARD);
|
||||
sendForward(from, InterfaceState(to), std::move(trajectory));
|
||||
return success;
|
||||
}
|
||||
|
||||
bool MoveTo::computeBackward(const InterfaceState &to)
|
||||
void MoveTo::computeBackward(const InterfaceState &to)
|
||||
{
|
||||
planning_scene::PlanningScenePtr from;
|
||||
SubTrajectory trajectory;
|
||||
|
||||
bool success = compute(to, from, trajectory, BACKWARD);
|
||||
if (!success) trajectory.setCost(std::numeric_limits<double>::infinity());
|
||||
compute(to, from, trajectory, BACKWARD);
|
||||
sendBackward(InterfaceState(from), to, std::move(trajectory));
|
||||
return success;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -203,9 +203,9 @@ bool Task::canCompute() const
|
||||
return stages()->canCompute();
|
||||
}
|
||||
|
||||
bool Task::compute()
|
||||
void Task::compute()
|
||||
{
|
||||
return stages()->compute();
|
||||
stages()->compute();
|
||||
}
|
||||
|
||||
bool Task::plan()
|
||||
@ -214,13 +214,11 @@ bool Task::plan()
|
||||
init();
|
||||
|
||||
while(ros::ok() && canCompute()) {
|
||||
if (compute()) {
|
||||
for (const auto& cb : task_cbs_)
|
||||
cb(*this);
|
||||
if (introspection_)
|
||||
introspection_->publishTaskState();
|
||||
} else
|
||||
break;
|
||||
compute();
|
||||
for (const auto& cb : task_cbs_)
|
||||
cb(*this);
|
||||
if (introspection_)
|
||||
introspection_->publishTaskState();
|
||||
}
|
||||
printState();
|
||||
return numSolutions() > 0;
|
||||
|
||||
@ -13,7 +13,7 @@ class GeneratorMockup : public Generator {
|
||||
public:
|
||||
GeneratorMockup(int runs = 0) : Generator("generator"), runs(runs) {}
|
||||
bool canCompute() const override { return runs > 0; }
|
||||
bool compute() override { return --runs >= 0 ? true : false; }
|
||||
void compute() override { if (runs > 0) --runs; }
|
||||
};
|
||||
|
||||
class PropagatorMockup : public PropagatingEitherWay {
|
||||
@ -21,8 +21,8 @@ class PropagatorMockup : public PropagatingEitherWay {
|
||||
int bw_runs = 0;
|
||||
public:
|
||||
PropagatorMockup(int fw = 0, int bw = 0) : PropagatingEitherWay("either way"), fw_runs(fw), bw_runs(bw) {}
|
||||
bool computeForward(const InterfaceState &from) override { return --fw_runs >= 0 ? true : false; }
|
||||
bool computeBackward(const InterfaceState &from) override { return --bw_runs >= 0 ? true : false; }
|
||||
void computeForward(const InterfaceState &from) override { if (fw_runs > 0) --fw_runs; }
|
||||
void computeBackward(const InterfaceState &from) override { if (bw_runs > 0) --bw_runs; }
|
||||
};
|
||||
class ForwardMockup : public PropagatorMockup {
|
||||
public:
|
||||
@ -43,7 +43,7 @@ class ConnectMockup : public Connecting {
|
||||
int runs = 0;
|
||||
public:
|
||||
ConnectMockup(int runs = 0) : Connecting("connect"), runs(runs) {}
|
||||
bool compute(const InterfaceState& from, const InterfaceState& to) override { return --runs >= 0 ? true : false; }
|
||||
void compute(const InterfaceState& from, const InterfaceState& to) override { if (runs > 0) --runs; }
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -28,11 +28,10 @@ public:
|
||||
}
|
||||
|
||||
bool canCompute() const override { return true; }
|
||||
bool compute() override {
|
||||
void compute() override {
|
||||
InterfaceState state(ps);
|
||||
state.properties().set("target_pose", geometry_msgs::PoseStamped());
|
||||
spawn(std::move(state), 0.0);
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user