Component for centralized trajectory execution. More...

#include <cp_trajectory_executor.hpp>

Inheritance diagram for cl_moveit2z::CpTrajectoryExecutor:

Collaboration diagram for cl_moveit2z::CpTrajectoryExecutor:

Public Member Functions
	CpTrajectoryExecutor ()=default

virtual	~CpTrajectoryExecutor ()=default

void	onInitialize () override
	Initialize the component and get references to required components/clients.

ExecutionResult	execute (const moveit_msgs::msg::RobotTrajectory &trajectory, const ExecutionOptions &options={})
	Execute a trajectory synchronously.

ExecutionResult	executePlan (const moveit::planning_interface::MoveGroupInterface::Plan &plan, const ExecutionOptions &options={})
	Execute a motion plan synchronously.

void	cancel ()
	Cancel ongoing execution.

CpTrajectoryHistory *	getTrajectoryHistory ()
	Get the trajectory history component.

Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Private Member Functions
void	applyExecutionOptions (moveit::planning_interface::MoveGroupInterface &moveGroup, const ExecutionOptions &options)
	Apply execution options to MoveGroupInterface.

void	recordToHistory (const moveit_msgs::msg::RobotTrajectory &trajectory, const moveit_msgs::msg::MoveItErrorCodes &errorCode, const std::string &name)
	Record trajectory to history component.

Private Attributes
ClMoveit2z *	moveit2zClient_ = nullptr

CpTrajectoryHistory *	trajectoryHistory_ = nullptr

Additional Inherited Members
Protected Member Functions inherited from smacc2::ISmaccComponent
template<typename TOrthogonal , typename TClient >
void	onComponentInitialization ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TClient >
void	requiresClient (TClient *&requiredClientStorage)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingComponent (TArgs... targs)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingNamedComponent (std::string name, TArgs... targs)

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger () const

ISmaccStateMachine *	getStateMachine ()

Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Component for centralized trajectory execution.

This component provides a unified interface for executing trajectories with:

Automatic recording to CpTrajectoryHistory
Consistent error handling and event posting
Execution options (velocity/acceleration scaling)
Execution time tracking

Pattern: Executor with signals (like action clients)

Definition at line 76 of file cp_trajectory_executor.hpp.

Constructor & Destructor Documentation

◆ CpTrajectoryExecutor()

cl_moveit2z::CpTrajectoryExecutor::CpTrajectoryExecutor ( )

default

◆ ~CpTrajectoryExecutor()

virtual cl_moveit2z::CpTrajectoryExecutor::~CpTrajectoryExecutor ( )

virtualdefault

Member Function Documentation

◆ applyExecutionOptions()

void cl_moveit2z::CpTrajectoryExecutor::applyExecutionOptions	(	moveit::planning_interface::MoveGroupInterface &	moveGroup,
		const ExecutionOptions &	options )

inlineprivate

Apply execution options to MoveGroupInterface.

Parameters

moveGroup	Reference to MoveGroupInterface
options	Execution options to apply

Definition at line 238 of file cp_trajectory_executor.hpp.

  {
    if (options.maxVelocityScaling)
    {
      moveGroup.setMaxVelocityScalingFactor(*options.maxVelocityScaling);
    }
 
    if (options.maxAccelerationScaling)
    {
      moveGroup.setMaxAccelerationScalingFactor(*options.maxAccelerationScaling);
    }
  }

References cl_moveit2z::ExecutionOptions::maxAccelerationScaling, and cl_moveit2z::ExecutionOptions::maxVelocityScaling.

◆ cancel()

void cl_moveit2z::CpTrajectoryExecutor::cancel ( )

inline

Cancel ongoing execution.

Note: This is a best-effort cancellation and may not immediately stop the robot

Definition at line 199 of file cp_trajectory_executor.hpp.

  {
    if (!moveit2zClient_)
    {
      RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] Cannot cancel: client not available");
      return;
    }
 
    auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
    try
    {
      moveGroup->stop();
      RCLCPP_INFO(getLogger(), "[CpTrajectoryExecutor] Trajectory execution cancelled");
    }
    catch (const std::exception & e)
    {
      RCLCPP_ERROR(
        getLogger(), "[CpTrajectoryExecutor] Exception during cancellation: %s", e.what());
    }
  }

References smacc2::ISmaccComponent::getLogger(), cl_moveit2z::ClMoveit2z::moveGroupClientInterface, and moveit2zClient_.

Here is the call graph for this function:

◆ execute()

ExecutionResult cl_moveit2z::CpTrajectoryExecutor::execute	(	const moveit_msgs::msg::RobotTrajectory &	trajectory,
		const ExecutionOptions &	options = {} )

inline

Execute a trajectory synchronously.

Parameters

trajectory	The robot trajectory to execute
options	Execution configuration options

Returns: ExecutionResult with success status and error information

