MoveIt Pro API
Core Behaviors for MoveIt Pro
Loading...
Searching...
No Matches
moveit_studio::behaviors::StandardTrajectoryExecutor Class Reference

Standard trajectory execution (e.g. JTC) pipeline implementation. More...

#include <execute_mtc_solution.hpp>

Inheritance diagram for moveit_studio::behaviors::StandardTrajectoryExecutor:
Collaboration diagram for moveit_studio::behaviors::StandardTrajectoryExecutor:

Public Types

using ActionType = control_msgs::action::FollowJointTrajectory
 
- Public Types inherited from moveit_studio::behaviors::BaseTrajectoryExecutor< control_msgs::action::FollowJointTrajectory >
using ClientGoalHandle = rclcpp_action::ClientGoalHandle< control_msgs::action::FollowJointTrajectory >
 
using ActionClient = rclcpp_action::Client< control_msgs::action::FollowJointTrajectory >
 

Public Member Functions

 StandardTrajectoryExecutor (rclcpp::Node::SharedPtr node, rclcpp::CallbackGroup::SharedPtr callback_group)
 Constructor for StandardTrajectoryExecutor.
 
tl::expected< bool, std::string > initialize (const std::string &action_name) override
 Initialize the trajectory execution pipeline.
 
tl::expected< bool, std::string > executeTrajectory (const trajectory_msgs::msg::JointTrajectory &joint_trajectory, 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, std::size_t index) override
 Execute a trajectory with the given parameters.
 
tl::expected< void, std::string > cancelExecution () override
 Cancel the currently active trajectory execution.
 
- Public Member Functions inherited from moveit_studio::behaviors::BaseTrajectoryExecutor< control_msgs::action::FollowJointTrajectory >
 BaseTrajectoryExecutor (rclcpp::Node::SharedPtr node, rclcpp::CallbackGroup::SharedPtr callback_group)
 
tl::expected< bool, std::string > initializeActionClient (const std::string &action_name, const std::string &server_description)
 Common action client initialization logic.
 
tl::expected< typename ClientGoalHandle::SharedPtr, std::string > sendGoal (const typename ActionType::Goal &goal, std::size_t trajectory_index)
 Common goal sending logic.
 
tl::expected< bool, std::string > waitForResult (typename ClientGoalHandle::SharedPtr goal_handle, double goal_duration_tolerance_seconds, std::size_t trajectory_index)
 Common result waiting logic.
 
tl::expected< void, std::string > cancelActiveGoal ()
 
virtual std::string extractErrorMessage (const typename ActionType::Result::SharedPtr &result) const=0
 Derived classes implement this to extract error messages from the action result.
 
- Public Member Functions inherited from moveit_studio::behaviors::TrajectoryExecutionPipeline
virtual ~TrajectoryExecutionPipeline ()=default
 

Protected Member Functions

std::string extractErrorMessage (const ActionType::Result::SharedPtr &result) const override
 

Additional Inherited Members

- Public Attributes inherited from moveit_studio::behaviors::BaseTrajectoryExecutor< control_msgs::action::FollowJointTrajectory >
rclcpp::Node::SharedPtr node_
 
rclcpp::CallbackGroup::SharedPtr callback_group_
 
ActionClient::SharedPtr action_client_
 
ClientGoalHandle::SharedPtr active_goal_handle_
 
std::string current_action_name_
 
std::mutex goal_mutex_
 
- Static Public Attributes inherited from moveit_studio::behaviors::BaseTrajectoryExecutor< control_msgs::action::FollowJointTrajectory >
static constexpr std::chrono::seconds kWaitForServerTimeout
 
static constexpr std::chrono::seconds kGoalRequestTimeout
 
static constexpr std::chrono::seconds kCancelRequestTimeout
 

Detailed Description

Standard trajectory execution (e.g. JTC) pipeline implementation.

Member Typedef Documentation

◆ ActionType

using moveit_studio::behaviors::StandardTrajectoryExecutor::ActionType = control_msgs::action::FollowJointTrajectory

Constructor & Destructor Documentation

◆ StandardTrajectoryExecutor()

moveit_studio::behaviors::StandardTrajectoryExecutor::StandardTrajectoryExecutor ( rclcpp::Node::SharedPtr  node,
rclcpp::CallbackGroup::SharedPtr  callback_group 
)
explicit

Constructor for StandardTrajectoryExecutor.

Parameters
nodeROS node for creating action client
callback_groupCallback group for action client operations

Member Function Documentation

◆ cancelExecution()

tl::expected< void, std::string > moveit_studio::behaviors::StandardTrajectoryExecutor::cancelExecution ( )
overridevirtual

Cancel the currently active trajectory execution.

Returns
Success/failure with error message.

Implements moveit_studio::behaviors::TrajectoryExecutionPipeline.

◆ executeTrajectory()

tl::expected< bool, std::string > moveit_studio::behaviors::StandardTrajectoryExecutor::executeTrajectory ( const trajectory_msgs::msg::JointTrajectory &  joint_trajectory,
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,
std::size_t  index 
)
overridevirtual

Execute a trajectory with the given parameters.

Parameters
joint_trajectoryThe joint trajectory to execute.
admittance_paramsAdmittance control parameters (may be ignored by non-JTAC implementations).
goal_toleranceOptional goal position tolerance.
path_toleranceOptional path position tolerance.
absolute_force_torque_thresholdOptional force/torque limits (JTAC-specific).
goal_duration_tolerance_secondsTimeout in seconds (negative means wait indefinitely).
indexSub-trajectory index for error reporting.
Returns
Success/failure with error message

Implements moveit_studio::behaviors::TrajectoryExecutionPipeline.

◆ extractErrorMessage()

std::string moveit_studio::behaviors::StandardTrajectoryExecutor::extractErrorMessage ( const ActionType::Result::SharedPtr &  result) const
overrideprotected

◆ initialize()

tl::expected< bool, std::string > moveit_studio::behaviors::StandardTrajectoryExecutor::initialize ( const std::string &  action_name)
overridevirtual

Initialize the trajectory execution pipeline.

Parameters
action_nameName of the trajectory action server
Returns
Success/failure with error message

Implements moveit_studio::behaviors::TrajectoryExecutionPipeline.


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