#include <cb_move_end_effector_trajectory.h>

Inheritance diagram for cl_move_group_interface::CbMoveEndEffectorTrajectory:

Collaboration diagram for cl_move_group_interface::CbMoveEndEffectorTrajectory:

Public Member Functions
	CbMoveEndEffectorTrajectory (std::string tipLink="")

	CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::PoseStamped > &endEffectorTrajectory, std::string tipLink="")

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

virtual void	onEntry () override

virtual void	onExit () override

virtual void	update () override

Public Member Functions inherited from smacc::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)

Public Member Functions inherited from smacc::ISmaccClientBehavior
	ISmaccClientBehavior ()

virtual	~ISmaccClientBehavior ()

ISmaccStateMachine *	getStateMachine ()

std::string	getName () const

template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)

template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage)

ros::NodeHandle	getNode ()

Public Member Functions inherited from smacc::ISmaccUpdatable
	ISmaccUpdatable ()

	ISmaccUpdatable (ros::Duration duration)

void	executeUpdate ()

void	setUpdatePeriod (ros::Duration duration)

Public Attributes
boost::optional< std::string >	group_

boost::optional< std::string >	tipLink_

boost::optional< bool >	allowInitialTrajectoryStateJointDiscontinuity_

Protected Member Functions
ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::RobotTrajectory &computedJointTrajectory)

void	executeJointSpaceTrajectory (const moveit_msgs::RobotTrajectory &computedJointTrajectory)

virtual void	generateTrajectory ()

virtual void	createMarkers ()

void	getCurrentEndEffectorPose (std::string globalFrame, tf::StampedTransform &currentEndEffectorTransform)

Protected Member Functions inherited from smacc::SmaccAsyncClientBehavior
virtual void	executeOnEntry () override

virtual void	executeOnExit () override

void	postSuccessEvent ()

void	postFailureEvent ()

virtual void	dispose () override

Protected Member Functions inherited from smacc::ISmaccClientBehavior
virtual void	runtimeConfigure ()

virtual void	onEntry ()

virtual void	onExit ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

ISmaccState *	getCurrentState ()

virtual void	executeOnEntry ()

virtual void	executeOnExit ()

virtual void	dispose ()

virtual void	update ()=0

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

ClMoveGroup *	movegroupClient_ = nullptr

visualization_msgs::MarkerArray	beahiorMarkers_

Private Member Functions
void	initializeROS ()

Private Attributes
ros::Publisher	markersPub_

std::atomic< bool >	markersInitialized_

ros::ServiceClient	iksrv_

std::mutex	m_mutex_

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

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

std::function< void()>	postMotionExecutionFailureEvents

bool	autocleanmarkers = false

Detailed Description

Definition at line 35 of file cb_move_end_effector_trajectory.h.

Constructor & Destructor Documentation

◆ CbMoveEndEffectorTrajectory() [1/2]

cl_move_group_interface::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory ( std::string tipLink = "" )

Definition at line 16 of file cb_move_end_effector_trajectory.cpp.

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

References initializeROS().

Here is the call graph for this function:

◆ CbMoveEndEffectorTrajectory() [2/2]

cl_move_group_interface::CbMoveEndEffectorTrajectory::CbMoveEndEffectorTrajectory	(	const std::vector< geometry_msgs::PoseStamped > &	endEffectorTrajectory,
		std::string	tipLink = `""`
	)

Definition at line 22 of file cb_move_end_effector_trajectory.cpp.

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

References initializeROS().

Here is the call graph for this function:

Member Function Documentation

◆ computeJointSpaceTrajectory()

ComputeJointTrajectoryErrorCode cl_move_group_interface::CbMoveEndEffectorTrajectory::computeJointSpaceTrajectory ( moveit_msgs::RobotTrajectory & computedJointTrajectory )

protected

