#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	onStateOrthogonalAllocation ()

virtual void	onEntry () override

virtual void	onExit () override

Public Member Functions inherited from cl_moveit2z::CbMoveit2zClientBehaviorBase
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

virtual	~CbMoveit2zClientBehaviorBase ()

Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

virtual	~SmaccAsyncClientBehavior ()

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onSuccess (TCallback callback, T *object)

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onFinished (TCallback callback, T *object)

template<typename TCallback , typename T >
smacc2::SmaccSignalConnection	onFailure (TCallback callback, T *object)

void	requestForceFinish ()

void	executeOnEntry () override

void	executeOnExit () override

void	waitOnEntryThread (bool requestFinish)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	onSuccess (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	onFinished (TCallbackMethod callback, T *object)

template<typename TCallbackMethod , typename T >
smacc2::SmaccSignalConnection	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, ComponentRequirement requirementType=ComponentRequirement::SOFT)

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

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)

Protected Member Functions inherited from cl_moveit2z::CbMoveit2zClientBehaviorBase
void	postMotionSuccess ()

void	postMotionFailure ()

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 Attributes
std::vector< geometry_msgs::msg::PoseStamped >	endEffectorTrajectory_

CpTrajectoryVisualizer *	cpTrajectoryVisualizer_ = nullptr

CpJointSpaceTrajectoryPlanner *	cpJointSpaceTrajectoryPlanner_ = nullptr

CpTfListener *	cpTfListener_ = nullptr

visualization_msgs::msg::MarkerArray	beahiorMarkers_

Protected Attributes inherited from cl_moveit2z::CbMoveit2zClientBehaviorBase
CpMoveGroupInterface *	cpMoveGroup_ = nullptr

CpMotionPlanner *	cpMotionPlanner_ = nullptr

CpTrajectoryExecutor *	cpTrajectoryExecutor_ = nullptr

CpTrajectoryHistory *	cpTrajectoryHistory_ = nullptr

Private Member Functions
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

Detailed Description

Definition at line 58 of file cb_move_end_effector_trajectory.hpp.

Constructor & Destructor Documentation

◆ CbMoveEndEffectorTrajectory() [1/2]

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

inline

Definition at line 68 of file cb_move_end_effector_trajectory.hpp.

                                                                           : tipLink_(tipLink)
  {
  }

◆ CbMoveEndEffectorTrajectory() [2/2]

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

inline

Definition at line 72 of file cb_move_end_effector_trajectory.hpp.

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

Member Function Documentation

◆ computeJointSpaceTrajectory()

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

inlineprotected

Definition at line 203 of file cb_move_end_effector_trajectory.hpp.

  {
    // Use the CpJointSpaceTrajectoryPlanner component when present (preferred)
    CpJointSpaceTrajectoryPlanner * trajectoryPlanner = cpJointSpaceTrajectoryPlanner_;
 
    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 = cpMoveGroup_->moveGroupClientInterface->getCurrentState(100);
      auto groupname = cpMoveGroup_->moveGroupClientInterface->getName();
 
      RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting joint names");
      auto currentjointnames =
        currentState->getJointModelGroup(groupname)->getActiveJointModelNames();
 
      if (!tipLink_ || *tipLink_ == "")
      {
        tipLink_ = cpMoveGroup_->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_, cpJointSpaceTrajectoryPlanner_, cl_moveit2z::CbMoveit2zClientBehaviorBase::cpMoveGroup_, smacc2::ISmaccClientBehavior::currentState, endEffectorTrajectory_, smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), iksrv_, cl_moveit2z::INCORRECT_INITIAL_STATE, cl_moveit2z::JOINT_TRAJECTORY_DISCONTINUITY, cl_moveit2z::CpMoveGroupInterface::moveGroupClientInterface, cl_moveit2z::CpJointSpaceTrajectoryPlanner::planFromWaypoints(), 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:

◆ createMarkers()

virtual void cl_moveit2z::CbMoveEndEffectorTrajectory::createMarkers ( )

inlineprotectedvirtual

Reimplemented in cl_moveit2z::CbCircularPivotMotion.

Definition at line 527 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(), and onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ executeJointSpaceTrajectory()

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

inlineprotected

Definition at line 455 of file cb_move_end_effector_trajectory.hpp.

  {
    RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Executing joint trajectory");
 
    // Use the CpTrajectoryExecutor component when present (preferred)
    CpTrajectoryExecutor * trajectoryExecutor = cpTrajectoryExecutor_;
 
    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->cpMoveGroup_->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)
    {
      this->postMotionSuccess();
    }
    else
    {
      this->postMotionExecutionFailureEvents();
      this->postFailureEvent();
    }
  }

