【ROS1仿真】小车跟随--改善跟随的位置（路径规划、slam、定位、雷达）_ros 雷达跟随

最终效果

代码改进

这里给出上一篇博客的链接：https://blog.csdn.net/m0_71523511/article/details/135610191

使用上一篇的launch文件创建机器人时，ros会自动创建一个坐标系相对关系发布节点：/tf,通过观察此节点发布的信息，我发现可以通过监听里程计坐标系:/robot2/odom和小车底盘坐标系/robot2/base_footprint的坐标变换关系来实现想要的效果。直接上代码：

#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"nav_msgs/Odometry.h"
#include"geometry_msgs/PoseStamped.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"geometry_msgs/TransformStamped.h"
#include"geometry_msgs/Twist.h"


struct EulerAngles {
    double roll, pitch, yaw;
};

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"test03_control_turtle2");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("/robot1/move_base_simple/goal",100);

    ros::Rate rate(0.5);
    while (ros::ok())
    {
        try
        {
            geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("map","base_link2",ros::Time(0));
            geometry_msgs::TransformStamped son1toson3 = buffer.lookupTransform("robot2/odom","robot2/base_footprint",ros::Time(0));
            ROS_INFO("son1xiangduiyuson2 : pianyiliang(%.2f,%.2f)",son1toson2.transform.translation.x,son1toson2.transform.translation.y);
            geometry_msgs::PoseStamped odometry;
            odometry.header.frame_id = "robot1/odom";
            
            //下面这四行就是四元数转为偏航角的公式
            EulerAngles angles;

            double sinr_cosp = 2 * (son1toson3.transform.rotation.w * son1toson3.transform.rotation.z + son1toson3.transform.rotation.y * son1toson3.transform.rotation.x);
            double cosr_cosp = 1 - 2 * (son1toson3.transform.rotation.z * son1toson3.transform.rotation.z + son1toson3.transform.rotation.y * son1toson3.transform.rotation.y);
            angles.yaw = std::atan2(sinr_cosp, cosr_cosp);

            if((angles.yaw >0) && (angles.yaw < 1.5))
            {
                odometry.pose.position.x = (son1toson2.transform.translation.x-0.3);
                odometry.pose.position.y = (son1toson2.transform.translation.y-0.3);
            }
            else if((angles.yaw >1.5) && (angles.yaw < 3))
            {
                odometry.pose.position.x = (son1toson2.transform.translation.x+0.3);
                odometry.pose.position.y = (son1toson2.transform.translation.y-0.3);
            }
            else if((angles.yaw >-1.5) && (angles.yaw < 0))
            {
                odometry.pose.position.x = (son1toson2.transform.translation.x-0.3);
                odometry.pose.position.y = (son1toson2.transform.translation.y+0.3);
            }
            else if((angles.yaw >-3) && (angles.yaw < -1.5))
            {
                odometry.pose.position.x = (son1toson2.transform.translation.x+0.3);
                odometry.pose.position.y = (son1toson2.transform.translation.y+0.3);
            }
            else
            {
                odometry.pose.position.x = (son1toson2.transform.translation.x-0.3);
                odometry.pose.position.y = (son1toson2.transform.translation.y-0.3);
            }
            odometry.pose.orientation.w = 1.0;
            ROS_INFO("stop_here = (%.2f,%.2f)",odometry.pose.position.x,odometry.pose.position.y);

            ROS_INFO("siyuanshu = (%.10f)",angles.yaw);
            pub.publish(odometry);
        }
        catch(const std::exception& e)
        {
            ROS_ERROR("Exception: %s", e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84

通过代码不难看出，这里的实现就是将检测到的坐标系变换中的四元数转为了偏航角，通过偏航角就可以知道小车此时的朝向，通过测试最终写成四段if多条件判断语句，小车朝向不同，那么第二辆进行跟随的小车它的落点也就不同，这样就可以实现进行跟随的小车始终跟随在前面一辆下车的后面。
如果想要更精细的落点可以增加判断的语句。