Definition at line 36 of file cb_move_end_effector_trajectory.cpp.

    {
        // get current robot state
        auto currentState = movegroupClient_->moveGroupClientInterface.getCurrentState();
 
        // get the IK client
        auto groupname = movegroupClient_->moveGroupClientInterface.getName();
        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<ros::Duration> trajectoryTimeStamps;
 
        trajectory.push_back(jointPositions);
        trajectoryTimeStamps.push_back(ros::Duration(0));
 
        auto &first = endEffectorTrajectory_.front();
        ros::Time referenceTime = first.header.stamp;
 
        std::vector<int> discontinuityIndexes;
 
        int ikAttempts = 4;
        for (int k = 0; k < this->endEffectorTrajectory_.size(); k++)
        {
            auto &pose = this->endEffectorTrajectory_[k];
            moveit_msgs::GetPositionIKRequest req;
            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;
 
            moveit_msgs::GetPositionIKResponse res;
 
            //pose.header.stamp = ros::Time::now();
            req.ik_request.pose_stamped = pose;
 
            ROS_DEBUG_STREAM("IK request: " << req);
            if (iksrv_.call(req, res))
            {
                auto &prevtrajpoint = trajectory.back();
                //jointPositions.clear();
 
                std::stringstream ss;
                for (int 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
                int 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 (int ji = 0; ji < jointPositions.size(); ji++)
                        {
                            ss << currentjointnames[ji] << ": " << jointPositions[ji] << std::endl;
                        }
 
                        for (int 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;
                        }
 
                        ROS_ERROR_STREAM(ss.str());
 
                        trajectory.push_back(jointPositions);
                        ros::Duration durationFromStart = pose.header.stamp - referenceTime;
                        trajectoryTimeStamps.push_back(durationFromStart);
 
                        continue;
                    }
                    else
                    {
                        trajectory.push_back(jointPositions);
                        ros::Duration durationFromStart = pose.header.stamp - referenceTime;
                        trajectoryTimeStamps.push_back(durationFromStart);
 
                        ROS_DEBUG_STREAM("IK solution: " << res.solution.joint_state);
                        ROS_DEBUG_STREAM("trajpoint: " << std::endl
                                                      << ss.str());
                    }
                }
            }
            else
            {
                ROS_ERROR("[CbMoveEndEffectorTrajectory] wrong IK call");
            }
 
            ROS_WARN_STREAM("-----");
        }
 
        // 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::JointTrajectoryPoint jp;
            jp.positions = p;
            jp.time_from_start = trajectoryTimeStamps[i]; //ros::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_, smacc::ISmaccClientBehavior::currentState, endEffectorTrajectory_, 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 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 337 of file cb_move_end_effector_trajectory.cpp.

    {
        tf::Transform localdirection;
        localdirection.setIdentity();
        localdirection.setOrigin(tf::Vector3(0.05, 0, 0));
        auto frameid = this->endEffectorTrajectory_.front().header.frame_id;
 
        for (auto &pose : this->endEffectorTrajectory_)
        {
            visualization_msgs::Marker marker;
            marker.header.frame_id = frameid;
            marker.header.stamp = ros::Time::now();
            marker.ns = "trajectory";
            marker.id = this->beahiorMarkers_.markers.size();
            marker.type = visualization_msgs::Marker::ARROW;
            marker.action = visualization_msgs::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::Point start, end;
            start.x = 0;
            start.y = 0;
            start.z = 0;
 
            tf::Transform basetransform;
            tf::poseMsgToTF(pose.pose, basetransform);
            tf::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_, and endEffectorTrajectory_.

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

Here is the caller graph for this function:

◆ executeJointSpaceTrajectory()

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

protected

Definition at line 230 of file cb_move_end_effector_trajectory.cpp.

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

References smacc::ISmaccClientBehavior::getName(), movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionSucceded(), smacc::SmaccAsyncClientBehavior::postFailureEvent(), postMotionExecutionFailureEvents, and smacc::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 ( )

protectedvirtual

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

Definition at line 383 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,
		tf::StampedTransform &	currentEndEffectorTransform
	)

protected

Definition at line 389 of file cb_move_end_effector_trajectory.cpp.

    {
        tf::TransformListener tfListener;
 
        try
        {
            if (!tipLink_ || *tipLink_ == "")
            {
                tipLink_ = this->movegroupClient_->moveGroupClientInterface.getEndEffectorLink();
            }
 
            tfListener.waitForTransform(globalFrame, *tipLink_, ros::Time(0), ros::Duration(10));
            tfListener.lookupTransform(globalFrame, *tipLink_, ros::Time(0), currentEndEffectorTransform);
 
            // we define here the global frame as the pivot frame id
            // tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, ros::Time(0), ros::Duration(10));
            // tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, ros::Time(0), globalBaseLink);
        }
        catch (const std::exception &e)
        {
            std::cerr << e.what() << '\n';
        }
    }

References 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 caller graph for this function:

◆ initializeROS()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::initializeROS ( )

private

Definition at line 29 of file cb_move_end_effector_trajectory.cpp.

    {
        ros::NodeHandle nh;
        markersPub_ = nh.advertise<visualization_msgs::MarkerArray>("trajectory_markers", 1);
        iksrv_ = nh.serviceClient<moveit_msgs::GetPositionIK>("/compute_ik");
    }

References iksrv_, and markersPub_.

Referenced by CbMoveEndEffectorTrajectory().

Here is the caller graph for this function:

◆ onEntry()

void cl_move_group_interface::CbMoveEndEffectorTrajectory::onEntry ( )

overridevirtual

Reimplemented from smacc::ISmaccClientBehavior.

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

Definition at line 249 of file cb_move_end_effector_trajectory.cpp.

    {
        this->requiresClient(movegroupClient_);
 
        this->generateTrajectory();
 
        if (this->endEffectorTrajectory_.size() == 0)
        {
            ROS_WARN_STREAM("[" << smacc::demangleSymbol(typeid(*this).name()) << "] No points in the trajectory. Skipping behavior.");
            return;
        }
 
        this->createMarkers();
        markersInitialized_ = true;
 
        moveit_msgs::RobotTrajectory computedTrajectory;
 
        auto errorcode = computeJointSpaceTrajectory(computedTrajectory);
 
        bool trajectoryGenerationSuccess = errorcode == ComputeJointTrajectoryErrorCode::SUCCESS;
 
        CpTrajectoryHistory *trajectoryHistory;
        this->requiresComponent(trajectoryHistory);
 
        if (!trajectoryGenerationSuccess)
        {
            ROS_INFO_STREAM("[" << this->getName() << "] Incorrect trajectory. Posting failure event.");
            if (trajectoryHistory != nullptr)
            {
                moveit_msgs::MoveItErrorCodes error;
                error.val = moveit_msgs::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::MoveItErrorCodes error;
                error.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
                trajectoryHistory->pushTrajectory(this->getName(), computedTrajectory, error);
            }
 
            this->executeJointSpaceTrajectory(computedTrajectory);
        }
 
        // handle finishing events
    } // namespace cl_move_group_interface

References computeJointSpaceTrajectory(), createMarkers(), smacc::introspection::demangleSymbol(), endEffectorTrajectory_, executeJointSpaceTrajectory(), generateTrajectory(), smacc::ISmaccClientBehavior::getName(), cl_move_group_interface::INCORRECT_INITIAL_STATE, cl_move_group_interface::JOINT_TRAJECTORY_DISCONTINUITY, markersInitialized_, movegroupClient_, cl_move_group_interface::ClMoveGroup::postEventMotionExecutionFailed(), smacc::SmaccAsyncClientBehavior::postFailureEvent(), postIncorrectInitialStateEvent, postJointDiscontinuityEvent, cl_move_group_interface::CpTrajectoryHistory::pushTrajectory(), smacc::ISmaccClientBehavior::requiresClient(), smacc::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 smacc::ISmaccClientBehavior.

Definition at line 320 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 = ros::Time::now();
                marker.action = visualization_msgs::Marker::DELETE;
            }
 
            markersPub_.publish(beahiorMarkers_);
        }
    }

