mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Stage: generally allow forwarding of interface properties
This commit is contained in:
parent
92296e020f
commit
0dab499533
@ -159,6 +159,13 @@ public:
|
|||||||
void setTimeout(double timeout) { setProperty("timeout", timeout); }
|
void setTimeout(double timeout) { setProperty("timeout", timeout); }
|
||||||
double timeout() const { return properties().get<double>("timeout"); }
|
double timeout() const { return properties().get<double>("timeout"); }
|
||||||
|
|
||||||
|
/// forwarding of properties between interface states
|
||||||
|
void forwardProperties(const InterfaceState& source, InterfaceState& dest);
|
||||||
|
std::set<std::string> forwardedProperties() const
|
||||||
|
{ return properties().get<std::set<std::string>>("forwarded_properties"); }
|
||||||
|
void setForwardedProperties(const std::set<std::string>& names)
|
||||||
|
{ setProperty("forwarded_properties", names); }
|
||||||
|
|
||||||
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
|
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
|
||||||
typedef std::list<SolutionCallback> SolutionCallbackList;
|
typedef std::list<SolutionCallback> SolutionCallbackList;
|
||||||
/// add function to be called for every newly found solution or failure
|
/// add function to be called for every newly found solution or failure
|
||||||
|
|||||||
@ -104,10 +104,6 @@ public:
|
|||||||
void setIgnoreCollisions(bool flag) {
|
void setIgnoreCollisions(bool flag) {
|
||||||
setProperty("ignore_collisions", flag);
|
setProperty("ignore_collisions", flag);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setForwardedProperties(const std::set<std::string>& names) {
|
|
||||||
setProperty("forward_properties", names);
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -105,6 +105,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
|
|||||||
assert(nextStarts());
|
assert(nextStarts());
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
me()->forwardProperties(from, to);
|
||||||
|
|
||||||
auto to_it = states_.insert(states_.end(), std::move(to));
|
auto to_it = states_.insert(states_.end(), std::move(to));
|
||||||
|
|
||||||
@ -122,6 +123,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
|
|||||||
assert(prevEnds());
|
assert(prevEnds());
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
me()->forwardProperties(to, from);
|
||||||
|
|
||||||
auto from_it = states_.insert(states_.end(), std::move(from));
|
auto from_it = states_.insert(states_.end(), std::move(from));
|
||||||
|
|
||||||
@ -181,6 +183,8 @@ Stage::Stage(StagePrivate *impl)
|
|||||||
assert(impl);
|
assert(impl);
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<double>("timeout", "timeout per run (s)");
|
p.declare<double>("timeout", "timeout per run (s)");
|
||||||
|
p.declare<std::set<std::string>>("forwarded_properties", std::set<std::string>(),
|
||||||
|
"set of interface properties to forward");
|
||||||
p.declare<std::string>("marker_ns", "marker namespace");
|
p.declare<std::string>("marker_ns", "marker namespace");
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -246,6 +250,17 @@ void Stage::setName(const std::string& name)
|
|||||||
pimpl_->name_ = name;
|
pimpl_->name_ = name;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest)
|
||||||
|
{
|
||||||
|
const PropertyMap& src = source.properties();
|
||||||
|
PropertyMap& dst = dest.properties();
|
||||||
|
for (const auto& name : forwardedProperties()) {
|
||||||
|
if (!src.hasProperty(name))
|
||||||
|
continue;
|
||||||
|
dst.set(name, src.get(name));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb)
|
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb)
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
|
|||||||
@ -61,7 +61,6 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
|
|||||||
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)");
|
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)");
|
||||||
p.declare<uint32_t>("max_ik_solutions", 1);
|
p.declare<uint32_t>("max_ik_solutions", 1);
|
||||||
p.declare<bool>("ignore_collisions", false);
|
p.declare<bool>("ignore_collisions", false);
|
||||||
p.declare<std::set<std::string>>("forward_properties", "to-be-forwarded properties from input");
|
|
||||||
|
|
||||||
// ik_frame and target_pose are read from the interface
|
// ik_frame and target_pose are read from the interface
|
||||||
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
|
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
|
||||||
@ -370,12 +369,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
robot_state.update();
|
robot_state.update();
|
||||||
|
|
||||||
InterfaceState state(scene);
|
InterfaceState state(scene);
|
||||||
const boost::any &forwards = props.get("forward_properties");
|
forwardProperties(*s.start(), state);
|
||||||
if (!forwards.empty()) {
|
|
||||||
auto p = s.start()->properties();
|
|
||||||
p.exposeTo(state.properties(), boost::any_cast<std::set<std::string>>(forwards));
|
|
||||||
}
|
|
||||||
|
|
||||||
spawn(std::move(state), std::move(solution));
|
spawn(std::move(state), std::move(solution));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user