References cl_moveit2z::CbMoveit2zClientBehaviorBase::cpMoveGroup_, cl_moveit2z::CbMoveit2zClientBehaviorBase::cpTrajectoryExecutor_, cl_moveit2z::CpTrajectoryExecutor::execute(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), cl_moveit2z::CpMoveGroupInterface::moveGroupClientInterface, smacc2::SmaccAsyncClientBehavior::postFailureEvent(), postMotionExecutionFailureEvents, cl_moveit2z::CbMoveit2zClientBehaviorBase::postMotionSuccess(), and cl_moveit2z::ExecutionOptions::trajectoryName.

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ generateTrajectory()

virtual void cl_moveit2z::CbMoveEndEffectorTrajectory::generateTrajectory ( )

inlineprotectedvirtual

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

Definition at line 521 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:

◆ getCurrentEndEffectorPose()

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

inlineprotected

Definition at line 581 of file cb_move_end_effector_trajectory.hpp.

  {
    // components resolved in onStateOrthogonalAllocation
    CpTfListener * tfListener = cpTfListener_;
 
    try
    {
      if (!tipLink_ || *tipLink_ == "")
      {
        tipLink_ = this->cpMoveGroup_->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 cl_moveit2z::CbMoveit2zClientBehaviorBase::cpMoveGroup_, cpTfListener_, smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_moveit2z::CpTfListener::lookupTransform(), cl_moveit2z::CpMoveGroupInterface::moveGroupClientInterface, and tipLink_.

Referenced by cl_moveit2z::CbMoveCartesianRelative2::generateTrajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initializeROS()

void cl_moveit2z::CbMoveEndEffectorTrajectory::initializeROS ( )

inlineprivate

Definition at line 632 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 onStateOrthogonalAllocation().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onEntry()

virtual void cl_moveit2z::CbMoveEndEffectorTrajectory::onEntry ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

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

Definition at line 114 of file cb_move_end_effector_trajectory.hpp.

  {
    // components resolved in onStateOrthogonalAllocation
    CpTrajectoryVisualizer * trajectoryVisualizer = cpTrajectoryVisualizer_;
 
    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 = cpTrajectoryHistory_;
 
    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);
      }
 
      cpMoveGroup_->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(), cl_moveit2z::CbMoveit2zClientBehaviorBase::cpMoveGroup_, cl_moveit2z::CbMoveit2zClientBehaviorBase::cpTrajectoryHistory_, cpTrajectoryVisualizer_, createMarkers(), smacc2::introspection::demangleSymbol(), endEffectorTrajectory_, executeJointSpaceTrajectory(), generateTrajectory(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), cl_moveit2z::INCORRECT_INITIAL_STATE, cl_moveit2z::JOINT_TRAJECTORY_DISCONTINUITY, cl_moveit2z::CpMoveGroupInterface::postEventMotionExecutionFailed(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, cl_moveit2z::CpTrajectoryHistory::pushTrajectory(), cl_moveit2z::CpTrajectoryVisualizer::setTrajectory(), and cl_moveit2z::SUCCESS.

Here is the call graph for this function:

◆ onExit()

virtual void cl_moveit2z::CbMoveEndEffectorTrajectory::onExit ( )

inlineoverridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 200 of file cb_move_end_effector_trajectory.hpp.

200{}

◆ onStateOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >

void cl_moveit2z::CbMoveEndEffectorTrajectory::onStateOrthogonalAllocation ( )

inline

Definition at line 80 of file cb_move_end_effector_trajectory.hpp.

  {
    this->initializeROS();
 
    // optional components specific to this behavior family; the shared ones
    // (move group, motion planner, executor, history) come from the base chain
    this->requiresComponent(cpTrajectoryVisualizer_, smacc2::ComponentRequirement::SOFT);
    this->requiresComponent(cpJointSpaceTrajectoryPlanner_, smacc2::ComponentRequirement::SOFT);
    this->requiresComponent(cpTfListener_, smacc2::ComponentRequirement::SOFT);
 
    CbMoveit2zClientBehaviorBase::onStateOrthogonalAllocation<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");
      cpMoveGroup_->postEventMotionExecutionFailed();
      this->postEvent<EvMoveGroupMotionExecutionFailed<TSourceObject, TOrthogonal>>();
    };
  }