diff --git a/core/include/moveit/task_constructor/execution.h b/core/include/moveit/task_constructor/execution.h new file mode 100644 index 000000000..ade30e6c7 --- /dev/null +++ b/core/include/moveit/task_constructor/execution.h @@ -0,0 +1,105 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke */ + +#pragma once +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace moveit { +namespace task_constructor { +inline namespace execution { + +class PlanExecution; +struct ExecutableTrajectory +{ + using EffectFn = std::function; + + std::string description; + robot_trajectory::RobotTrajectoryConstPtr trajectory; + std::vector controller_names; + // std::list doesn't invalidate iterators upon insertion/deletion + std::list start_effects; + std::list end_effects; + bool stop = false; +}; +// std::list doesn't invalidate iterators upon insertion/deletion +using ExecutableMotionPlan = std::list; + +class PlanExecution +{ + planning_scene_monitor::PlanningSceneMonitorPtr psm_; + trajectory_execution_manager::TrajectoryExecutionManagerPtr tem_; + ExecutableMotionPlan components_; + ExecutableMotionPlan::iterator next_; + + void call(const std::list& effects, const char* name); + void onDone(const moveit_controller_manager::ExecutionStatus& status); + void onSuccessfulComponent(); + +public: + PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr& psm, + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& tem); + + void prepare(ExecutableMotionPlan components); + moveit::core::MoveItErrorCode run(); + + auto remaining() const { return std::distance(next_, components_.end()); } + + const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const { return psm_; } + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const { + return tem_; + } +}; + +/// Execute trajectory using ExecuteTaskSolution provided as move_group capability +using ExecuteTaskSolutionSimpleActionClient = + actionlib::SimpleActionClient; +bool execute(const SolutionBase& s, ExecuteTaskSolutionSimpleActionClient* ac = nullptr, bool wait = true); + +/// Construct a motion plan for execution with MoveIt's PlanExecution +ExecutableMotionPlan executableMotionPlan(const SolutionBase& s); + +} // namespace execution +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 23d561783..8ec9b4467 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -265,6 +265,11 @@ class CostTerm; class StagePrivate; class ContainerBasePrivate; struct TmpSolutionContext; + +MOVEIT_CLASS_FORWARD(SolutionBase); +MOVEIT_CLASS_FORWARD(SubTrajectory); +MOVEIT_CLASS_FORWARD(SolutionSequence); + /// abstract base class for solutions (primitive and sequences) class SolutionBase { @@ -311,6 +316,8 @@ class SolutionBase auto& markers() { return markers_; } const auto& markers() const { return markers_; } + virtual void visitSubTrajectories(const std::function& f) const = 0; + /// append this solution to Solution msg virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const = 0; @@ -340,7 +347,6 @@ class SolutionBase const InterfaceState* start_ = nullptr; const InterfaceState* end_ = nullptr; }; -MOVEIT_CLASS_FORWARD(SolutionBase); /// SubTrajectory connects interface states of ComputeStages class SubTrajectory : public SolutionBase @@ -354,6 +360,7 @@ class SubTrajectory : public SolutionBase robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; } void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; } + void visitSubTrajectories(const std::function& f) const override; void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override; double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -368,7 +375,6 @@ class SubTrajectory : public SolutionBase // actual trajectory, might be empty robot_trajectory::RobotTrajectoryConstPtr trajectory_; }; -MOVEIT_CLASS_FORWARD(SubTrajectory); /** Sequence of individual sub solutions * @@ -386,6 +392,7 @@ class SolutionSequence : public SolutionBase void push_back(const SolutionBase& solution); + void visitSubTrajectories(const std::function& f) const override; /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; @@ -400,7 +407,6 @@ class SolutionSequence : public SolutionBase /// series of sub solutions container_type subsolutions_; }; -MOVEIT_CLASS_FORWARD(SolutionSequence); /** Wrap an existing solution * @@ -418,6 +424,8 @@ class WrappedSolution : public SolutionBase : SolutionBase(creator, cost), wrapped_(wrapped) {} explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped) : WrappedSolution(creator, wrapped, wrapped->cost()) {} + + void visitSubTrajectories(const std::function& f) const override; void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const override; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index 36ff5f942..19f05a59e 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -123,8 +123,8 @@ class Task : protected WrapperBase moveit::core::MoveItErrorCode plan(size_t max_solutions = 0); /// interrupt current planning (or execution) void preempt(); - /// execute solution, return the result - moveit::core::MoveItErrorCode execute(const SolutionBase& s); + /// execute solution (blocking), returns success + [[deprecated("Use plain function execute(SolutionBase) instead")]] bool execute(const SolutionBase& s); /// print current task state (number of found solutions and propagated states) to std::cout void printState(std::ostream& os = std::cout) const; diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 45cefe57c..39b3e1079 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -22,6 +22,7 @@ add_library(${PROJECT_NAME} container.cpp cost_terms.cpp + execution.cpp introspection.cpp marker_tools.cpp merge.cpp diff --git a/core/src/execution.cpp b/core/src/execution.cpp new file mode 100644 index 000000000..d6591360a --- /dev/null +++ b/core/src/execution.cpp @@ -0,0 +1,165 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke */ + +#include +#include +#include +#include +#include + +using namespace moveit_controller_manager; + +namespace moveit { +namespace task_constructor { +inline namespace execution { + +static const std::string LOGGER("execution"); + +PlanExecution::PlanExecution(const planning_scene_monitor::PlanningSceneMonitorPtr& psm, + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& tem) + : psm_(psm), tem_(tem) {} + +void PlanExecution::prepare(ExecutableMotionPlan components) { + components_ = std::move(components); + next_ = components_.begin(); +} + +moveit::core::MoveItErrorCode PlanExecution::run() { + if (next_ == components_.end()) // empty / already done + return moveit_msgs::MoveItErrorCodes::SUCCESS; + + // TODO: Only stop our own execution + tem_->stopExecution(); // preempt any running trajectory sequence + // TODO: shorten next trajectory segment such that the start point deviation from current is minimized + + // push components to TEM + for (auto it = next_, end = components_.end(); it != end; ++it) { + moveit_msgs::RobotTrajectory msg; + if (it->trajectory && !it->trajectory->empty() && (it->trajectory->getDuration() > 0.0)) + it->trajectory->getRobotTrajectoryMsg(msg); + + if (!tem_->push(msg, it->controller_names)) { + ROS_ERROR_STREAM_NAMED(LOGGER, "Trajectory initialization failed"); + tem_->clear(); + next_ = end; + return moveit_msgs::MoveItErrorCodes::CONTROL_FAILED; + } else if (it->stop) + break; + } + + call(next_->start_effects, "start"); + tem_->execute([this](const ExecutionStatus& status) { onDone(status); }, + [this](std::size_t) { onSuccessfulComponent(); }); + + auto result = tem_->waitForExecution(); + ROS_DEBUG_STREAM_NAMED(LOGGER, remaining() << " segments remaining"); + switch (result) { + case ExecutionStatus::SUCCEEDED: + return moveit_msgs::MoveItErrorCodes::SUCCESS; + case ExecutionStatus::PREEMPTED: + return moveit_msgs::MoveItErrorCodes::PREEMPTED; + case ExecutionStatus::TIMED_OUT: + return moveit_msgs::MoveItErrorCodes::TIMED_OUT; + case ExecutionStatus::ABORTED: + return moveit_msgs::MoveItErrorCodes::ABORT; + default: + return moveit_msgs::MoveItErrorCodes::CONTROL_FAILED; + } +} + +void PlanExecution::call(const std::list& effects, const char* name) { + ROS_DEBUG_STREAM_NAMED(LOGGER + ".apply", effects.size() << " " << name << " effects of: " << next_->description); + for (const auto& f : effects) + f(this); +} + +void PlanExecution::onDone(const ExecutionStatus& /*status*/) {} +void PlanExecution::onSuccessfulComponent() { + call(next_->end_effects, "end"); + bool stop = next_->stop; // stop on user request + if (++next_ == components_.end()) + stop = true; // stop on end of components + if (!stop) + call(next_->start_effects, "start"); +} + +bool execute(const SolutionBase& s, ExecuteTaskSolutionSimpleActionClient* ac, bool wait) { + std::unique_ptr fallback; + if (!ac) { + fallback = std::make_unique("execute_task_solution"); + ac = fallback.get(); + } + ac->waitForServer(); + + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; + s.fillMessage(goal.solution); + s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene); + + ac->sendGoal(goal); + if (wait) { + ac->waitForResult(); + return ac->getResult()->error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS; + } + return true; +} + +ExecutableMotionPlan executableMotionPlan(const SolutionBase& s) { + ExecutableMotionPlan plan; + auto f = [&](const SubTrajectory& sub) { + ExecutableTrajectory t; + t.description = sub.creator()->name(); + t.trajectory = sub.trajectory(); + plan.push_back(std::move(t)); + auto& c = plan.back(); + + // apply planning scene changes (assuming only no-trajectory solution have those) + if (!sub.trajectory()) { + c.end_effects.push_back([scene = sub.end()->scene()](PlanExecution* plan) { + if (!plan->getPlanningSceneMonitor()) + return; + moveit_msgs::PlanningScene diff; + scene->getPlanningSceneDiffMsg(diff); + plan->getPlanningSceneMonitor()->newPlanningSceneMessage(diff); + }); + } + }; + s.visitSubTrajectories(f); + return plan; +} + +} // namespace execution +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/storage.cpp b/core/src/storage.cpp index e59fda669..4b970d09c 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -216,6 +216,10 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In std::copy(markers.begin(), markers.end(), info.markers.begin()); } +void SubTrajectory::visitSubTrajectories(const std::function& f) const { + f(*this); +} + void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { msg.sub_trajectory.emplace_back(); moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back(); @@ -235,6 +239,11 @@ void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } +void SolutionSequence::visitSubTrajectories(const std::function& f) const { + for (const SolutionBase* s : subsolutions_) + s->visitSubTrajectories(f); +} + void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -274,6 +283,10 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co return f(*this, comment); } +void WrappedSolution::visitSubTrajectories(const std::function& f) const { + wrapped_->visitSubTrajectories(f); +} + void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection) const { wrapped_->fillMessage(solution, introspection); diff --git a/core/src/task.cpp b/core/src/task.cpp index 243205262..4d236f219 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -38,10 +38,8 @@ #include #include #include -#include - +#include #include -#include #include #include @@ -262,17 +260,8 @@ void Task::preempt() { pimpl()->preempt_requested_ = true; } -moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) { - actionlib::SimpleActionClient ac("execute_task_solution"); - ac.waitForServer(); - - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; - s.fillMessage(goal.solution, pimpl()->introspection_.get()); - s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene); - - ac.sendGoal(goal); - ac.waitForResult(); - return ac.getResult()->error_code; +bool Task::execute(const SolutionBase& s) { + return ::moveit::task_constructor::execute(s); } void Task::publishAllSolutions(bool wait) { diff --git a/demo/src/pick_place_task.cpp b/demo/src/pick_place_task.cpp index f9732dd27..e37588597 100644 --- a/demo/src/pick_place_task.cpp +++ b/demo/src/pick_place_task.cpp @@ -36,6 +36,7 @@ #include #include +#include namespace moveit_task_constructor_demo { @@ -495,22 +496,24 @@ bool PickPlaceTask::plan() { bool PickPlaceTask::execute() { ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); - moveit_msgs::MoveItErrorCodes execute_result; - - execute_result = task_->execute(*task_->solutions().front()); - // // If you want to inspect the goal message, use this instead: - // actionlib::SimpleActionClient - // execute("execute_task_solution", true); execute.waitForServer(); - // moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; - // task_->solutions().front()->fillMessage(execute_goal.solution); - // execute.sendGoalAndWait(execute_goal); - // execute_result = execute.getResult()->error_code; - - if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val); + +#if 1 // Blocking execution + if (!::moveit::task_constructor::execute(*task_->solutions().front())) +#else // Non-blocking execution + ExecuteTaskSolutionSimpleActionClient ac("execute_task_solution", true); + ac.waitForServer(); + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; + task_->solutions().front()->fillMessage(execute_goal.solution); + + execute.sendGoalAndWait(execute_goal); + moveit_msgs::MoveItErrorCodes execute_result = execute.getResult()->error_code; + + if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) +#endif + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed"); return false; } - return true; } } // namespace moveit_task_constructor_demo