ExecuteMTCSolution Class

Execute each joint trajectory contained in an MTC solution by sending sequential goals to a JTAC or JTC-like controller, depending on the value of execution_pipeline. After each subtrajectory succeeds, apply the corresponding planning scene diff so that scene updates from the MTC task persist. More...

Declaration

class moveit_pro::behaviors::ExecuteMTCSolution { ... }

Included Headers

#include <execute_mtc_solution.hpp>

Base class

class	AsyncBehaviorBase
	A base class for behaviors which need to asynchronously run a function that might take a long time to complete. More...

Public Member Typedefs Index

using	ServiceClient = rclcpp::Client< moveit_msgs::srv::ApplyPlanningScene >

Public Constructors Index

	ExecuteMTCSolution (const std::string &name, const BT::NodeConfiguration &config, const std::shared_ptr< BehaviorContext > &shared_resources)

Public Member Functions Index

std::shared_future< tl::expected< bool, std::string > > &	getFuture () override
	Gets the shared future which is used to monitor the progress of the async process. More...

Private Member Functions Index

tl::expected< bool, std::string >	doWork () override
	User-implemented function which handles executing the potentially-long-running process. More...
tl::expected< void, std::string >	doHalt () override
	Optionally implement additional work needed to cleanly interrupt the async process. More...
tl::expected< bool, std::string >	initializePlanningSceneServiceClient (const std::string &service_name)
tl::expected< bool, std::string >	applyPlanningScene (const moveit_msgs::msg::PlanningScene &scene_diff) const
tl::expected< bool, std::string >	executeSubTrajectory (const moveit_task_constructor_msgs::msg::SubTrajectory &sub_trajectory, std::size_t index, const std::optional< moveit_pro_controllers_msgs::msg::AdmittanceParameters > &admittance_params, const std::optional< double > &goal_tolerance, const std::optional< double > &path_tolerance, const std::optional< std::vector< double > > &absolute_force_torque_threshold, double goal_duration_tolerance_seconds)

Private Member Attributes Index

std::unique_ptr< TrajectoryExecutionPipeline >	trajectory_executor_
std::mutex	goal_mutex_
bool	goal_active_ = false
std::atomic< bool >	halt_requested_ = false
ServiceClient::SharedPtr	service_client_
std::string	current_service_name_
std::shared_future< tl::expected< bool, std::string > >	future_

Public Static Functions Index

static BT::PortsList	providedPorts ()
static BT::KeyValueVector	metadata ()

Private Static Attributes Index

static constexpr auto	kDescriptionExecuteMTCSolution = ...
static constexpr auto	kPortSolution = "solution"
static constexpr auto	kPortExecutionPipeline = "execution_pipeline"
static constexpr auto	kPortAdmittanceParameters = "admittance_parameters_msg"
static constexpr auto	kPortControllerActionName = "controller_action_name"
static constexpr auto	kPortGoalPositionTolerance = "goal_position_tolerance"
static constexpr auto	kPortPathPositionTolerance = "path_position_tolerance"
static constexpr auto	kPortAbsoluteForceTorqueThreshold = "absolute_force_torque_threshold"
static constexpr auto	kPortGoalDurationTolerance = "goal_duration_tolerance"
static constexpr auto	kPortApplyPlanningSceneService = "apply_planning_scene_service"
static constexpr auto	kPortApplyStartScene = "apply_start_scene"
static constexpr std::chrono::seconds	kServiceCallTimeout { 2 }

Description

Execute each joint trajectory contained in an MTC solution by sending sequential goals to a JTAC or JTC-like controller, depending on the value of execution_pipeline. After each subtrajectory succeeds, apply the corresponding planning scene diff so that scene updates from the MTC task persist.

This behavior is useful when Motion Task Constructor stages rely on planning scene changes such as attaching or detaching collision objects and the execution pipeline must run through a compliant controller, or just a standard JTC-like controller.

The behavior processes each sub-trajectory in the MTC solution sequentially, sending them to the controller and applying any associated planning scene changes after successful execution.

Data Port Name	Port Type	Object Type
solution	input	moveit_task_constructor_msgs::msg::Solution
admittance_parameters_msg	input	moveit_pro_controllers_msgs::msg::AdmittanceParameters
controller_action_name	input	std::string
goal_position_tolerance	input	double
path_position_tolerance	input	double
absolute_force_torque_threshold	input	std::vector<double>
goal_duration_tolerance	input	double
apply_planning_scene_service	input	std::string
apply_start_scene	input	bool

