From b520209b6f4c6bd2ed41c7ef08acd06b7468be48 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Mar 2023 10:41:09 +0100 Subject: [PATCH 1/8] Move Task::execute() to free function in execution.h --- .../moveit/task_constructor/execution.h | 52 +++++++++++++++ core/include/moveit/task_constructor/task.h | 4 +- core/src/CMakeLists.txt | 1 + core/src/execution.cpp | 65 +++++++++++++++++++ core/src/task.cpp | 17 +---- 5 files changed, 123 insertions(+), 16 deletions(-) create mode 100644 core/include/moveit/task_constructor/execution.h create mode 100644 core/src/execution.cpp diff --git a/core/include/moveit/task_constructor/execution.h b/core/include/moveit/task_constructor/execution.h new file mode 100644 index 000000000..41c224b4c --- /dev/null +++ b/core/include/moveit/task_constructor/execution.h @@ -0,0 +1,52 @@ +/********************************************************************* + * 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 + +namespace moveit { +namespace task_constructor { + +/// Execute trajectory using ExecuteTaskSolution provided as move_group capability +using ExecuteTaskSolutionSimpleActionClient = + actionlib::SimpleActionClient; +bool execute(const SolutionBase& s, ExecuteTaskSolutionSimpleActionClient* ac = nullptr, bool wait = true); + +} // namespace task_constructor +} // namespace moveit 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..4984e36e6 --- /dev/null +++ b/core/src/execution.cpp @@ -0,0 +1,65 @@ +/********************************************************************* + * 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 + +namespace moveit { +namespace task_constructor { + +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; +} + +} // namespace task_constructor +} // namespace moveit 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) { From 7eaa13447020d5a8e9ce8842a057888992bf3377 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Mar 2023 10:43:51 +0100 Subject: [PATCH 2/8] Fix deprecation warning --- demo/src/pick_place_task.cpp | 31 +++++++++++++++++-------------- 1 file changed, 17 insertions(+), 14 deletions(-) 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 From fda0f2f92e6a8baab53a0d6db0ac6f13a27cea4d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Mar 2023 12:16:34 +0100 Subject: [PATCH 3/8] Add SolutionBase::visitSubTrajectories() --- core/include/moveit/task_constructor/storage.h | 14 +++++++++++--- core/src/storage.cpp | 13 +++++++++++++ 2 files changed, 24 insertions(+), 3 deletions(-) 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/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); From e16805dbb688467e55527dac421797a2c729e956 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Mar 2023 13:43:34 +0100 Subject: [PATCH 4/8] execute(SolutionBase, MoveItCpp) --- .../moveit/task_constructor/execution.h | 5 +++ core/src/execution.cpp | 32 +++++++++++++++++++ 2 files changed, 37 insertions(+) diff --git a/core/include/moveit/task_constructor/execution.h b/core/include/moveit/task_constructor/execution.h index 41c224b4c..6e045adec 100644 --- a/core/include/moveit/task_constructor/execution.h +++ b/core/include/moveit/task_constructor/execution.h @@ -39,6 +39,8 @@ #include #include #include +#include +#include namespace moveit { namespace task_constructor { @@ -48,5 +50,8 @@ 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 +plan_execution::ExecutableMotionPlan executableMotionPlan(const SolutionBase& s); + } // namespace task_constructor } // namespace moveit diff --git a/core/src/execution.cpp b/core/src/execution.cpp index 4984e36e6..863a8bd9d 100644 --- a/core/src/execution.cpp +++ b/core/src/execution.cpp @@ -36,7 +36,11 @@ #include #include +#include #include +#include + +using namespace plan_execution; namespace moveit { namespace task_constructor { @@ -61,5 +65,33 @@ bool execute(const SolutionBase& s, ExecuteTaskSolutionSimpleActionClient* ac, b return true; } +ExecutableMotionPlan executableMotionPlan(const SolutionBase& s) { + ExecutableMotionPlan plan; + auto f = [&](const SubTrajectory& sub) { + // Need to copy *const* RobotTrajectory + Handle nullptr RobotTrajectory + auto copy = sub.trajectory() ? + std::make_shared(*sub.trajectory()) : + std::make_shared(sub.start()->scene()->getRobotModel()); + plan.plan_components_.emplace_back(std::move(copy), sub.creator()->name()); + auto& c = plan.plan_components_.back(); + + moveit_msgs::PlanningScene scene_diff; + sub.end()->scene()->getPlanningSceneDiffMsg(scene_diff); + + if (!moveit::core::isEmpty(scene_diff)) + c.effect_on_success_ = [idx = plan.plan_components_.size() - 1, + diff = std::move(scene_diff)](const ExecutableMotionPlan* plan) { + if (plan->planning_scene_monitor_) { + ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", + "apply effect of " << plan->plan_components_[idx].description_); + return plan->planning_scene_monitor_->newPlanningSceneMessage(diff); + } + return true; + }; + }; + s.visitSubTrajectories(f); + return plan; +} + } // namespace task_constructor } // namespace moveit From 3c4ad1a8beb0f27a14b66ace905a53d3e19c723e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 9 Mar 2023 15:52:34 +0100 Subject: [PATCH 5/8] own PlanExecution - Drop trajectory unwinding (should be handled elsewhere) - Allow multiple start+end effects - Gracefully handle NULL RobotTrajectories --- .../moveit/task_constructor/execution.h | 53 ++++++++- core/src/execution.cpp | 105 ++++++++++++++---- 2 files changed, 133 insertions(+), 25 deletions(-) diff --git a/core/include/moveit/task_constructor/execution.h b/core/include/moveit/task_constructor/execution.h index 6e045adec..29665de0d 100644 --- a/core/include/moveit/task_constructor/execution.h +++ b/core/include/moveit/task_constructor/execution.h @@ -36,14 +36,58 @@ #pragma once -#include #include +#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::vector start_effects; + std::vector end_effects; + bool stop = false; +}; +using ExecutableMotionPlan = std::vector; + +class PlanExecution +{ + planning_scene_monitor::PlanningSceneMonitorPtr psm_; + trajectory_execution_manager::TrajectoryExecutionManagerPtr tem_; + ExecutableMotionPlan components_; + ExecutableMotionPlan::iterator next_; + + void call(const std::vector& 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 = @@ -51,7 +95,8 @@ using ExecuteTaskSolutionSimpleActionClient = bool execute(const SolutionBase& s, ExecuteTaskSolutionSimpleActionClient* ac = nullptr, bool wait = true); /// Construct a motion plan for execution with MoveIt's PlanExecution -plan_execution::ExecutableMotionPlan executableMotionPlan(const SolutionBase& s); +ExecutableMotionPlan executableMotionPlan(const SolutionBase& s); +} // namespace execution } // namespace task_constructor } // namespace moveit diff --git a/core/src/execution.cpp b/core/src/execution.cpp index 863a8bd9d..bdecdd16e 100644 --- a/core/src/execution.cpp +++ b/core/src/execution.cpp @@ -40,10 +40,76 @@ #include #include -using namespace plan_execution; +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; + + // push components to TEM + for (auto it = next_, end = components_.end(); it != end; ++it) { + moveit_msgs::RobotTrajectory msg; + if (it->trajectory) + 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::vector& 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; @@ -68,30 +134,27 @@ bool execute(const SolutionBase& s, ExecuteTaskSolutionSimpleActionClient* ac, b ExecutableMotionPlan executableMotionPlan(const SolutionBase& s) { ExecutableMotionPlan plan; auto f = [&](const SubTrajectory& sub) { - // Need to copy *const* RobotTrajectory + Handle nullptr RobotTrajectory - auto copy = sub.trajectory() ? - std::make_shared(*sub.trajectory()) : - std::make_shared(sub.start()->scene()->getRobotModel()); - plan.plan_components_.emplace_back(std::move(copy), sub.creator()->name()); - auto& c = plan.plan_components_.back(); - - moveit_msgs::PlanningScene scene_diff; - sub.end()->scene()->getPlanningSceneDiffMsg(scene_diff); - - if (!moveit::core::isEmpty(scene_diff)) - c.effect_on_success_ = [idx = plan.plan_components_.size() - 1, - diff = std::move(scene_diff)](const ExecutableMotionPlan* plan) { - if (plan->planning_scene_monitor_) { - ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", - "apply effect of " << plan->plan_components_[idx].description_); - return plan->planning_scene_monitor_->newPlanningSceneMessage(diff); - } - return true; - }; + 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 From 0f2e60cc4a069a2a27fd63a9e83f1c49a0639cc6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 13 Mar 2023 10:21:49 +0100 Subject: [PATCH 6/8] workaround for INVALID_GOAL on zero-duration trajectory See https://github.com/ros-planning/moveit/pull/3362 for an upstream fix.it->trajectory --- core/src/execution.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/src/execution.cpp b/core/src/execution.cpp index bdecdd16e..e226d0f7e 100644 --- a/core/src/execution.cpp +++ b/core/src/execution.cpp @@ -64,8 +64,9 @@ moveit::core::MoveItErrorCode PlanExecution::run() { // push components to TEM for (auto it = next_, end = components_.end(); it != end; ++it) { moveit_msgs::RobotTrajectory msg; - if (it->trajectory) + 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(); From c8e6a9cacc8a02471d181b13e680fa4b5fbf9f24 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 14 Mar 2023 12:45:18 +0100 Subject: [PATCH 7/8] Turn ExecutableMotionPlan and effects into std::list ... to allow remembering iterators into those lists --- core/include/moveit/task_constructor/execution.h | 11 +++++++---- core/src/execution.cpp | 2 +- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/execution.h b/core/include/moveit/task_constructor/execution.h index 29665de0d..ade30e6c7 100644 --- a/core/include/moveit/task_constructor/execution.h +++ b/core/include/moveit/task_constructor/execution.h @@ -35,6 +35,7 @@ /* Authors: Robert Haschke */ #pragma once +#include #include #include @@ -57,11 +58,13 @@ struct ExecutableTrajectory std::string description; robot_trajectory::RobotTrajectoryConstPtr trajectory; std::vector controller_names; - std::vector start_effects; - std::vector end_effects; + // std::list doesn't invalidate iterators upon insertion/deletion + std::list start_effects; + std::list end_effects; bool stop = false; }; -using ExecutableMotionPlan = std::vector; +// std::list doesn't invalidate iterators upon insertion/deletion +using ExecutableMotionPlan = std::list; class PlanExecution { @@ -70,7 +73,7 @@ class PlanExecution ExecutableMotionPlan components_; ExecutableMotionPlan::iterator next_; - void call(const std::vector& effects, const char* name); + void call(const std::list& effects, const char* name); void onDone(const moveit_controller_manager::ExecutionStatus& status); void onSuccessfulComponent(); diff --git a/core/src/execution.cpp b/core/src/execution.cpp index e226d0f7e..b49f56e9e 100644 --- a/core/src/execution.cpp +++ b/core/src/execution.cpp @@ -96,7 +96,7 @@ moveit::core::MoveItErrorCode PlanExecution::run() { } } -void PlanExecution::call(const std::vector& effects, const char* name) { +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); From a8255bdcce6013e1dc7c17e6512a3440d7d8259b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 16 Mar 2023 23:04:46 +0100 Subject: [PATCH 8/8] Preempt running trajectories before pushing new ones --- core/src/execution.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/core/src/execution.cpp b/core/src/execution.cpp index b49f56e9e..d6591360a 100644 --- a/core/src/execution.cpp +++ b/core/src/execution.cpp @@ -61,6 +61,10 @@ 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;