点击打开链接原文在这，搬运以便学习记录，感谢原作者

<span style="font-family: Arial, Helvetica, sans-serif;">#include "ros/ros.h"  </span>

#include "std_msgs/String.h"//geometry_msgs
#include "geometry_msgs/Twist.h"//包含elocity space消息
#include <tf/transform_listener.h>
#include "math.h"
#include <sstream>
#include <iostream>
#include <rbx1_nav/CalibrateAngularConfig.h>
#include <rbx1_nav/CalibrateLinearConfig.h>
using namespace std;  int main(int argc, char **argv)
{  ros::init(argc,argv,"out_and_back");//指定节点“out_and_back”  ros::NodeHandle n;//创造一个节点句柄  ros::Publisher cmd_vel_pub=n.advertise<geometry_msgs::Twist>("/cmd_vel",1000);//将在/cmd_vel话题上发布一个geometry_msgs::Twist消息  int rate=20;//定义更新频率  ros::Rate loop_rate(rate);//更新频率20Hz，它会追踪记录自上一次调用Rate::sleep()后时间的流逝，并休眠直到一个频率周期的时间  //初始化操作  double linear_speed=0.2;//向前的线速度0.2m/s  double goal_distance=1.0;//行进记录1.0m  double angular_speed=1.0;//角度素1.0rad/s  double goal_angle=M_PI;  double angular_tolerance = 0.5*M_PI/180;//角度容忍度  tf::TransformListener listener;  geometry_msgs::Twist move_cmd;//定义消息对象  move_cmd.linear.x=move_cmd.linear.y=move_cmd.linear.z=0;  move_cmd.angular.x=move_cmd.angular.y=move_cmd.angular.z=0;  tf::StampedTransform transform;  try{  listener.waitForTransform("odom", "base_link", ros::Time(0), ros::Duration(1.0));  listener.lookupTransform("odom", "base_link",  ros::Time(0), transform);  }  catch (tf::TransformException &ex) {  ROS_ERROR("%s",ex.what());  ros::Duration(1.0).sleep();  }  double x_start = transform.getOrigin().x();  double y_start = transform.getOrigin().y();  double angle_start = acos(transform.getRotation().z())*2;  double distance = 0.0;  double angle = 0.0;  cout<<"angle_start: "<<angle_start<<endl;  while(ros::ok())//等待键盘ctrl+C操作则停止  {  tf::StampedTransform transform_;  try{  listener.waitForTransform("odom", "base_link", ros::Time(0), ros::Duration(1.0));  listener.lookupTransform("odom", "base_link",  ros::Time(0), transform_);  }  catch (tf::TransformException &ex) {  ROS_ERROR("%s",ex.what());  ros::Duration(1.0).sleep();  }  move_cmd.linear.x = linear_speed;  if(distance < goal_distance){  cmd_vel_pub.publish(move_cmd);  loop_rate.sleep();  distance = sqrt(pow((transform_.getOrigin().x() - x_start),2)+pow((transform_.getOrigin().y() - y_start),2));  cout<<"distance: "<<distance<<endl;  cout<<"odom.x: "<<transform_.getOrigin().x()<<endl;  cout<<"odom.y: "<<transform_.getOrigin().y()<<endl;  }  else {  if(angle + angular_tolerance < goal_angle){  move_cmd.linear.x=0.0;  move_cmd.angular.z=angular_speed;  cmd_vel_pub.publish(move_cmd);  loop_rate.sleep();  angle = abs(acos(transform_.getRotation().z())*2 - angle_start);  cout<<"angle: "<<angle<<endl;  }  else{  move_cmd.linear.x=0.0;  move_cmd.angular.z=0.0;  cmd_vel_pub.publish(move_cmd);  loop_rate.sleep();  cout<<"stop!"<<endl;  }  }  }  return 0;
}