cl_moveit2z::CbMoveEndEffectorTrajectory Class Reference

#include <cb_move_end_effector_trajectory.hpp>

Inheritance diagram for cl_moveit2z::CbMoveEndEffectorTrajectory:

Collaboration diagram for cl_moveit2z::CbMoveEndEffectorTrajectory:

Public Member Functions
	CbMoveEndEffectorTrajectory (std::optional< std::string > tipLink=std::nullopt)
	CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::msg::PoseStamped > &endEffectorTrajectory, std::optional< std::string > tipLink=std::nullopt)
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
virtual void	onEntry () override
virtual void	onExit () override
	CbMoveEndEffectorTrajectory (std::optional< std::string > tipLink=std::nullopt)
	CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::msg::PoseStamped > &endEffectorTrajectory, std::optional< std::string > tipLink=std::nullopt)
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
virtual void	onEntry () override
virtual void	onExit () override
virtual void	update () override
Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
virtual	~SmaccAsyncClientBehavior ()
template<typename TCallback , typename T >
boost::signals2::connection	onSuccess (TCallback callback, T *object)
template<typename TCallback , typename T >
boost::signals2::connection	onFinished (TCallback callback, T *object)
template<typename TCallback , typename T >
boost::signals2::connection	onFailure (TCallback callback, T *object)
void	requestForceFinish ()
void	executeOnEntry () override
void	executeOnExit () override
void	waitOnEntryThread (bool requestFinish)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onSuccess (TCallbackMethod callback, T *object)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFinished (TCallbackMethod callback, T *object)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFailure (TCallbackMethod callback, T *object)
Public Member Functions inherited from smacc2::ISmaccClientBehavior
	ISmaccClientBehavior ()
virtual	~ISmaccClientBehavior ()
ISmaccStateMachine *	getStateMachine ()
std::string	getName () const
template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, bool throwExceptionIfNotExist)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
Public Member Functions inherited from smacc2::ISmaccUpdatable
	ISmaccUpdatable ()
	ISmaccUpdatable (rclcpp::Duration duration)
void	executeUpdate (rclcpp::Node::SharedPtr node)
void	setUpdatePeriod (rclcpp::Duration duration)

Public Attributes
std::optional< std::string >	group_
std::optional< std::string >	tipLink_
std::optional< bool >	allowInitialTrajectoryStateJointDiscontinuity_

