diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 5fb25215..54b03cf1 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -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()); diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index fecfef24..3161cf68 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -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_; } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index f573cc37..5f232981 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -158,8 +158,6 @@ public: virtual size_t numSolutions() const = 0; virtual size_t numFailures() const = 0; - bool storeFailures() const; - typedef std::function 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); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 9ef2d6ae..f9cb4d6e 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -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 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 solutions_; std::list 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); diff --git a/core/include/moveit/task_constructor/stages/connect.h b/core/include/moveit/task_constructor/stages/connect.h index 35e4df2f..f8c48785 100644 --- a/core/include/moveit/task_constructor/stages/connect.h +++ b/core/include/moveit/task_constructor/stages/connect.h @@ -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; diff --git a/core/include/moveit/task_constructor/stages/current_state.h b/core/include/moveit/task_constructor/stages/current_state.h index 2452149d..1cca86e5 100644 --- a/core/include/moveit/task_constructor/stages/current_state.h +++ b/core/include/moveit/task_constructor/stages/current_state.h @@ -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); diff --git a/core/include/moveit/task_constructor/stages/fix_collision_objects.h b/core/include/moveit/task_constructor/stages/fix_collision_objects.h index 66b63edb..2ce19df9 100644 --- a/core/include/moveit/task_constructor/stages/fix_collision_objects.h +++ b/core/include/moveit/task_constructor/stages/fix_collision_objects.h @@ -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); diff --git a/core/include/moveit/task_constructor/stages/fixed_state.h b/core/include/moveit/task_constructor/stages/fixed_state.h index 4b3952b9..7e68373b 100644 --- a/core/include/moveit/task_constructor/stages/fixed_state.h +++ b/core/include/moveit/task_constructor/stages/fixed_state.h @@ -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_; diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index 1cfc79bf..2c2d4863 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -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); diff --git a/core/include/moveit/task_constructor/stages/generate_pose.h b/core/include/moveit/task_constructor/stages/generate_pose.h index 0817e494..b0cd1dbb 100644 --- a/core/include/moveit/task_constructor/stages/generate_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_pose.h @@ -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)); diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index a6508d90..c60ca02f 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -63,8 +63,8 @@ public: typedef std::function 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; } diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index c98fed94..fd60928c 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -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: diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index c50e9700..7c67a23c 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -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); diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 515bf968..28abb387 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -195,6 +195,7 @@ public: inline double cost() const { return cost_; } void setCost(double cost); + void markAsFailure() { setCost(std::numeric_limits::infinity()); } inline bool isFailure() const { return !std::isfinite(cost_); } const std::string& comment() const { return comment_; } diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 85556066..8ca6ad20 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -131,7 +131,7 @@ public: protected: bool canCompute() const override; - bool compute() override; + void compute() override; void onNewSolution(const SolutionBase &s) override; private: diff --git a/core/src/container.cpp b/core/src/container.cpp index 2dabf27a..3616e2f5 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -94,10 +94,10 @@ bool ContainerBasePrivate::canCompute() const return static_cast(me_)->canCompute(); } -bool ContainerBasePrivate::compute() +void ContainerBasePrivate::compute() { // call the method of the public interface - return static_cast(me_)->compute(); + static_cast(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) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index fd002cb9..5c96bf58 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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(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(me_)->canCompute(); } -bool GeneratorPrivate::compute() { - return countFailures(static_cast(me_)->compute()); +void GeneratorPrivate::compute() { + static_cast(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(me_)->compute(from, to)); + static_cast(me_)->compute(from, to); } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index cc67a1a0..3db63e22 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -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::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(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::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::infinity()); + solution.markAsFailure(); solution.setComment("no IK found"); // ik target link placement diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index a8f2735c..3f47eb53 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -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("timeout"); const auto& path_constraints = props.get("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::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& sub_trajectories, diff --git a/core/src/stages/current_state.cpp b/core/src/stages/current_state.cpp index fc764cec..cfd7d319 100644 --- a/core/src/stages/current_state.cpp +++ b/core/src/stages/current_state.cpp @@ -64,7 +64,7 @@ bool CurrentState::canCompute() const{ return !scene_; } -bool CurrentState::compute() { +void CurrentState::compute() { scene_ = std::make_shared(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; } } } } diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 9b9fdf61..326a2f97 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -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::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::infinity()); + if (!success) solution.markAsFailure(); sendBackward(InterfaceState(from), to, std::move(solution)); - return success; } bool FixCollisionObjects::fixCollisions(planning_scene::PlanningScene &scene, std::deque &markers) const { const auto& props = properties(); double penetration = props.get("max_penetration"); + (void) penetration; collision_detection::CollisionRequest req; collision_detection::CollisionResult res; diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index 99ba79dc..17151463 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -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; } } } } diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 62529d05..3ef54dd3 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -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; } } } } diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index a73f923c..65bb4bd7 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -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; } } } } diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index 609ecf8c..89581742 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -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, diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 42e587dd..a36c0a98 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -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("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(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::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::infinity()); + compute(to, from, trajectory, BACKWARD); sendBackward(InterfaceState(from), to, std::move(trajectory)); - return success; } } } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index e2fc057b..cb46e93d 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -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("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(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::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::infinity()); + compute(to, from, trajectory, BACKWARD); sendBackward(InterfaceState(from), to, std::move(trajectory)); - return success; } } } } diff --git a/core/src/task.cpp b/core/src/task.cpp index 1e807ad1..40468a40 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -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; diff --git a/core/test/test_container.cpp b/core/test/test_container.cpp index fe457f64..cc99d9fd 100644 --- a/core/test/test_container.cpp +++ b/core/test/test_container.cpp @@ -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; } }; diff --git a/core/test/test_stage.cpp b/core/test/test_stage.cpp index fbcb69b3..cca39e3b 100644 --- a/core/test/test_stage.cpp +++ b/core/test/test_stage.cpp @@ -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; } };