如何在ROS2中编写一个基于雷达数据的障碍物回避算法的示例代码？

在ROS2中编写一个基于雷达数据的障碍物回避算法，你需要遵循一些基本步骤。这里提供一个简化版的伪代码示例，实际项目中你会需要用到`rclcpp`库以及特定的雷达数据包：

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/twist.hpp>

class ObstacleAvoidanceNode : public rclcpp::Node
{
public:
    explicit ObstacleAvoidanceNode(const std::string & node_name)
        : Node(node_name), laser_sub_(*this, "radar_data", 10), twist_pub_(node_name + "/cmd_vel")
    {
        // 创建回调函数处理雷达数据
        laser_sub_.subscribe([this](const sensor_msgs::msg::LaserScan::SharedPtr msg) {
            handle_lidar_data(msg);
        });
    }

private:
    void handle_lidar_data(sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        // 这里处理雷达数据，例如找出最近的障碍物距离
        double min_distance = get_min_distance_from_msg(msg);

        if (min_distance >避障阈值) { // 如果障碍物足够远
            geometry_msgs::msg::Twist twist;
            twist.linear.x = 0.5; // 设置前进速度
            twist.angular.z = 0; // 直线行驶
            twist_pub_.publish(twist); // 发布控制命令
        } else {
            twist.linear.x = 0; // 避开障碍，停止或减速
            twist.angular.z = -0.5 * min_distance / 规划半径; // 根据最小距离调整旋转速度
            twist_pub_.publish(twist); // 发布控制命令
        }
    }

    // 其他辅助函数...
    double get_min_distance_from_msg(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
        double min_dist = msg->ranges[0];
        for (size_t i = 1; i < msg->ranges.size(); ++i) {
            if (msg->ranges[i] < min_dist && msg->ranges[i] != msg->invalid_range) {
                min_dist = msg->ranges[i];
            }
        }
        return min_dist;
    }

    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = ObstacleAvoidanceNode("obstacle_avoidance");
    rclcpp::spin_some(node);
    rclcpp::shutdown();
    return 0;
}

这个例子假设你已经有了从雷达数据包`radar_data`订阅来的`sensor_msgs/LaserScan`消息。代码首先创建了节点，设置了雷达数据的订阅者和控制命令的发布者。当收到雷达数据时，它会计算最小距离，如果超过预设的避障阈值，则直线行驶，否则则调整方向以避开障碍。

注意这只是一个基础框架，实际应用中还需要处理更多细节，比如数据融合、传感器噪声、并发处理和算法优化等。同时，你还需要对ROS2的生命周期管理和错误处理有深入理解。