Definition at line 114 of file cp_trajectory_executor.hpp.

                                                                                         {})
  {
    ExecutionResult result;
 
    if (!moveit2zClient_)
    {
      result.errorMessage = "ClMoveit2z client not available";
      RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str());
      return result;
    }
 
    auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
    try
    {
      // Apply execution options
      applyExecutionOptions(*moveGroup, options);
 
      RCLCPP_INFO(
        getLogger(), "[CpTrajectoryExecutor] Executing trajectory with %lu points...",
        trajectory.joint_trajectory.points.size());
 
      // Track execution time
      auto startTime = std::chrono::steady_clock::now();
 
      // Execute trajectory
      result.errorCode = moveGroup->execute(trajectory);
 
      auto endTime = std::chrono::steady_clock::now();
      result.executionTime = std::chrono::duration<double>(endTime - startTime);
 
      result.success = (result.errorCode.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
 
      if (result.success)
      {
        RCLCPP_INFO(
          getLogger(), "[CpTrajectoryExecutor] Trajectory execution succeeded (%.2f seconds)",
          result.executionTime.count());
      }
      else
      {
        result.errorMessage =
          "Trajectory execution failed with error code: " + std::to_string(result.errorCode.val);
        RCLCPP_WARN(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str());
      }
 
      // Record to history if enabled
      if (options.recordToHistory && trajectoryHistory_)
      {
        std::string trajName = options.trajectoryName.value_or("unnamed_trajectory");
        recordToHistory(trajectory, result.errorCode, trajName);
      }
    }
    catch (const std::exception & e)
    {
      result.success = false;
      result.errorMessage = std::string("Exception during trajectory execution: ") + e.what();
      RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str());
    }
 
    return result;
  }

Referenced by cl_moveit2z::CbMoveEndEffectorTrajectory::executeJointSpaceTrajectory().

Here is the caller graph for this function:

◆ executePlan()

ExecutionResult cl_moveit2z::CpTrajectoryExecutor::executePlan	(	const moveit::planning_interface::MoveGroupInterface::Plan &	plan,
		const ExecutionOptions &	options = {} )

inline

Execute a motion plan synchronously.

Convenience method that accepts a MoveGroupInterface::Plan directly

Parameters

plan	The motion plan containing the trajectory
options	Execution configuration options

Returns: ExecutionResult with success status and error information

Definition at line 187 of file cp_trajectory_executor.hpp.

                                       {})
  {
    return execute(plan.trajectory_, options);
  }

Referenced by cl_moveit2z::CbMoveJoints::moveJoints(), and cl_moveit2z::CbMoveEndEffector::moveToAbsolutePose().

Here is the caller graph for this function:

◆ getTrajectoryHistory()

CpTrajectoryHistory * cl_moveit2z::CpTrajectoryExecutor::getTrajectoryHistory ( )

inline

Get the trajectory history component.

Returns: Pointer to CpTrajectoryHistory, or nullptr if not available

Definition at line 226 of file cp_trajectory_executor.hpp.

226{ return trajectoryHistory_; }

References trajectoryHistory_.

◆ onInitialize()

void cl_moveit2z::CpTrajectoryExecutor::onInitialize ( )

inlineoverridevirtual

Initialize the component and get references to required components/clients.

Reimplemented from smacc2::ISmaccComponent.

Definition at line 85 of file cp_trajectory_executor.hpp.

  {
    this->requiresClient(moveit2zClient_);
 
    // CpTrajectoryHistory is optional but recommended
    this->requiresComponent(trajectoryHistory_, false);
 
    if (trajectoryHistory_)
    {
      RCLCPP_INFO(
        getLogger(),
        "[CpTrajectoryExecutor] Component initialized with trajectory history recording enabled");
    }
    else
    {
      RCLCPP_WARN(
        getLogger(),
        "[CpTrajectoryExecutor] Component initialized without CpTrajectoryHistory "
        "(trajectory history will not be recorded)");
    }
  }

References smacc2::ISmaccComponent::getLogger(), moveit2zClient_, smacc2::ISmaccComponent::requiresClient(), smacc2::ISmaccComponent::requiresComponent(), and trajectoryHistory_.

Here is the call graph for this function:

◆ recordToHistory()

void cl_moveit2z::CpTrajectoryExecutor::recordToHistory	(	const moveit_msgs::msg::RobotTrajectory &	trajectory,
		const moveit_msgs::msg::MoveItErrorCodes &	errorCode,
		const std::string &	name )

inlineprivate

Record trajectory to history component.

Parameters

trajectory	The executed trajectory
errorCode	Execution result error code
name	Name for this trajectory entry

Definition at line 259 of file cp_trajectory_executor.hpp.

  {
    if (!trajectoryHistory_)
    {
      return;
    }
 
    try
    {
      trajectoryHistory_->pushTrajectory(name, trajectory, errorCode);
      RCLCPP_DEBUG(
        getLogger(), "[CpTrajectoryExecutor] Recorded trajectory '%s' to history", name.c_str());
    }
    catch (const std::exception & e)
    {
      RCLCPP_WARN(
        getLogger(), "[CpTrajectoryExecutor] Failed to record trajectory to history: %s", e.what());
    }
  }