Definition at line 61 of file execute_mtc_solution.hpp.

Public Member Typedefs

ServiceClient

using moveit_pro::behaviors::ExecuteMTCSolution::ServiceClient = rclcpp::Client<moveit_msgs::srv::ApplyPlanningScene>

Definition at line 64 of file execute_mtc_solution.hpp.

Public Constructors

ExecuteMTCSolution()

moveit_pro::behaviors::ExecuteMTCSolution::ExecuteMTCSolution (const std::string & name, const BT::NodeConfiguration & config, const std::shared_ptr< BehaviorContext > & shared_resources)

Declaration at line 66 of file execute_mtc_solution.hpp, definition at line 30 of file execute_mtc_solution.cpp.

Public Member Functions

getFuture()

std::shared_future< tl::expected< bool, std::string > > & moveit_pro::behaviors::ExecuteMTCSolution::getFuture ()

virtual

Gets the shared future which is used to monitor the progress of the async process.

Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member.

This exists to prevent destruction of the derived class while the async process is still in-progress. If the derived class is destroyed, the definitions of the functions used within doWork() will be destroyed too, which will result in the virtual functions in the base class being called instead and cause a fault.

This function will force derived classes to add an instance of this type and return a reference to it. The base class can then use this virtual function to access the shared future in functions like onStart.

By adding this virtual function we're properly demonstrating how this future depends on things from the derived class and the natural flow of object lifetimes will do the hard work for us. The std::shared_future destructor will get the value of the future before the derived class is destructed assuming it's the last reference to the shared state. Doing it this way means neither the base nor derived class should need to implement a destructor which is a nice property to have.

Returns

Returns the shared_future, which should be owned by the child class.

Declaration at line 73 of file execute_mtc_solution.hpp, definition at line 77 of file execute_mtc_solution.cpp.

Private Member Functions

applyPlanningScene()

tl::expected< bool, std::string > moveit_pro::behaviors::ExecuteMTCSolution::applyPlanningScene (const moveit_msgs::msg::PlanningScene & scene_diff)

Declaration at line 101 of file execute_mtc_solution.hpp, definition at line 233 of file execute_mtc_solution.cpp.

doHalt()

tl::expected< void, std::string > moveit_pro::behaviors::ExecuteMTCSolution::doHalt ()

virtual

Optionally implement additional work needed to cleanly interrupt the async process.

The default implementation of this function is a no-op which will not interrupt the async process. This will mean that onHalted will wait until the process finishes before returning.

Returns

Return an empty tl::expected<void, std::string> if the additional work to halt was successful. Return an error message if something failed while doing additional work to halt.

Declaration at line 78 of file execute_mtc_solution.hpp, definition at line 194 of file execute_mtc_solution.cpp.

doWork()

tl::expected< bool, std::string > moveit_pro::behaviors::ExecuteMTCSolution::doWork ()

virtual

User-implemented function which handles executing the potentially-long-running process.

This function is called within an async process in a separate thread.

Returns

A tl::expected which contains a bool indicating task success if the process completed successfully or was canceled, or an error message if the process failed unexpectedly.

Declaration at line 76 of file execute_mtc_solution.hpp, definition at line 82 of file execute_mtc_solution.cpp.

executeSubTrajectory()

tl::expected< bool, std::string > moveit_pro::behaviors::ExecuteMTCSolution::executeSubTrajectory (const moveit_task_constructor_msgs::msg::SubTrajectory & sub_trajectory, std::size_t index, const std::optional< moveit_pro_controllers_msgs::msg::AdmittanceParameters > & admittance_params, const std::optional< double > & goal_tolerance, const std::optional< double > & path_tolerance, const std::optional< std::vector< double > > & absolute_force_torque_threshold, double goal_duration_tolerance_seconds)

Declaration at line 104 of file execute_mtc_solution.hpp, definition at line 260 of file execute_mtc_solution.cpp.

initializePlanningSceneServiceClient()

tl::expected< bool, std::string > moveit_pro::behaviors::ExecuteMTCSolution::initializePlanningSceneServiceClient (const std::string & service_name)

Declaration at line 99 of file execute_mtc_solution.hpp, definition at line 215 of file execute_mtc_solution.cpp.

Private Member Attributes

current_service_name_

std::string moveit_pro::behaviors::ExecuteMTCSolution::current_service_name_

