Rework execution

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 <list>
+
+#include <actionlib/client/simple_action_client.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
+#include <moveit/trajectory_execution_manager/trajectory_execution_manager.h>
+#include <moveit/utils/moveit_error_code.h>
+
+#include <moveit/task_constructor/storage.h>
+#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
+#include <moveit_msgs/PlanningScene.h>
+
+namespace moveit {
+namespace task_constructor {
+inline namespace execution {
+
+class PlanExecution;
+struct ExecutableTrajectory
+{
+	using EffectFn = std::function<void(PlanExecution*)>;
+
+	std::string description;
+	robot_trajectory::RobotTrajectoryConstPtr trajectory;
+	std::vector<std::string> controller_names;
+	// std::list doesn't invalidate iterators upon insertion/deletion
+	std::list<EffectFn> start_effects;
+	std::list<EffectFn> end_effects;
+	bool stop = false;
+};
+// std::list doesn't invalidate iterators upon insertion/deletion
+using ExecutableMotionPlan = std::list<ExecutableTrajectory>;
+
+class PlanExecution
+{
+	planning_scene_monitor::PlanningSceneMonitorPtr psm_;
+	trajectory_execution_manager::TrajectoryExecutionManagerPtr tem_;
+	ExecutableMotionPlan components_;
+	ExecutableMotionPlan::iterator next_;
+
+	void call(const std::list<ExecutableTrajectory::EffectFn>& 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<ExecutableMotionPlan::const_iterator>(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<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>;
+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

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<void(const SubTrajectory&)>& 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<void(const SubTrajectory&)>& 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<void(const SubTrajectory&)>& 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<void(const SubTrajectory&)>& f) const override;
 	void fillMessage(moveit_task_constructor_msgs::Solution& solution,
 	                 Introspection* introspection = nullptr) const override;
 

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;

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

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 <moveit/task_constructor/execution.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/utils/message_checks.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
+#include <moveit/task_constructor/stage.h>
+
+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<ExecutableTrajectory::EffectFn>& 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<ExecuteTaskSolutionSimpleActionClient> fallback;
+	if (!ac) {
+		fallback = std::make_unique<ExecuteTaskSolutionSimpleActionClient>("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

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<void(const SubTrajectory&)>& 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<void(const SubTrajectory&)>& 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<void(const SubTrajectory&)>& f) const {
+	wrapped_->visitSubTrajectories(f);
+}
+
 void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution,
                                   Introspection* introspection) const {
 	wrapped_->fillMessage(solution, introspection);