odom_out_and_back.cpp

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

<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;  
}