// calculation relative coordinate of point from current_pose frame
计算全局坐标系相对于机器人坐标系中的位置,即point相对于current_pose坐标系的位置
公式参考,R=L^2/(2*y),论文--一种纯追踪模型改进算法
{
tf::Transform inverse;
tf::poseMsgToTF(current_pose, inverse);
tf::Transform transform = inverse.inverse();
tf::Point p;
pointMsgToTF(point_msg, p);
tf::Point tf_p = transform * p;
geometry_msgs::Point tf_point_msg;
pointTFToMsg(tf_p, tf_point_msg);
return tf_point_msg;
}
例如:
geometry_msgs::Point point_msg;
point_msg.x=1;
point_msg.y=2;
point_msg.z=0;
geometry_msgs::Pose current_pose;
current_pose.position.x=4;
current_pose.position.y=7;
current_pose.position.z=0;
current_pose.orientation.x=0;
current_pose.orientation.y=0;
current_pose.orientation.z=0;
current_pose.orientation.w=1;
计算结果为
x: -3
y: -5i
z: 0
一、设置机器人的预瞄距离
pp_.setLookaheadDista