Definition at line 118 of file execute_mtc_solution.hpp.

future_

std::shared_future<tl::expected<bool, std::string> > moveit_pro::behaviors::ExecuteMTCSolution::future_

Definition at line 120 of file execute_mtc_solution.hpp.

goal_active_

bool moveit_pro::behaviors::ExecuteMTCSolution::goal_active_ = false

Definition at line 113 of file execute_mtc_solution.hpp.

goal_mutex_

std::mutex moveit_pro::behaviors::ExecuteMTCSolution::goal_mutex_

Definition at line 112 of file execute_mtc_solution.hpp.

halt_requested_

std::atomic<bool> moveit_pro::behaviors::ExecuteMTCSolution::halt_requested_ = false

Definition at line 114 of file execute_mtc_solution.hpp.

service_client_

ServiceClient::SharedPtr moveit_pro::behaviors::ExecuteMTCSolution::service_client_

Definition at line 117 of file execute_mtc_solution.hpp.

trajectory_executor_

std::unique_ptr<TrajectoryExecutionPipeline> moveit_pro::behaviors::ExecuteMTCSolution::trajectory_executor_

Definition at line 111 of file execute_mtc_solution.hpp.

Public Static Functions

metadata()

BT::KeyValueVector moveit_pro::behaviors::ExecuteMTCSolution::metadata ()

static

Declaration at line 71 of file execute_mtc_solution.hpp, definition at line 71 of file execute_mtc_solution.cpp.

providedPorts()

BT::PortsList moveit_pro::behaviors::ExecuteMTCSolution::providedPorts ()

static

Declaration at line 69 of file execute_mtc_solution.hpp, definition at line 36 of file execute_mtc_solution.cpp.

Private Static Attributes

kDescriptionExecuteMTCSolution

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kDescriptionExecuteMTCSolution

constexpr static

Initialiser

= R"(     <p>Execute each joint trajectory contained in an MTC solution by sending sequential goals to a JTAC or JTC-like     controller, depending on the value of `execution_pipeline`. After each subtrajectory succeeds, apply the     corresponding planning scene diff so that scene updates from the MTC task persist.</p>   )"

Definition at line 80 of file execute_mtc_solution.hpp.

kPortAbsoluteForceTorqueThreshold

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortAbsoluteForceTorqueThreshold = "absolute_force_torque_threshold"

constexpr static

Definition at line 92 of file execute_mtc_solution.hpp.

kPortAdmittanceParameters

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortAdmittanceParameters = "admittance_parameters_msg"

constexpr static

Definition at line 88 of file execute_mtc_solution.hpp.

kPortApplyPlanningSceneService

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortApplyPlanningSceneService = "apply_planning_scene_service"

constexpr static

Definition at line 94 of file execute_mtc_solution.hpp.

kPortApplyStartScene

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortApplyStartScene = "apply_start_scene"

constexpr static

Definition at line 95 of file execute_mtc_solution.hpp.

kPortControllerActionName

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortControllerActionName = "controller_action_name"

constexpr static

Definition at line 89 of file execute_mtc_solution.hpp.

kPortExecutionPipeline

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortExecutionPipeline = "execution_pipeline"

constexpr static

Definition at line 87 of file execute_mtc_solution.hpp.

kPortGoalDurationTolerance

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortGoalDurationTolerance = "goal_duration_tolerance"

constexpr static

Definition at line 93 of file execute_mtc_solution.hpp.

kPortGoalPositionTolerance

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortGoalPositionTolerance = "goal_position_tolerance"

constexpr static

Definition at line 90 of file execute_mtc_solution.hpp.

kPortPathPositionTolerance

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortPathPositionTolerance = "path_position_tolerance"

constexpr static

Definition at line 91 of file execute_mtc_solution.hpp.

kPortSolution

constexpr auto moveit_pro::behaviors::ExecuteMTCSolution::kPortSolution = "solution"

constexpr static

Definition at line 86 of file execute_mtc_solution.hpp.

kServiceCallTimeout

constexpr std::chrono::seconds moveit_pro::behaviors::ExecuteMTCSolution::kServiceCallTimeout { 2 }

constexpr static

Definition at line 97 of file execute_mtc_solution.hpp.

---

The documentation for this class was generated from the following files:

• execute_mtc_solution.hpp
• execute_mtc_solution.cpp

---

Generated via doxygen2docusaurus 2.2.2 by Doxygen 1.9.8.