References autocleanmarkers, beahiorMarkers_, m_mutex_, markersInitialized_, and markersPub_.

◆ onOrthogonalAllocation()

template<typename TOrthogonal , typename TSourceObject >

void cl_move_group_interface::CbMoveEndEffectorTrajectory::onOrthogonalAllocation ( )

inline

Definition at line 51 of file cb_move_end_effector_trajectory.h.

    {
      smacc::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]
      {
          ROS_INFO_STREAM("[" << this->getName() << "] motion execution failed");
          movegroupClient_->postEventMotionExecutionFailed();
          this->postEvent<EvMoveGroupMotionExecutionFailed<TSourceObject,TOrthogonal>>();
      };
    }

References smacc::ISmaccClientBehavior::getName(), movegroupClient_, smacc::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 smacc::ISmaccUpdatable.

Definition at line 311 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_

boost::optional<bool> cl_move_group_interface::CbMoveEndEffectorTrajectory::allowInitialTrajectoryStateJointDiscontinuity_

Definition at line 44 of file cb_move_end_effector_trajectory.h.

Referenced by computeJointSpaceTrajectory().

◆ autocleanmarkers

bool cl_move_group_interface::CbMoveEndEffectorTrajectory::autocleanmarkers = false

private

Definition at line 115 of file cb_move_end_effector_trajectory.h.

