#include <cb_move_end_effector_trajectory.hpp>

Inheritance diagram for cl_move_group_interface::CbMoveEndEffectorTrajectory:

Collaboration diagram for cl_move_group_interface::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)

virtual void	onEntry ()

virtual void	onExit ()

virtual void	executeOnEntry ()

virtual void	executeOnExit ()

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

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 void	dispose ()

virtual rclcpp::Node::SharedPtr	getNode () const

virtual rclcpp::Logger	getLogger () const

virtual void	update ()=0

Protected Attributes
std::vector< geometry_msgs::msg::PoseStamped >	endEffectorTrajectory_

ClMoveGroup *	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_move_group_interface::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_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory::computeJointSpaceTrajectory ( moveit_msgs::msg::RobotTrajectory & computedJointTrajectory )

protected

Definition at line 57 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_move_group_interface::INCORRECT_INITIAL_STATE, cl_move_group_interface::JOINT_TRAJECTORY_DISCONTINUITY, movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, cl_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory::createMarkers ( )

protectedvirtual

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

Definition at line 384 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_move_group_interface::CbCircularPivotMotion::createMarkers(), cl_move_group_interface::CbCircularPouringMotion::createMarkers(), and onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ executeJointSpaceTrajectory()

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

protected

Definition at line 267 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_move_group_interface::ClMoveGroup::moveGroupClientInterface, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionSucceded(), smacc2::SmaccAsyncClientBehavior::postFailureEvent(), postMotionExecutionFailureEvents, and smacc2::SmaccAsyncClientBehavior::postSuccessEvent().

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ generateTrajectory()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::generateTrajectory ( )

protectedpure virtual

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

Definition at line 430 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_move_group_interface::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose	(	std::string	globalFrame,
		tf2::Stamped< tf2::Transform > &	currentEndEffectorTransform
	)

protected

Definition at line 436 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_move_group_interface::ClMoveGroup::moveGroupClientInterface, and tipLink_.

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

Here is the call graph for this function:

Here is the caller graph for this function:

◆ initializeROS()

void cl_move_group_interface::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", 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_move_group_interface::CbMoveEndEffectorTrajectory::onEntry ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

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

Definition at line 288 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_move_group_interface

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

Here is the call graph for this function:

◆ onExit()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::onExit ( )

overridevirtual

Reimplemented from smacc2::ISmaccClientBehavior.

Definition at line 367 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_move_group_interface::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_move_group_interface::ClMoveGroup::postEventMotionExecutionFailed(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, and postMotionExecutionFailureEvents.

Here is the call graph for this function:

◆ update()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::update ( )

overridevirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 358 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_move_group_interface::CbMoveEndEffectorTrajectory::allowInitialTrajectoryStateJointDiscontinuity_

Definition at line 60 of file cb_move_end_effector_trajectory.hpp.

Referenced by computeJointSpaceTrajectory().

◆ autocleanmarkers

bool cl_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory::beahiorMarkers_

protected

Definition at line 118 of file cb_move_end_effector_trajectory.hpp.

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

◆ endEffectorTrajectory_

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

protected

Definition at line 114 of file cb_move_end_effector_trajectory.hpp.

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

◆ group_

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

Definition at line 56 of file cb_move_end_effector_trajectory.hpp.

◆ iksrv_

rclcpp::Client<moveit_msgs::srv::GetPositionIK>::SharedPtr cl_move_group_interface::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_move_group_interface::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_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory::markersPub_

private

Definition at line 126 of file cb_move_end_effector_trajectory.hpp.

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

◆ movegroupClient_

ClMoveGroup* cl_move_group_interface::CbMoveEndEffectorTrajectory::movegroupClient_ = nullptr

protected

Definition at line 116 of file cb_move_end_effector_trajectory.hpp.

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

◆ postIncorrectInitialStateEvent

std::function<void(moveit_msgs::msg::RobotTrajectory &)> cl_move_group_interface::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_move_group_interface::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_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory::tipLink_

Definition at line 58 of file cb_move_end_effector_trajectory.hpp.

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

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

smacc2_client_library/move_group_interface_client/include/move_group_interface_client/client_behaviors/cb_move_end_effector_trajectory.hpp
smacc2_client_library/move_group_interface_client/src/move_group_interface_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_