Protected Member Functions
ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
void	executeJointSpaceTrajectory (const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
virtual void	generateTrajectory ()
virtual void	createMarkers ()
void	getCurrentEndEffectorPose (std::string globalFrame, tf2::Stamped< tf2::Transform > &currentEndEffectorTransform)
ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
void	executeJointSpaceTrajectory (const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
virtual void	generateTrajectory ()=0
virtual void	createMarkers ()
void	getCurrentEndEffectorPose (std::string globalFrame, tf2::Stamped< tf2::Transform > &currentEndEffectorTransform)
Protected Member Functions inherited from smacc2::SmaccAsyncClientBehavior
void	postSuccessEvent ()
void	postFailureEvent ()
virtual void	dispose () override
bool	isShutdownRequested ()
	onEntry is executed in a new thread. However the current state cannot be left until the onEntry thread finishes. This flag can be checked from the onEntry thread to force finishing the thread.
Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
ISmaccState *	getCurrentState ()
virtual rclcpp::Node::SharedPtr	getNode () const
virtual rclcpp::Logger	getLogger () const
Protected Member Functions inherited from smacc2::ISmaccUpdatable

Protected Attributes
std::vector< geometry_msgs::msg::PoseStamped >	endEffectorTrajectory_
ClMoveit2z *	movegroupClient_ = nullptr
visualization_msgs::msg::MarkerArray	beahiorMarkers_

Private Member Functions
void	initializeROS ()
void	initializeROS ()

Private Attributes
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	markersPub_
rclcpp::Client< moveit_msgs::srv::GetPositionIK >::SharedPtr	iksrv_
std::mutex	m_mutex_
std::function< void(moveit_msgs::msg::RobotTrajectory &)>	postJointDiscontinuityEvent
std::function< void(moveit_msgs::msg::RobotTrajectory &)>	postIncorrectInitialStateEvent
std::function< void()>	postMotionExecutionFailureEvents
bool	autocleanmarkers = false
std::atomic< bool >	markersInitialized_

Detailed Description

Definition at line 51 of file cb_move_end_effector_trajectory.hpp.

Constructor & Destructor Documentation

CbMoveEndEffectorTrajectory() [1/4]

cl_moveit2z::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory ( std::optional< std::string > tipLink = std::nullopt )

inline

Definition at line 70 of file cb_move_end_effector_trajectory.hpp.

                                                                           : tipLink_(tipLink)
  {
  }

CbMoveEndEffectorTrajectory() [2/4]

cl_moveit2z::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory ( const std::vector< geometry_msgs::msg::PoseStamped > & endEffectorTrajectory, std::optional< std::string > tipLink = std::nullopt )

inline

Definition at line 74 of file cb_move_end_effector_trajectory.hpp.

  : tipLink_(tipLink), endEffectorTrajectory_(endEffectorTrajectory)
  {
  }

CbMoveEndEffectorTrajectory() [3/4]

cl_moveit2z::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory

(

std::optional< std::string >

tipLink = std::nullopt

)

CbMoveEndEffectorTrajectory() [4/4]

cl_moveit2z::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory	(	const std::vector< geometry_msgs::msg::PoseStamped > &	endEffectorTrajectory,
		std::optional< std::string >	tipLink = std::nullopt )

Member Function Documentation

computeJointSpaceTrajectory() [1/2]

ComputeJointTrajectoryErrorCode cl_moveit2z::CbMoveEndEffectorTrajectory::computeJointSpaceTrajectory ( moveit_msgs::msg::RobotTrajectory & computedJointTrajectory )

inlineprotected

Definition at line 228 of file cb_move_end_effector_trajectory.hpp.

  {
    // Try to use CpJointSpaceTrajectoryPlanner component (preferred)
    CpJointSpaceTrajectoryPlanner * trajectoryPlanner = nullptr;
    this->requiresComponent(trajectoryPlanner, false);  // Optional component
 
    if (trajectoryPlanner != nullptr)
    {
      // Use component-based trajectory planner (preferred)
      RCLCPP_INFO(
        getLogger(),
        "[CbMoveEndEffectorTrajectory] Using CpJointSpaceTrajectoryPlanner component for IK "
        "trajectory generation");
 
      JointTrajectoryOptions options;
      if (tipLink_ && !tipLink_->empty())
      {
        options.tipLink = *tipLink_;
      }
      if (allowInitialTrajectoryStateJointDiscontinuity_)
      {
        options.allowInitialDiscontinuity = *allowInitialTrajectoryStateJointDiscontinuity_;
      }
 
      auto result = trajectoryPlanner->planFromWaypoints(endEffectorTrajectory_, options);
 
      if (result.success)
      {
        computedJointTrajectory = result.trajectory;
        RCLCPP_INFO(
          getLogger(),
          "[CbMoveEndEffectorTrajectory] IK trajectory generation succeeded (via "
          "CpJointSpaceTrajectoryPlanner)");
        return ComputeJointTrajectoryErrorCode::SUCCESS;
      }
      else
      {
        computedJointTrajectory = result.trajectory;  // Still return partial trajectory
        RCLCPP_WARN(
          getLogger(),
          "[CbMoveEndEffectorTrajectory] IK trajectory generation failed (via "
          "CpJointSpaceTrajectoryPlanner): %s",
          result.errorMessage.c_str());
 
        // Map error codes
        switch (result.errorCode)
        {
          case JointTrajectoryErrorCode::INCORRECT_INITIAL_STATE:
            return ComputeJointTrajectoryErrorCode::INCORRECT_INITIAL_STATE;
          case JointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY:
            return ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY;
          default:
            return ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY;
        }
      }
    }
    else
    {
      // Fallback to legacy IK trajectory generation
      RCLCPP_WARN(
        getLogger(),
        "[CbMoveEndEffectorTrajectory] CpJointSpaceTrajectoryPlanner component not available, "
        "using legacy IK trajectory generation (consider adding CpJointSpaceTrajectoryPlanner "
        "component)");
 
      // LEGACY IMPLEMENTATION (keep for backward compatibility)
      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current state.. waiting");
 
      auto currentState = movegroupClient_->moveGroupClientInterface->getCurrentState(100);
      auto groupname = movegroupClient_->moveGroupClientInterface->getName();
 
      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting joint names");
      auto currentjointnames =
        currentState->getJointModelGroup(groupname)->getActiveJointModelNames();
 
      if (!tipLink_ || *tipLink_ == "")
      {
        tipLink_ = movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
      }
 
      std::vector<double> jointPositions;
      currentState->copyJointGroupPositions(groupname, jointPositions);
 
      std::vector<std::vector<double>> trajectory;
      std::vector<rclcpp::Duration> trajectoryTimeStamps;
 
      trajectory.push_back(jointPositions);
      trajectoryTimeStamps.push_back(rclcpp::Duration(0s));
 
      auto & first = endEffectorTrajectory_.front();
      rclcpp::Time referenceTime(first.header.stamp);
 
      std::vector<int> discontinuityIndexes;
 
      int ikAttempts = 4;
      for (size_t k = 0; k < this->endEffectorTrajectory_.size(); k++)
      {
        auto & pose = this->endEffectorTrajectory_[k];
        auto req = std::make_shared<moveit_msgs::srv::GetPositionIK::Request>();
 
        req->ik_request.ik_link_name = *tipLink_;
        req->ik_request.robot_state.joint_state.name = currentjointnames;
        req->ik_request.robot_state.joint_state.position = jointPositions;
        req->ik_request.group_name = groupname;
        req->ik_request.avoid_collisions = true;
        req->ik_request.pose_stamped = pose;
 
        RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] IK request: " << k << " " << *req);
 
        auto resfut = iksrv_->async_send_request(req);
        auto status = resfut.wait_for(3s);
 
        if (status == std::future_status::ready)
        {
          auto & prevtrajpoint = trajectory.back();
          auto res = resfut.get();
 
          std::stringstream ss;
          for (size_t j = 0; j < res->solution.joint_state.position.size(); j++)
          {
            auto & jointname = res->solution.joint_state.name[j];
            auto it = std::find(currentjointnames.begin(), currentjointnames.end(), jointname);
            if (it != currentjointnames.end())
            {
              int index = std::distance(currentjointnames.begin(), it);
              jointPositions[index] = res->solution.joint_state.position[j];
              ss << jointname << "(" << index << "): " << jointPositions[index] << std::endl;
            }
          }
 
          // Continuity check
          size_t jointindex = 0;
          int discontinuityJointIndex = -1;
          double discontinuityDeltaJointIndex = -1;
          double deltajoint;
 
          bool check = k > 0 || !allowInitialTrajectoryStateJointDiscontinuity_ ||
                       (allowInitialTrajectoryStateJointDiscontinuity_ &&
                        !(*allowInitialTrajectoryStateJointDiscontinuity_));
          if (check)
          {
            for (jointindex = 0; jointindex < jointPositions.size(); jointindex++)
            {
              deltajoint = jointPositions[jointindex] - prevtrajpoint[jointindex];
              if (fabs(deltajoint) > 0.3)
              {
                discontinuityDeltaJointIndex = deltajoint;
                discontinuityJointIndex = jointindex;
              }
            }
          }
 
          if (ikAttempts > 0 && discontinuityJointIndex != -1)
          {
            k--;
            ikAttempts--;
            continue;
          }
          else
          {
            bool discontinuity = false;
            if (ikAttempts == 0)
            {
              discontinuityIndexes.push_back(k);
              discontinuity = true;
            }
 
            ikAttempts = 4;
 
            if (discontinuity && discontinuityJointIndex != -1)
            {
              std::stringstream ss;
              ss << "Traj[" << k << "/" << endEffectorTrajectory_.size() << "] "
                 << currentjointnames[discontinuityJointIndex]
                 << " IK discontinuity : " << discontinuityDeltaJointIndex << std::endl
                 << "prev joint value: " << prevtrajpoint[discontinuityJointIndex] << std::endl
                 << "current joint value: " << jointPositions[discontinuityJointIndex] << std::endl;
 
              ss << std::endl;
              for (size_t ji = 0; ji < jointPositions.size(); ji++)
              {
                ss << currentjointnames[ji] << ": " << jointPositions[ji] << std::endl;
              }
 
              for (size_t kindex = 0; kindex < trajectory.size(); kindex++)
              {
                ss << "[" << kindex << "]: " << trajectory[kindex][discontinuityJointIndex]
                   << std::endl;
              }
 
              if (k == 0)
              {
                ss
                  << "This is the first posture of the trajectory. Maybe the robot initial posture "
                     "is "
                     "not coincident to the initial posture of the generated joint trajectory."
                  << std::endl;
              }
 
              RCLCPP_ERROR_STREAM(getLogger(), ss.str());
 
              trajectory.push_back(jointPositions);
              rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime;
              trajectoryTimeStamps.push_back(durationFromStart);
              continue;
            }
            else
            {
              trajectory.push_back(jointPositions);
              rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime;
              trajectoryTimeStamps.push_back(durationFromStart);
 
              RCLCPP_DEBUG_STREAM(getLogger(), "IK solution: " << res->solution.joint_state);
              RCLCPP_DEBUG_STREAM(getLogger(), "trajpoint: " << std::endl << ss.str());
            }
          }
        }
        else
        {
          RCLCPP_ERROR_STREAM(getLogger(), "[" << getName() << "] wrong IK call");
        }
      }
 
      computedJointTrajectory.joint_trajectory.joint_names = currentjointnames;
      int i = 0;
      for (auto & p : trajectory)
      {
        if (i == 0)  // Skip current state
        {
          i++;
          continue;
        }
 
        trajectory_msgs::msg::JointTrajectoryPoint jp;
        jp.positions = p;
        jp.time_from_start = trajectoryTimeStamps[i];
        computedJointTrajectory.joint_trajectory.points.push_back(jp);
        i++;
      }
 
      if (discontinuityIndexes.size())
      {
        if (discontinuityIndexes[0] == 0)
          return ComputeJointTrajectoryErrorCode::INCORRECT_INITIAL_STATE;
        else
          return ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY;
      }
 
      return ComputeJointTrajectoryErrorCode::SUCCESS;
    }
  }

References cl_moveit2z::JointTrajectoryOptions::allowInitialDiscontinuity, allowInitialTrajectoryStateJointDiscontinuity_, smacc2::ISmaccClientBehavior::currentState, endEffectorTrajectory_, smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), iksrv_, cl_moveit2z::INCORRECT_INITIAL_STATE, cl_moveit2z::JOINT_TRAJECTORY_DISCONTINUITY, movegroupClient_, cl_moveit2z::ClMoveit2z::moveGroupClientInterface, cl_moveit2z::CpJointSpaceTrajectoryPlanner::planFromWaypoints(), smacc2::ISmaccClientBehavior::requiresComponent(), cl_moveit2z::SUCCESS, cl_moveit2z::JointTrajectoryOptions::tipLink, and tipLink_.

