Component for centralized motion planning operations. More...

#include <cp_motion_planner.hpp>

Inheritance diagram for cl_moveit2z::CpMotionPlanner:

Collaboration diagram for cl_moveit2z::CpMotionPlanner:

Public Member Functions
	CpMotionPlanner ()=default

virtual	~CpMotionPlanner ()=default

void	onInitialize () override
	Initialize the component and get reference to ClMoveit2z client.

PlanningResult	planToPose (const geometry_msgs::msg::PoseStamped &target, const std::optional< std::string > &tipLink=std::nullopt, const PlanningOptions &options={})
	Plan to a Cartesian pose.

PlanningResult	planToJointTarget (const std::map< std::string, double > &jointTargets, const PlanningOptions &options={})
	Plan to joint values.

PlanningResult	planCartesianPath (const std::vector< geometry_msgs::msg::Pose > &waypoints, double maxStep=0.01, double jumpThreshold=0.0, bool avoidCollisions=true)
	Plan a Cartesian path.

moveit::core::RobotStatePtr	getCurrentState (double waitTime=1.0)
	Get current robot state.

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

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Private Member Functions
void	applyPlanningOptions (moveit::planning_interface::MoveGroupInterface &moveGroup, const PlanningOptions &options)
	Apply planning options to MoveGroupInterface.

Private Attributes
ClMoveit2z *	moveit2zClient_ = 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 motion planning operations.

This component wraps all MoveGroupInterface planning operations to provide a consistent interface for planning motions. It supports planning to:

Cartesian poses
Joint configurations
Named targets
Cartesian paths

Pattern: API wrapper component with result types

Definition at line 79 of file cp_motion_planner.hpp.

Constructor & Destructor Documentation

◆ CpMotionPlanner()

cl_moveit2z::CpMotionPlanner::CpMotionPlanner ( )

default

◆ ~CpMotionPlanner()

virtual cl_moveit2z::CpMotionPlanner::~CpMotionPlanner ( )

virtualdefault

Member Function Documentation

◆ applyPlanningOptions()

void cl_moveit2z::CpMotionPlanner::applyPlanningOptions	(	moveit::planning_interface::MoveGroupInterface &	moveGroup,
		const PlanningOptions &	options )

inlineprivate

Apply planning options to MoveGroupInterface.

Parameters

moveGroup	Reference to MoveGroupInterface
options	Planning options to apply

Definition at line 341 of file cp_motion_planner.hpp.

  {
    if (options.planningTime)
    {
      moveGroup.setPlanningTime(*options.planningTime);
    }
 
    if (options.maxVelocityScaling)
    {
      moveGroup.setMaxVelocityScalingFactor(*options.maxVelocityScaling);
    }
 
    if (options.maxAccelerationScaling)
    {
      moveGroup.setMaxAccelerationScalingFactor(*options.maxAccelerationScaling);
    }
 
    if (options.planningPipelineId)
    {
      moveGroup.setPlanningPipelineId(*options.planningPipelineId);
    }
 
    if (options.plannerId)
    {
      moveGroup.setPlannerId(*options.plannerId);
    }
  }

References cl_moveit2z::PlanningOptions::maxAccelerationScaling, cl_moveit2z::PlanningOptions::maxVelocityScaling, cl_moveit2z::PlanningOptions::plannerId, cl_moveit2z::PlanningOptions::planningPipelineId, and cl_moveit2z::PlanningOptions::planningTime.

◆ getCurrentState()

moveit::core::RobotStatePtr cl_moveit2z::CpMotionPlanner::getCurrentState ( double waitTime = 1.0 )

inline

Get current robot state.

Parameters

waitTime Maximum time to wait for state (seconds)

Returns: Pointer to current robot state, or nullptr if unavailable

Definition at line 311 of file cp_motion_planner.hpp.

  {
    if (!moveit2zClient_)
    {
      RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] ClMoveit2z client not available");
      return nullptr;
    }
 
    auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
    try
    {
      return moveGroup->getCurrentState(waitTime);
    }
    catch (const std::exception & e)
    {
      RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] Exception getting current state: %s", e.what());
      return nullptr;
    }
  }

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

