11#include <tf/transform_datatypes.h>
38 const double METERS_PER_SAMPLE = 0.001;
40 float dist_meters = 0;
42 tf::vector3MsgToTF(
offset_, voffset);
44 float totallineardist = voffset.length();
46 int totalSamplesCount = totallineardist / METERS_PER_SAMPLE;
47 int steps = totallineardist / METERS_PER_SAMPLE;
49 float interpolation_factor_step = 1.0 / totalSamplesCount;
51 double secondsPerSample;
58 secondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_);
60 tf::StampedTransform currentEndEffectorTransform;
64 tf::Transform finalEndEffectorTransform = currentEndEffectorTransform;
65 finalEndEffectorTransform.setOrigin(finalEndEffectorTransform.getOrigin() + voffset);
67 float linc = totallineardist / steps;
68 float interpolation_factor = 0;
70 ros::Time startTime = ros::Time::now();
72 for (
float i = 0; i < steps; i++)
74 interpolation_factor += interpolation_factor_step;
77 tf::Vector3 vi = currentEndEffectorTransform.getOrigin().lerp(finalEndEffectorTransform.getOrigin(), interpolation_factor);
81 pose.setRotation(currentEndEffectorTransform.getRotation());
83 geometry_msgs::PoseStamped pointerPose;
84 tf::poseTFToMsg(pose, pointerPose.pose);
86 pointerPose.header.stamp = startTime + ros::Duration(i * secondsPerSample);
91 ROS_INFO_STREAM(
"[CbMoveEndEffectorRelative2] Target End efector Pose: " << this->
endEffectorTrajectory_.back());
geometry_msgs::Vector3 offset_
virtual void generateTrajectory() override
boost::optional< double > linearSpeed_m_s_
CbMoveCartesianRelative2(std::string referenceFrame, std::string tipLink)
CbMoveCartesianRelative2(std::string referenceFrame, std::string tipLink, geometry_msgs::Vector3 offset)
void getCurrentEndEffectorPose(std::string globalFrame, tf::StampedTransform ¤tEndEffectorTransform)
std::vector< geometry_msgs::PoseStamped > endEffectorTrajectory_