go_and_back.cpp

#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>

ros::Publisher cmdVelPub;

void shutdown(int sig)
{
    cmdVelPub.publish(geometry_msgs::Twist());
    ROS_INFO("odom_out_and_back.cpp ended!");
    ros::shutdown();
}

int main(int argc, char** argv)
{
    
    double rate = 20;
    ros::init(argc, argv, "go_and_back");
    std::string topic = "/cmd_vel";
    ros::NodeHandle node;
    cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);

    ros::Rate loopRate(rate);
    geometry_msgs::Twist speed;	   //控制信号载体 Twist
    signal(SIGINT, shutdown);
    ROS_INFO("odom_out_and_back.cpp start...");


    float linear_speed = 0.2;     //前进速度
    float goal_distance = 1.0;    //前进距离
    float angular_speed = 0.5;    //转弯角速度
    double goal_angle = M_PI;     //转的角度


    double angular_tolerance = 2.5*M_PI/180;    //角度允许误差，转换成弧度：deg*PI/180

    tf::TransformListener listener;
    tf::StampedTransform transform;

    std::string odom_frame = "/odom";    //里程计坐标系为frame_id
    std::string base_frame;
    try    //监听 /odom 到 /base_footprint 的坐标变换
    {
        listener.waitForTransform(odom_frame, "/base_footprint", ros::Time(), ros::Duration(2.0) );
        base_frame = "/base_footprint";            
        ROS_INFO("base_frame = /base_footprint");
    }
    catch (tf::TransformException & ex)
    {
        try
        {
            listener.waitForTransform(odom_frame, "/base_link", ros::Time(), ros::Duration(2.0) );
            base_frame = "/base_link";
            ROS_INFO("base_frame = /base_link");
        }
        catch (tf::TransformException ex)
        {
            ROS_INFO("Cannot find transform between /odom and /base_link or /base_footprint");
            cmdVelPub.publish(geometry_msgs::Twist());
            ros::shutdown();
        }
    }

    for(int i = 0;i < 2;i++)
    {
        ROS_INFO("go straight...!");
        speed.linear.x = linear_speed;    //设置线速度，正为前进，负为后退
        listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);    //获取初始位姿
        float x_start = transform.getOrigin().x();
        float y_start = transform.getOrigin().y();

        float distance = 0;    //记录走多远了
        while( (distance < goal_distance) && (ros::ok()) )
        {
            cmdVelPub.publish(speed);
            loopRate.sleep();
            listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
            float x = transform.getOrigin().x();
            float y = transform.getOrigin().y();
            distance = sqrt(pow((x - x_start), 2) +  pow((y - y_start), 2));
        }
        cmdVelPub.publish(geometry_msgs::Twist());
        ros::Duration(1).sleep(); 
        ROS_INFO("rotation...!");

        speed.linear.x = 0;
        speed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转

        double last_angle = fabs(tf::getYaw(transform.getRotation()));    //yaw是围绕Y轴旋转，也叫偏航角
        double turn_angle = 0;    //记录转了多少
        
        while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) )
        {
            cmdVelPub.publish(speed);
            loopRate.sleep();
            listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);
            double rotation = fabs(tf::getYaw(transform.getRotation()));    //获取yaw
            double delta_angle = fabs(rotation - last_angle);    //计算每一次转了多少
            turn_angle += delta_angle;
            last_angle = rotation;
        }
        speed.angular.z = 0;
        cmdVelPub.publish(geometry_msgs::Twist());
        ros::Duration(1).sleep();
    }
    cmdVelPub.publish(geometry_msgs::Twist());
    ros::Duration(1).sleep();
    ROS_INFO("odom_out_and_back.cpp ended!");
    ros::shutdown();
    return 0;
}