Here is the call graph for this function:

◆ onInitialize()

void cl_moveit2z::CpMotionPlanner::onInitialize ( )

inlineoverridevirtual

Initialize the component and get reference to ClMoveit2z client.

Reimplemented from smacc2::ISmaccComponent.

Definition at line 88 of file cp_motion_planner.hpp.

  {
    this->requiresClient(moveit2zClient_);
    RCLCPP_INFO(getLogger(), "[CpMotionPlanner] Component initialized");
  }

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

Here is the call graph for this function:

◆ planCartesianPath()

PlanningResult cl_moveit2z::CpMotionPlanner::planCartesianPath	(	const std::vector< geometry_msgs::msg::Pose > &	waypoints,
		double	maxStep = 0.01,
		double	jumpThreshold = 0.0,
		bool	avoidCollisions = true )

inline

Plan a Cartesian path.

Parameters

waypoints	Vector of waypoint poses
maxStep	Maximum distance between path points (meters)
jumpThreshold	Maximum joint jump threshold (0.0 = disabled)
avoidCollisions	Whether to check for collisions

Returns: PlanningResult with success status and computed trajectory

Definition at line 243 of file cp_motion_planner.hpp.

  {
    PlanningResult result;
 
    if (!moveit2zClient_)
    {
      result.errorMessage = "ClMoveit2z client not available";
      RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
      return result;
    }
 
    if (waypoints.empty())
    {
      result.errorMessage = "No waypoints specified";
      RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
      return result;
    }
 
    auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
    try
    {
      RCLCPP_INFO(
        getLogger(), "[CpMotionPlanner] Planning Cartesian path with %lu waypoints...",
        waypoints.size());
 
      moveit_msgs::msg::RobotTrajectory trajectory;
      double fractionAchieved = moveGroup->computeCartesianPath(
        waypoints, maxStep, jumpThreshold, trajectory, avoidCollisions);
 
      result.plan.trajectory_ = trajectory;
 
      // Consider successful if we achieved at least 95% of the path
      result.success = (fractionAchieved >= 0.95);
 
      if (result.success)
      {
        result.errorCode = moveit::core::MoveItErrorCode::SUCCESS;
        RCLCPP_INFO(
          getLogger(), "[CpMotionPlanner] Cartesian path planning succeeded (%.1f%% achieved)",
          fractionAchieved * 100.0);
      }
      else
      {
        result.errorCode = moveit::core::MoveItErrorCode::PLANNING_FAILED;
        result.errorMessage = "Cartesian path planning achieved only " +
                              std::to_string(fractionAchieved * 100.0) + "% of the path";
        RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
      }
    }
    catch (const std::exception & e)
    {
      result.success = false;
      result.errorMessage = std::string("Exception during Cartesian path planning: ") + e.what();
      RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
    }
 
    return result;
  }

References cl_moveit2z::PlanningResult::errorCode, cl_moveit2z::PlanningResult::errorMessage, smacc2::ISmaccComponent::getLogger(), cl_moveit2z::ClMoveit2z::moveGroupClientInterface, moveit2zClient_, cl_moveit2z::PlanningResult::plan, and cl_moveit2z::PlanningResult::success.

Here is the call graph for this function:

◆ planToJointTarget()

PlanningResult cl_moveit2z::CpMotionPlanner::planToJointTarget	(	const std::map< std::string, double > &	jointTargets,
		const PlanningOptions &	options = {} )

inline

Plan to joint values.

Parameters

jointTargets	Map of joint names to target values
options	Planning configuration options

Returns: PlanningResult with success status and computed plan

