C++实现

以下是一个基于ROS和C++实现的简单激光雷达避障算法示例：

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

class ObstacleAvoidance
{
public:
    ObstacleAvoidance();

private:
    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg);

    ros::NodeHandle nh_;
    ros::Publisher cmd_vel_pub_;
    ros::Subscriber laser_sub_;
    bool obstacle_detected_;
};

ObstacleAvoidance::ObstacleAvoidance()
{
    cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    laser_sub_ = nh_.subscribe("/laser_scan", 1, &ObstacleAvoidance::laserCallback, this);
    obstacle_detected_ = false;
}

void ObstacleAvoidance::laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    // 获取激光雷达数据，判断是否存在障碍物
    float min_range = *std::min_element(msg->ranges.begin(), msg->ranges.end());
    if (min_range < 1.0)
    {
        obstacle_detected_ = true;
    }
    else
    {
        obstacle_detected_ = false;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "obstacle_avoidance");
    ObstacleAvoidance obstacle_avoidance;

    ros::Rate r(10);
    while (ros::ok())
    {
        if (obstacle_avoidance.obstacle_detected_)
        {
            // 存在障碍物，进行避障操作
            geometry_msgs::Twist cmd_vel;
            cmd_vel.angular.z = 0.5; // 向左转
            obstacle_avoidance.cmd_vel_pub_.publish(cmd_vel);
        }
        else
        {
            // 无障碍物，正常飞行
            geometry_msgs::Twist cmd_vel;
            cmd_vel.linear.x = 1.0; // 直线前进
            obstacle_avoidance.cmd_vel_pub_.publish(cmd_vel);
        }
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}

该代码与前面的Python代码实现类似，主要区别在于使用了C++语言和ROS的相关库函数。需要注意的是，该代码仅供参考，具体实现需要根据具体硬件设备和应用场景进行调整和优化。