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:
Robert Haschke 2018-05-25 07:42:39 +02:00 committed by v4hn
parent eb50aaed0e
commit 52fdf30433
30 changed files with 140 additions and 191 deletions

View File

@ -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());

View File

@ -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_; }

View File

@ -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);

View File

@ -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);

View File

@ -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;

View File

@ -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);

View File

@ -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);

View File

@ -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_;

View File

@ -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);

View File

@ -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));

View File

@ -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; }

View File

@ -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:

View File

@ -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);

View File

@ -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_; }

View File

@ -131,7 +131,7 @@ public:
protected:
bool canCompute() const override;
bool compute() override;
void compute() override;
void onNewSolution(const SolutionBase &s) override;
private:

View File

@ -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)

View File

@ -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);
}

View File

@ -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

View File

@ -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,

View File

@ -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;
}
} } }

View File

@ -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;

View File

@ -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;
}
} } }

View File

@ -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;
}
} } }

View File

@ -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;
}
} } }

View File

@ -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,

View File

@ -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;
}
} } }

View File

@ -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;
}
} } }

View File

@ -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;

View File

@ -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; }
};

View File

@ -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;
}
};