ROS小车的司岚A1激光雷达跟随功能C++代码

以下是一个简单的ROS小车的司岚A1激光雷达跟随功能C++代码：

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

class LaserFollower {
public:
    LaserFollower() {
        scan_sub_ = nh_.subscribe<sensor_msgs::LaserScan>("scan", 10, &LaserFollower::scanCallback, this);
        cmd_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
    }

    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
        const float min_follow_distance = 0.5; // 最小跟随距离
        const float max_follow_distance = 1.0; // 最大跟随距离
        const float follow_angle = 0.0; // 跟随角度
        const float follow_speed = 0.2; // 跟随速度

        int num_ranges = scan->ranges.size();
        if (num_ranges == 0) {
            return;
        }

        float angle_min = scan->angle_min;
        float angle_increment = scan->angle_increment;
        float angle_max = angle_min + (num_ranges - 1) * angle_increment;

        float follow_angle_min = follow_angle - angle_increment;
        float follow_angle_max = follow_angle + angle_increment;

        int follow_index = -1;
        float follow_range = max_follow_distance;

        for (int i = 0; i < num_ranges; i++) {
            float angle = angle_min + i * angle_increment;
            float range = scan->ranges[i];

            if (angle < follow_angle_min || angle > follow_angle_max) {
                continue;
            }

            if (range < min_follow_distance || range > max_follow_distance) {
                continue;
            }

            if (range < follow_range) {
                follow_index = i;
                follow_range = range;
            }
        }

        if (follow_index != -1) {
            float follow_error = follow_range - min_follow_distance;
            float follow_speed_cmd = follow_speed * follow_error / (max_follow_distance - min_follow_distance);

            geometry_msgs::Twist cmd_vel;
            cmd_vel.linear.x = follow_speed_cmd;
            cmd_vel.angular.z = (follow_index - num_ranges / 2) * angle_increment * 2;
            cmd_pub_.publish(cmd_vel);
        }
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber scan_sub_;
    ros::Publisher cmd_pub_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "laser_follower");
    LaserFollower laser_follower;
    ros::spin();
    return 0;
}

这段代码使用ROS的LaserScan消息订阅激光雷达数据，并根据指定的跟随距离和角度，计算出跟随的目标点，并根据目标点和车辆当前位置的差距，输出相应的速度指令，以实现跟随功能。其中，linear.x和angular.z分别表示小车的线速度和角速度，可以根据实际情况进行调整。