#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

virtual void	update () override

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

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=false)

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

ClMoveit2z *	movegroupClient_ = nullptr

visualization_msgs::msg::MarkerArray	beahiorMarkers_

Private Member Functions
void	initializeROS ()

Private Attributes
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	markersPub_

std::atomic< bool >	markersInitialized_

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 51 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 )

Definition at line 35 of file cb_move_end_effector_trajectory.cpp.

: tipLink_(tipLink), markersInitialized_(false)
{
}

◆ CbMoveEndEffectorTrajectory() [2/2]

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

Definition at line 40 of file cb_move_end_effector_trajectory.cpp.

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

Member Function Documentation

◆ computeJointSpaceTrajectory()

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

protected

Definition at line 58 of file cb_move_end_effector_trajectory.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] getting current state.. waiting");
 
  // get current robot state
  auto currentState = movegroupClient_->moveGroupClientInterface->getCurrentState(100);
 
  // get the IK client
  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.attempts = 20;
 
    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;
 
    //pose.header.stamp = getNode()->now();
    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)
    {
      //if (rclcpp::spin_until_future_complete(getNode(), resfut) == rclcpp::FutureReturnCode::SUCCESS)
      //{
      auto & prevtrajpoint = trajectory.back();
      //jointPositions.clear();
 
      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 /*2.5 deg*/)
          {
            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)
        {
          // show a message and stop the trajectory generation && jointindex!= 7 || fabs(deltajoint) > 0.1 /*2.5 deg*/  && jointindex== 7
          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");
    }
  }
 
  // interpolate speeds?
 
  // interpolate accelerations?
 
  // get current robot state
  // fill plan message
  // computedMotionPlan.start_state_.joint_state.name = currentjointnames;
  // computedMotionPlan.start_state_.joint_state.position = trajectory.front();
  // computedMotionPlan.trajectory_.joint_trajectory.joint_names = currentjointnames;
 
  computedJointTrajectory.joint_trajectory.joint_names = currentjointnames;
  int i = 0;
  for (auto & p : trajectory)
  {
    if (
      i ==
      0)  // not copy the current state in the trajectory (used to solve discontinuity in other behaviors)
    {
      i++;
      continue;
    }
 
    trajectory_msgs::msg::JointTrajectoryPoint jp;
    jp.positions = p;
    jp.time_from_start = trajectoryTimeStamps[i];  //rclcpp::Duration(t);
    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 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::SUCCESS, and tipLink_.

Referenced by onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ createMarkers()

void cl_moveit2z::CbMoveEndEffectorTrajectory::createMarkers ( )

protectedvirtual

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

Definition at line 385 of file cb_move_end_effector_trajectory.cpp.

{
  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:

◆ executeJointSpaceTrajectory()

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

protected

Definition at line 268 of file cb_move_end_effector_trajectory.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Executing joint trajectory");
  // call execute
  auto executionResult =
    this->movegroupClient_->moveGroupClientInterface->execute(computedJointTrajectory);
 
  if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
  {
    RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] motion execution succeeded");
    movegroupClient_->postEventMotionExecutionSucceded();
    this->postSuccessEvent();
  }
  else
  {
    this->postMotionExecutionFailureEvents();
    this->postFailureEvent();
  }
}

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

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:

◆ generateTrajectory()

void cl_moveit2z::CbMoveEndEffectorTrajectory::generateTrajectory ( )

protectedpure virtual

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

Definition at line 431 of file cb_move_end_effector_trajectory.cpp.

{
  // 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
	)

protected

Definition at line 437 of file cb_move_end_effector_trajectory.cpp.

{
  tf2_ros::Buffer tfBuffer(getNode()->get_clock());
  tf2_ros::TransformListener tfListener(tfBuffer);
 
  try
  {
    if (!tipLink_ || *tipLink_ == "")
    {
      tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
    }
 
    tf2::fromMsg(
      tfBuffer.lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0), rclcpp::Duration(10s)),
      currentEndEffectorTransform);
 
    //tfListener.lookupTransform(globalFrame, *tipLink_, rclcpp::Time(0), currentEndEffectorTransform);
 
    // we define here the global frame as the pivot frame id
    // tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(10));
    // tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), globalBaseLink);
  }
  catch (const std::exception & e)
  {
    std::cerr << e.what() << std::endl;
  }
}

References smacc2::ISmaccClientBehavior::getNode(), movegroupClient_, cl_moveit2z::ClMoveit2z::moveGroupClientInterface, and tipLink_.

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initializeROS()