Referenced by onExit().

◆ beahiorMarkers_

visualization_msgs::MarkerArray cl_move_group_interface::CbMoveEndEffectorTrajectory::beahiorMarkers_

protected

Definition at line 95 of file cb_move_end_effector_trajectory.h.

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

◆ endEffectorTrajectory_

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

protected

Definition at line 91 of file cb_move_end_effector_trajectory.h.

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_

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

Definition at line 40 of file cb_move_end_effector_trajectory.h.

◆ iksrv_

ros::ServiceClient cl_move_group_interface::CbMoveEndEffectorTrajectory::iksrv_

private

Definition at line 106 of file cb_move_end_effector_trajectory.h.

Referenced by computeJointSpaceTrajectory(), and initializeROS().

◆ m_mutex_

std::mutex cl_move_group_interface::CbMoveEndEffectorTrajectory::m_mutex_

private

Definition at line 108 of file cb_move_end_effector_trajectory.h.

Referenced by onExit(), and update().

◆ markersInitialized_

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

private

Definition at line 104 of file cb_move_end_effector_trajectory.h.

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

◆ markersPub_

ros::Publisher cl_move_group_interface::CbMoveEndEffectorTrajectory::markersPub_

private

Definition at line 102 of file cb_move_end_effector_trajectory.h.

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

◆ movegroupClient_

ClMoveGroup* cl_move_group_interface::CbMoveEndEffectorTrajectory::movegroupClient_ = nullptr

protected

Definition at line 93 of file cb_move_end_effector_trajectory.h.

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::RobotTrajectory &)> cl_move_group_interface::CbMoveEndEffectorTrajectory::postIncorrectInitialStateEvent

private

Definition at line 111 of file cb_move_end_effector_trajectory.h.

Referenced by onEntry(), and onOrthogonalAllocation().

◆ postJointDiscontinuityEvent

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

private

Definition at line 110 of file cb_move_end_effector_trajectory.h.

Referenced by onEntry(), and onOrthogonalAllocation().

◆ postMotionExecutionFailureEvents

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

private

Definition at line 113 of file cb_move_end_effector_trajectory.h.

Referenced by executeJointSpaceTrajectory(), and onOrthogonalAllocation().

◆ tipLink_

boost::optional<std::string> cl_move_group_interface::CbMoveEndEffectorTrajectory::tipLink_

Definition at line 42 of file cb_move_end_effector_trajectory.h.

Referenced by 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:

smacc_client_library/move_group_interface_client/include/move_group_interface_client/client_behaviors/cb_move_end_effector_trajectory.h
smacc_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_