Referenced by onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

computeJointSpaceTrajectory() [2/2]

ComputeJointTrajectoryErrorCode cl_moveit2z::CbMoveEndEffectorTrajectory::computeJointSpaceTrajectory ( moveit_msgs::msg::RobotTrajectory & computedJointTrajectory )

protected

createMarkers() [1/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::createMarkers ( )

inlineprotectedvirtual

Reimplemented in cl_moveit2z::CbCircularPivotMotion, cl_moveit2z::CbCircularPivotMotion, and cl_moveit2z::CbCircularPouringMotion.

Definition at line 556 of file cb_move_end_effector_trajectory.hpp.

  {
    tf2::Transform localdirection;
    localdirection.setIdentity();
    localdirection.setOrigin(tf2::Vector3(0.05, 0, 0));
    auto frameid = this->endEffectorTrajectory_.front().header.frame_id;
 
    for (auto & pose : this->endEffectorTrajectory_)
    {
      visualization_msgs::msg::Marker marker;
      marker.header.frame_id = frameid;
      marker.header.stamp = getNode()->now();
      marker.ns = "trajectory";
      marker.id = this->beahiorMarkers_.markers.size();
      marker.type = visualization_msgs::msg::Marker::ARROW;
      marker.action = visualization_msgs::msg::Marker::ADD;
      marker.scale.x = 0.005;
      marker.scale.y = 0.01;
      marker.scale.z = 0.01;
      marker.color.a = 0.8;
      marker.color.r = 1.0;
      marker.color.g = 0;
      marker.color.b = 0;
 
      geometry_msgs::msg::Point start, end;
      start.x = 0;
      start.y = 0;
      start.z = 0;
 
      tf2::Transform basetransform;
      tf2::fromMsg(pose.pose, basetransform);
      // tf2::Transform endarrow = localdirection * basetransform;
 
      end.x = localdirection.getOrigin().x();
      end.y = localdirection.getOrigin().y();
      end.z = localdirection.getOrigin().z();
 
      marker.pose.position = pose.pose.position;
      marker.pose.orientation = pose.pose.orientation;
      marker.points.push_back(start);
      marker.points.push_back(end);
 
      beahiorMarkers_.markers.push_back(marker);
    }
  }

References beahiorMarkers_, endEffectorTrajectory_, and smacc2::ISmaccClientBehavior::getNode().

Referenced by cl_moveit2z::CbCircularPivotMotion::createMarkers(), cl_moveit2z::CbCircularPouringMotion::createMarkers(), and onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

createMarkers() [2/2]

virtual void cl_moveit2z::CbMoveEndEffectorTrajectory::createMarkers ( )

protectedvirtual

Reimplemented in cl_moveit2z::CbCircularPivotMotion, cl_moveit2z::CbCircularPivotMotion, and cl_moveit2z::CbCircularPouringMotion.

executeJointSpaceTrajectory() [1/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::executeJointSpaceTrajectory ( const moveit_msgs::msg::RobotTrajectory & computedJointTrajectory )

inlineprotected

Definition at line 481 of file cb_move_end_effector_trajectory.hpp.

  {
    RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Executing joint trajectory");
 
    // Try to use CpTrajectoryExecutor component (preferred)
    CpTrajectoryExecutor * trajectoryExecutor = nullptr;
    this->requiresComponent(trajectoryExecutor, false);  // Optional component
 
    bool executionSuccess = false;
 
    if (trajectoryExecutor != nullptr)
    {
      // Use component-based trajectory executor (preferred)
      RCLCPP_INFO(
        getLogger(),
        "[CbMoveEndEffectorTrajectory] Using CpTrajectoryExecutor component for execution");
 
      ExecutionOptions execOptions;
      execOptions.trajectoryName = this->getName();
 
      auto execResult = trajectoryExecutor->execute(computedJointTrajectory, execOptions);
      executionSuccess = execResult.success;
 
      if (executionSuccess)
      {
        RCLCPP_INFO(
          getLogger(),
          "[CbMoveEndEffectorTrajectory] Execution succeeded (via CpTrajectoryExecutor)");
      }
      else
      {
        RCLCPP_WARN(
          getLogger(),
          "[CbMoveEndEffectorTrajectory] Execution failed (via CpTrajectoryExecutor): %s",
          execResult.errorMessage.c_str());
      }
    }
    else
    {
      // Fallback to legacy direct execution
      RCLCPP_WARN(
        getLogger(),
        "[CbMoveEndEffectorTrajectory] CpTrajectoryExecutor component not available, using legacy "
        "execution (consider adding CpTrajectoryExecutor component)");
 
      auto executionResult =
        this->movegroupClient_->moveGroupClientInterface->execute(computedJointTrajectory);
      executionSuccess = (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
 
      RCLCPP_INFO(
        getLogger(), "[CbMoveEndEffectorTrajectory] Execution %s (legacy mode)",
        executionSuccess ? "succeeded" : "failed");
    }
 
    // Post events
    if (executionSuccess)
    {
      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] motion execution succeeded");
      movegroupClient_->postEventMotionExecutionSucceded();
      this->postSuccessEvent();
    }
    else
    {
      this->postMotionExecutionFailureEvents();
      this->postFailureEvent();
    }
  }

References cl_moveit2z::CpTrajectoryExecutor::execute(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), movegroupClient_, cl_moveit2z::ClMoveit2z::moveGroupClientInterface, cl_moveit2z::ClMoveit2z::postEventMotionExecutionSucceded(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), postMotionExecutionFailureEvents, smacc2::SmaccAsyncClientBehavior::postSuccessEvent(), smacc2::ISmaccClientBehavior::requiresComponent(), and cl_moveit2z::ExecutionOptions::trajectoryName.

Referenced by cl_moveit2z::CbExecuteLastTrajectory::onEntry(), onEntry(), and cl_moveit2z::CbUndoLastTrajectory::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

executeJointSpaceTrajectory() [2/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::executeJointSpaceTrajectory ( const moveit_msgs::msg::RobotTrajectory & computedJointTrajectory )

protected

generateTrajectory() [1/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::generateTrajectory ( )

inlineprotectedvirtual

Reimplemented in cl_moveit2z::CbCircularPivotMotion, cl_moveit2z::CbCircularPivotMotion, cl_moveit2z::CbCircularPouringMotion, cl_moveit2z::CbExecuteLastTrajectory, cl_moveit2z::CbMoveCartesianRelative2, cl_moveit2z::CbMoveCartesianRelative2, cl_moveit2z::CbUndoLastTrajectory, and cl_moveit2z::CbUndoLastTrajectory.

Definition at line 550 of file cb_move_end_effector_trajectory.hpp.

  {
    // bypass current trajectory, overridden in derived classes
    // this->endEffectorTrajectory_ = ...
  }

Referenced by onEntry().

Here is the caller graph for this function:

generateTrajectory() [2/2]

virtual void cl_moveit2z::CbMoveEndEffectorTrajectory::generateTrajectory ( )

protectedpure virtual

Implemented in cl_moveit2z::CbCircularPivotMotion, cl_moveit2z::CbCircularPivotMotion, cl_moveit2z::CbCircularPouringMotion, cl_moveit2z::CbExecuteLastTrajectory, cl_moveit2z::CbMoveCartesianRelative2, cl_moveit2z::CbMoveCartesianRelative2, cl_moveit2z::CbUndoLastTrajectory, and cl_moveit2z::CbUndoLastTrajectory.

getCurrentEndEffectorPose() [1/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose ( std::string globalFrame, tf2::Stamped< tf2::Transform > & currentEndEffectorTransform )

inlineprotected

Definition at line 608 of file cb_move_end_effector_trajectory.hpp.

  {
    // Use CpTfListener component for transform lookups
    CpTfListener * tfListener = nullptr;
    this->requiresComponent(tfListener, false);  // Optional component
 
    try
    {
      if (!tipLink_ || *tipLink_ == "")
      {
        tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
      }
 
      if (tfListener != nullptr)
      {
        // Use component-based TF listener (preferred)
        auto transformOpt = tfListener->lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0));
        if (transformOpt)
        {
          tf2::fromMsg(transformOpt.value(), currentEndEffectorTransform);
        }
        else
        {
          RCLCPP_ERROR_STREAM(
            getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_
                             << " to " << globalFrame);
        }
      }
      else
      {
        // Fallback to legacy TF2 usage if component not available
        RCLCPP_WARN_STREAM(
          getLogger(), "[" << getName()
                           << "] CpTfListener component not available, using legacy TF2 (consider "
                              "adding CpTfListener component)");
        tf2_ros::Buffer tfBuffer(getNode()->get_clock());
        tf2_ros::TransformListener tfListenerLegacy(tfBuffer);
 
        tf2::fromMsg(
          tfBuffer.lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0), rclcpp::Duration(10s)),
          currentEndEffectorTransform);
      }
    }
    catch (const std::exception & e)
    {
      RCLCPP_ERROR_STREAM(
        getLogger(), "[" << getName() << "] Exception in getCurrentEndEffectorPose: " << e.what());
    }
  }

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_moveit2z::CpTfListener::lookupTransform(), cl_moveit2z::ClMoveit2z::moveGroupClientInterface, smacc2::ISmaccClientBehavior::requiresComponent(), and tipLink_.

Referenced by cl_moveit2z::CbCircularPouringMotion::createMarkers(), cl_moveit2z::CbCircularPouringMotion::generateTrajectory(), and cl_moveit2z::CbMoveCartesianRelative2::generateTrajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

getCurrentEndEffectorPose() [2/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose ( std::string globalFrame, tf2::Stamped< tf2::Transform > & currentEndEffectorTransform )

protected

initializeROS() [1/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::initializeROS ( )

inlineprivate

Definition at line 660 of file cb_move_end_effector_trajectory.hpp.

  {
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] initializing ros");
 
    auto nh = this->getNode();
 
    // Only create marker publisher for legacy mode (when CpTrajectoryVisualizer is not used)
    markersPub_ = nh->create_publisher<visualization_msgs::msg::MarkerArray>(
      "trajectory_markers", rclcpp::QoS(1));
 
    iksrv_ = nh->create_client<moveit_msgs::srv::GetPositionIK>("/compute_ik");
  }

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), iksrv_, and markersPub_.

Referenced by onOrthogonalAllocation().

Here is the call graph for this function:

Here is the caller graph for this function:

initializeROS() [2/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::initializeROS ( )

private

onEntry() [1/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::onEntry ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_moveit2z::CbEndEffectorRotate, cl_moveit2z::CbEndEffectorRotate, cl_moveit2z::CbExecuteLastTrajectory, cl_moveit2z::CbUndoLastTrajectory, and cl_moveit2z::CbUndoLastTrajectory.

Definition at line 110 of file cb_move_end_effector_trajectory.hpp.

  {
    this->requiresClient(movegroupClient_);
 
    // Get optional components for visualization
    CpTrajectoryVisualizer * trajectoryVisualizer = nullptr;
    this->requiresComponent(trajectoryVisualizer, false);  // Optional component
 
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Generating end effector trajectory");
 
    this->generateTrajectory();
 
    if (this->endEffectorTrajectory_.size() == 0)
    {
      RCLCPP_WARN_STREAM(
        getLogger(), "[" << smacc2::demangleSymbol(typeid(*this).name())
                         << "] No points in the trajectory. Skipping behavior.");
      return;
    }
 
    // Use CpTrajectoryVisualizer if available, otherwise use legacy marker system
    if (trajectoryVisualizer != nullptr)
    {
      RCLCPP_INFO_STREAM(
        getLogger(),
        "[" << getName() << "] Setting trajectory visualization using CpTrajectoryVisualizer.");
      trajectoryVisualizer->setTrajectory(this->endEffectorTrajectory_, "trajectory");
    }
    else
    {
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << getName()
                         << "] Creating markers (legacy mode - consider adding "
                            "CpTrajectoryVisualizer component).");
      this->createMarkers();
    }
 
    moveit_msgs::msg::RobotTrajectory computedTrajectory;
 
    RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Computing joint space trajectory.");
 
    auto errorcode = computeJointSpaceTrajectory(computedTrajectory);
 
    bool trajectoryGenerationSuccess = errorcode == ComputeJointTrajectoryErrorCode::SUCCESS;
 
    CpTrajectoryHistory * trajectoryHistory;
    this->requiresComponent(trajectoryHistory);
 
    if (!trajectoryGenerationSuccess)
    {
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << this->getName() << "] Incorrect trajectory. Posting failure event.");
      if (trajectoryHistory != nullptr)
      {
        moveit_msgs::msg::MoveItErrorCodes error;
        error.val = moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION;
        trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error);
      }
 
      movegroupClient_->postEventMotionExecutionFailed();
      this->postFailureEvent();
 
      if (errorcode == ComputeJointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY)
      {
        this->postJointDiscontinuityEvent(computedTrajectory);
      }
      else if (errorcode == ComputeJointTrajectoryErrorCode::INCORRECT_INITIAL_STATE)
      {
        this->postIncorrectInitialStateEvent(computedTrajectory);
      }
      return;
    }
    else
    {
      if (trajectoryHistory != nullptr)
      {
        moveit_msgs::msg::MoveItErrorCodes error;
        error.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
        trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error);
      }
 
      this->executeJointSpaceTrajectory(computedTrajectory);
    }
 
    // handle finishing events
  }