Definition at line 176 of file cp_motion_planner.hpp.

                                                                                      {})
  {
    PlanningResult result;
 
    if (!moveit2zClient_)
    {
      result.errorMessage = "ClMoveit2z client not available";
      RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
      return result;
    }
 
    if (jointTargets.empty())
    {
      result.errorMessage = "No joint targets specified";
      RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
      return result;
    }
 
    auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
    try
    {
      // Apply planning options
      applyPlanningOptions(*moveGroup, options);
 
      // Set joint value targets
      moveGroup->setJointValueTarget(jointTargets);
 
      RCLCPP_INFO(
        getLogger(), "[CpMotionPlanner] Planning to joint configuration (%lu joints)...",
        jointTargets.size());
 
      // Plan
      result.errorCode = moveGroup->plan(result.plan);
      result.success = (result.errorCode == moveit::core::MoveItErrorCode::SUCCESS);
 
      if (result.success)
      {
        RCLCPP_INFO(getLogger(), "[CpMotionPlanner] Planning to joint configuration succeeded");
      }
      else
      {
        result.errorMessage = "Planning to joint configuration failed with error code: " +
                              std::to_string(result.errorCode.val);
        RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
      }
    }
    catch (const std::exception & e)
    {
      result.success = false;
      result.errorMessage = std::string("Exception during planning: ") + e.what();
      RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
    }
 
    return result;
  }

Referenced by cl_moveit2z::CbMoveJoints::moveJoints().

Here is the caller graph for this function:

◆ planToPose()

PlanningResult cl_moveit2z::CpMotionPlanner::planToPose	(	const geometry_msgs::msg::PoseStamped &	target,
		const std::optional< std::string > &	tipLink = std::nullopt,
		const PlanningOptions &	options = {} )

inline

Plan to a Cartesian pose.

Parameters

target	Target pose (position and orientation)
tipLink	End effector tip link (optional, uses default if not specified)
options	Planning configuration options

Returns: PlanningResult with success status and computed plan

Definition at line 102 of file cp_motion_planner.hpp.

                                                                                           {})
  {
    PlanningResult result;
 
    if (!moveit2zClient_)
    {
      result.errorMessage = "ClMoveit2z client not available";
      RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
      return result;
    }
 
    auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
    try
    {
      // Apply planning options
      applyPlanningOptions(*moveGroup, options);
 
      // Set pose target
      if (tipLink && !tipLink->empty())
      {
        moveGroup->setPoseTarget(target, *tipLink);
      }
      else
      {
        moveGroup->setPoseTarget(target);
      }
 
      // Set reference frame if specified
      if (options.poseReferenceFrame)
      {
        moveGroup->setPoseReferenceFrame(*options.poseReferenceFrame);
      }
      else if (!target.header.frame_id.empty())
      {
        moveGroup->setPoseReferenceFrame(target.header.frame_id);
      }
 
      RCLCPP_INFO(getLogger(), "[CpMotionPlanner] Planning to pose...");
 
      // Plan
      result.errorCode = moveGroup->plan(result.plan);
      result.success = (result.errorCode == moveit::core::MoveItErrorCode::SUCCESS);
 
      if (result.success)
      {
        RCLCPP_INFO(getLogger(), "[CpMotionPlanner] Planning to pose succeeded");
      }
      else
      {
        result.errorMessage =
          "Planning to pose failed with error code: " + std::to_string(result.errorCode.val);
        RCLCPP_WARN(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
      }
    }
    catch (const std::exception & e)
    {
      result.success = false;
      result.errorMessage = std::string("Exception during planning: ") + e.what();
      RCLCPP_ERROR(getLogger(), "[CpMotionPlanner] %s", result.errorMessage.c_str());
    }
 
    return result;
  }

Referenced by cl_moveit2z::CbMoveEndEffector::moveToAbsolutePose().

Here is the caller graph for this function:

Member Data Documentation

◆ moveit2zClient_

ClMoveit2z* cl_moveit2z::CpMotionPlanner::moveit2zClient_ = nullptr

private

Definition at line 333 of file cp_motion_planner.hpp.

Referenced by getCurrentState(), onInitialize(), and planCartesianPath().

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

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/components/cp_motion_planner.hpp

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CpMotionPlanner()

◆ ~CpMotionPlanner()

Member Function Documentation

◆ applyPlanningOptions()

◆ getCurrentState()

◆ onInitialize()

◆ planCartesianPath()

◆ planToJointTarget()

◆ planToPose()

Member Data Documentation

◆ moveit2zClient_