void cl_moveit2z::CbMoveEndEffectorTrajectory::initializeROS ( )

private

Definition at line 48 of file cb_move_end_effector_trajectory.cpp.

{
  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] initializing ros");
 
  auto nh = this->getNode();
  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:

◆ onEntry()

void cl_moveit2z::CbMoveEndEffectorTrajectory::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

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

Definition at line 289 of file cb_move_end_effector_trajectory.cpp.

{
  this->requiresClient(movegroupClient_);
 
  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;
  }
 
  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Creating markers.");
 
  this->createMarkers();
  markersInitialized_ = true;
  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
}  // namespace cl_moveit2z

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, markersInitialized_, movegroupClient_, cl_moveit2z::ClMoveit2z::postEventMotionExecutionFailed(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, cl_moveit2z::CpTrajectoryHistory::pushTrajectory(), smacc2::ISmaccClientBehavior::requiresClient(), smacc2::ISmaccClientBehavior::requiresComponent(), and cl_moveit2z::SUCCESS.

Here is the call graph for this function:

◆ onExit()

void cl_moveit2z::CbMoveEndEffectorTrajectory::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 368 of file cb_move_end_effector_trajectory.cpp.

{
  markersInitialized_ = false;
 
  if (autocleanmarkers)
  {
    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;
    }
 
    markersPub_->publish(beahiorMarkers_);
  }
}

References autocleanmarkers, beahiorMarkers_, smacc2::ISmaccClientBehavior::getNode(), m_mutex_, markersInitialized_, and markersPub_.

Here is the call graph for this function:

◆ onOrthogonalAllocation()

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::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_);
  }
}

References beahiorMarkers_, m_mutex_, markersInitialized_, and markersPub_.

Member Data Documentation

◆ allowInitialTrajectoryStateJointDiscontinuity_

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

Definition at line 60 of file cb_move_end_effector_trajectory.hpp.

Referenced by computeJointSpaceTrajectory().

◆ autocleanmarkers

bool cl_moveit2z::CbMoveEndEffectorTrajectory::autocleanmarkers = false

private

Definition at line 139 of file cb_move_end_effector_trajectory.hpp.

Referenced by onExit().

◆ beahiorMarkers_

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

protected

Definition at line 118 of file cb_move_end_effector_trajectory.hpp.

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

◆ endEffectorTrajectory_

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

protected

Definition at line 114 of file cb_move_end_effector_trajectory.hpp.

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

◆ group_

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

Definition at line 56 of file cb_move_end_effector_trajectory.hpp.

◆ iksrv_

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

private

Definition at line 130 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 132 of file cb_move_end_effector_trajectory.hpp.

Referenced by onExit(), and update().

◆ markersInitialized_

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

private

Definition at line 128 of file cb_move_end_effector_trajectory.hpp.

Referenced by onEntry(), onExit(), and update().

◆ markersPub_

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

private

Definition at line 126 of file cb_move_end_effector_trajectory.hpp.

Referenced by initializeROS(), onExit(), and update().

◆ movegroupClient_

ClMoveit2z* cl_moveit2z::CbMoveEndEffectorTrajectory::movegroupClient_ = nullptr

protected

Definition at line 116 of file cb_move_end_effector_trajectory.hpp.

Referenced by cl_moveit2z::CbCircularPivotMotion::computeCurrentEndEffectorPoseRelativeToPivot(), computeJointSpaceTrajectory(), executeJointSpaceTrajectory(), getCurrentEndEffectorPose(), 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 135 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 134 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 137 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 58 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/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

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CbMoveEndEffectorTrajectory() [1/2]

◆ CbMoveEndEffectorTrajectory() [2/2]

Member Function Documentation

◆ computeJointSpaceTrajectory()

◆ createMarkers()

◆ executeJointSpaceTrajectory()

◆ generateTrajectory()

◆ getCurrentEndEffectorPose()

◆ initializeROS()

◆ onEntry()

◆ onExit()

◆ onOrthogonalAllocation()

◆ update()

Member Data Documentation

◆ allowInitialTrajectoryStateJointDiscontinuity_

◆ autocleanmarkers

◆ beahiorMarkers_

◆ endEffectorTrajectory_

◆ group_

◆ iksrv_

◆ m_mutex_

◆ markersInitialized_

◆ markersPub_

◆ movegroupClient_

◆ postIncorrectInitialStateEvent

◆ postJointDiscontinuityEvent

◆ postMotionExecutionFailureEvents

◆ tipLink_