References computeJointSpaceTrajectory(), createMarkers(), smacc2::introspection::demangleSymbol(), endEffectorTrajectory_, executeJointSpaceTrajectory(), generateTrajectory(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), cl_moveit2z::INCORRECT_INITIAL_STATE, cl_moveit2z::JOINT_TRAJECTORY_DISCONTINUITY, movegroupClient_, cl_moveit2z::ClMoveit2z::postEventMotionExecutionFailed(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, cl_moveit2z::CpTrajectoryHistory::pushTrajectory(), smacc2::ISmaccClientBehavior::requiresClient(), smacc2::ISmaccClientBehavior::requiresComponent(), cl_moveit2z::CpTrajectoryVisualizer::setTrajectory(), and cl_moveit2z::SUCCESS.

Here is the call graph for this function:

onEntry() [2/2]

virtual void cl_moveit2z::CbMoveEndEffectorTrajectory::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Reimplemented in cl_moveit2z::CbEndEffectorRotate, cl_moveit2z::CbEndEffectorRotate, cl_moveit2z::CbExecuteLastTrajectory, cl_moveit2z::CbUndoLastTrajectory, and cl_moveit2z::CbUndoLastTrajectory.

onExit() [1/2]

void cl_moveit2z::CbMoveEndEffectorTrajectory::onExit ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 197 of file cb_move_end_effector_trajectory.hpp.

  {
    // Get optional components for visualization cleanup
    CpTrajectoryVisualizer * trajectoryVisualizer = nullptr;
    this->requiresComponent(trajectoryVisualizer, false);  // Optional component
 
    if (trajectoryVisualizer != nullptr && autocleanmarkers)
    {
      RCLCPP_INFO_STREAM(
        getLogger(),
        "[" << getName() << "] Clearing trajectory markers via CpTrajectoryVisualizer.");
      trajectoryVisualizer->clearMarkers();
    }
    else if (autocleanmarkers)
    {
      // Legacy marker cleanup
      std::lock_guard<std::mutex> guard(m_mutex_);
      for (auto & marker : this->beahiorMarkers_.markers)
      {
        marker.header.stamp = getNode()->now();
        marker.action = visualization_msgs::msg::Marker::DELETE;
      }
 
      if (markersPub_)
      {
        markersPub_->publish(beahiorMarkers_);
      }
    }
  }

References autocleanmarkers, beahiorMarkers_, cl_moveit2z::CpTrajectoryVisualizer::clearMarkers(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), m_mutex_, markersPub_, and smacc2::ISmaccClientBehavior::requiresComponent().

Here is the call graph for this function:

onExit() [2/2]

virtual void cl_moveit2z::CbMoveEndEffectorTrajectory::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

onOrthogonalAllocation() [1/2]

template<typename TOrthogonal , typename TSourceObject >

void cl_moveit2z::CbMoveEndEffectorTrajectory::onOrthogonalAllocation ( )

inline

Definition at line 82 of file cb_move_end_effector_trajectory.hpp.

  {
    this->initializeROS();
 
    smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
 
    postJointDiscontinuityEvent = [this](auto traj)
    {
      auto ev = new EvJointDiscontinuity<TSourceObject, TOrthogonal>();
      ev->trajectory = traj;
      this->postEvent(ev);
    };
 
    postIncorrectInitialStateEvent = [this](auto traj)
    {
      auto ev = new EvIncorrectInitialPosition<TSourceObject, TOrthogonal>();
      ev->trajectory = traj;
      this->postEvent(ev);
    };
 
    postMotionExecutionFailureEvents = [this]
    {
      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] motion execution failed");
      movegroupClient_->postEventMotionExecutionFailed();
      this->postEvent<EvMoveGroupMotionExecutionFailed<TSourceObject, TOrthogonal>>();
    };
  }

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), initializeROS(), movegroupClient_, smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation(), smacc2::ISmaccClientBehavior::postEvent(), cl_moveit2z::ClMoveit2z::postEventMotionExecutionFailed(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, and postMotionExecutionFailureEvents.

Here is the call graph for this function:

onOrthogonalAllocation() [2/2]

template<typename TOrthogonal , typename TSourceObject >

void cl_moveit2z::CbMoveEndEffectorTrajectory::onOrthogonalAllocation ( )

inline

Definition at line 69 of file cb_move_end_effector_trajectory.hpp.

  {
    this->initializeROS();
 
    smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
 
    postJointDiscontinuityEvent = [this](auto traj)
    {
      auto ev = new EvJointDiscontinuity<TSourceObject, TOrthogonal>();
      ev->trajectory = traj;
      this->postEvent(ev);
    };
 
    postIncorrectInitialStateEvent = [this](auto traj)
    {
      auto ev = new EvIncorrectInitialPosition<TSourceObject, TOrthogonal>();
      ev->trajectory = traj;
      this->postEvent(ev);
    };
 
    postMotionExecutionFailureEvents = [this]
    {
      RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] motion execution failed");
      movegroupClient_->postEventMotionExecutionFailed();
      this->postEvent<EvMoveGroupMotionExecutionFailed<TSourceObject, TOrthogonal>>();
    };
  }

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), initializeROS(), movegroupClient_, smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation(), smacc2::ISmaccClientBehavior::postEvent(), cl_moveit2z::ClMoveit2z::postEventMotionExecutionFailed(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, and postMotionExecutionFailureEvents.

Here is the call graph for this function:

update()

void cl_moveit2z::CbMoveEndEffectorTrajectory::update ( )

overridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 359 of file cb_move_end_effector_trajectory.cpp.

{
  if (markersInitialized_)
  {
    std::lock_guard<std::mutex> guard(m_mutex_);
    markersPub_->publish(beahiorMarkers_);
  }
}

Member Data Documentation

allowInitialTrajectoryStateJointDiscontinuity_

std::optional< bool > cl_moveit2z::CbMoveEndEffectorTrajectory::allowInitialTrajectoryStateJointDiscontinuity_

Definition at line 68 of file cb_move_end_effector_trajectory.hpp.

Referenced by computeJointSpaceTrajectory().

autocleanmarkers

bool cl_moveit2z::CbMoveEndEffectorTrajectory::autocleanmarkers = false

private

Definition at line 684 of file cb_move_end_effector_trajectory.hpp.

Referenced by onExit().

beahiorMarkers_

visualization_msgs::msg::MarkerArray cl_moveit2z::CbMoveEndEffectorTrajectory::beahiorMarkers_

protected

Definition at line 606 of file cb_move_end_effector_trajectory.hpp.

Referenced by cl_moveit2z::CbCircularPivotMotion::createMarkers(), cl_moveit2z::CbCircularPouringMotion::createMarkers(), createMarkers(), and onExit().

endEffectorTrajectory_

std::vector< geometry_msgs::msg::PoseStamped > cl_moveit2z::CbMoveEndEffectorTrajectory::endEffectorTrajectory_

protected

Definition at line 602 of file cb_move_end_effector_trajectory.hpp.

Referenced by computeJointSpaceTrajectory(), createMarkers(), cl_moveit2z::CbCircularPivotMotion::generateTrajectory(), cl_moveit2z::CbCircularPouringMotion::generateTrajectory(), cl_moveit2z::CbMoveCartesianRelative2::generateTrajectory(), and onEntry().

group_

std::optional< std::string > cl_moveit2z::CbMoveEndEffectorTrajectory::group_

Definition at line 64 of file cb_move_end_effector_trajectory.hpp.

iksrv_

rclcpp::Client< moveit_msgs::srv::GetPositionIK >::SharedPtr cl_moveit2z::CbMoveEndEffectorTrajectory::iksrv_

private

Definition at line 675 of file cb_move_end_effector_trajectory.hpp.

Referenced by computeJointSpaceTrajectory(), and initializeROS().

m_mutex_

std::mutex cl_moveit2z::CbMoveEndEffectorTrajectory::m_mutex_

private

Definition at line 677 of file cb_move_end_effector_trajectory.hpp.

Referenced by onExit().

markersInitialized_

std::atomic<bool> cl_moveit2z::CbMoveEndEffectorTrajectory::markersInitialized_

private

Definition at line 128 of file cb_move_end_effector_trajectory.hpp.

markersPub_

rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr cl_moveit2z::CbMoveEndEffectorTrajectory::markersPub_

private

Definition at line 673 of file cb_move_end_effector_trajectory.hpp.

Referenced by initializeROS(), and onExit().

movegroupClient_

ClMoveit2z * cl_moveit2z::CbMoveEndEffectorTrajectory::movegroupClient_ = nullptr

protected

Definition at line 604 of file cb_move_end_effector_trajectory.hpp.

Referenced by cl_moveit2z::CbCircularPivotMotion::computeCurrentEndEffectorPoseRelativeToPivot(), computeJointSpaceTrajectory(), executeJointSpaceTrajectory(), cl_moveit2z::CbEndEffectorRotate::onEntry(), cl_moveit2z::CbExecuteLastTrajectory::onEntry(), onEntry(), cl_moveit2z::CbUndoLastTrajectory::onEntry(), and onOrthogonalAllocation().

postIncorrectInitialStateEvent

std::function< void(moveit_msgs::msg::RobotTrajectory &)> cl_moveit2z::CbMoveEndEffectorTrajectory::postIncorrectInitialStateEvent

private

Definition at line 680 of file cb_move_end_effector_trajectory.hpp.

Referenced by onEntry(), and onOrthogonalAllocation().

postJointDiscontinuityEvent

std::function< void(moveit_msgs::msg::RobotTrajectory &)> cl_moveit2z::CbMoveEndEffectorTrajectory::postJointDiscontinuityEvent

private

Definition at line 679 of file cb_move_end_effector_trajectory.hpp.

Referenced by onEntry(), and onOrthogonalAllocation().

postMotionExecutionFailureEvents

std::function< void()> cl_moveit2z::CbMoveEndEffectorTrajectory::postMotionExecutionFailureEvents

private

Definition at line 682 of file cb_move_end_effector_trajectory.hpp.

Referenced by executeJointSpaceTrajectory(), and onOrthogonalAllocation().

tipLink_

std::optional< std::string > cl_moveit2z::CbMoveEndEffectorTrajectory::tipLink_

Definition at line 66 of file cb_move_end_effector_trajectory.hpp.

Referenced by cl_moveit2z::CbCircularPivotMotion::CbCircularPivotMotion(), cl_moveit2z::CbCircularPivotMotion::computeCurrentEndEffectorPoseRelativeToPivot(), computeJointSpaceTrajectory(), getCurrentEndEffectorPose(), and cl_moveit2z::CbEndEffectorRotate::onEntry().

---

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

• smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.hpp
• smacc2_client_library/moveit2z_client/include/moveit2z_client/client_behaviors/cb_move_end_effector_trajectory.hpp
• smacc2_client_library/moveit2z_client/src/moveit2z_client/client_behaviors/cb_move_end_effector_trajectory.cpp