一、适配传感器 首先声明，踩了那么多坑后，明白一个道理，要想少出错，少踩坑，还是要遵循一切都参控官方文档的原则。不要怕官网文档繁杂又全是英语想省事，就一切都遵从csdn找到的别人整理的二手文档，包括我以下写的都只能当作参考，每个人硬件软件环境都不一样，具体步骤肯定有所差异，官网还是考虑最全的文档，英语有障碍可以下一个插件，推荐"沉浸式翻译"这个插件。https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/
设置传感器数据通信接口 根据官方提供的流程图，可以看到各个节点之间的数据通信：https://autowarefoundation.github.io/autoware-documentation/galactic/design/autoware-architecture/node-diagram/
雷达点云设置 Autoware输入的点云为 ==/sensing/lidar/top/outlier_filtered/pointcloud== 和 ==/sensing/lidar/concatenated/pointcloud==（frame_id均为base_link）。
编写雷达点云转换节点 打开终端并执行以下命令来创建工作空间和功能包：
1 2 3 mkdir -p /home/buaa/autoware_universe/autoware/src/sensor_driver/robosense_ws/src cd /home/buaa/autoware_universe/autoware/src/sensor_driver/robosense_ws/src ros2 pkg create --build-type ament_cmake lidar_transform
编写玩节点代码:
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 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 #include <rclcpp/rclcpp.hpp> #include <sensor_msgs/msg/point_cloud2.hpp> #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> #include <pcl_ros/transforms.hpp> #include <pcl_conversions/pcl_conversions.h> // 包含PCL转换头文件 class LidarTransformNode : public rclcpp::Node { public: // 构造函数 LidarTransformNode() : Node("points_raw_transform_node") { // 初始化坐标转换参数 // this->declare_parameter<double>("transform_x", 0.0); // this->get_parameter("transform_x", transform_x); transform_x = this->declare_parameter("transform_x", 0.0); transform_y = this->declare_parameter("transform_y", 0.0); transform_z = this->declare_parameter("transform_z", 0.0); transform_roll = this->declare_parameter("transform_roll", 0.0); transform_pitch = this->declare_parameter("transform_pitch", 0.0); transform_yaw = this->declare_parameter("transform_yaw", 0.0); RadiusOutlierFilter = this->declare_parameter("RadiusOutlierFilter", 1.0); std::cout << "robosense to base_link:" << std::endl << " transform_x: " << transform_x << std::endl << " transform_y: " << transform_y << std::endl << " transform_z: " << transform_z << std::endl << " transform_roll: " << transform_roll << std::endl << " transform_pitch:" << transform_pitch << std::endl << " transform_yaw: " << transform_yaw << std::endl; // Initialize translation transform_stamp.transform.translation.x = transform_x; transform_stamp.transform.translation.y = transform_y; transform_stamp.transform.translation.z = transform_z; // Initialize rotation (quaternion) tf2::Quaternion quaternion; quaternion.setRPY(transform_roll, transform_pitch, transform_yaw); transform_stamp.transform.rotation.x = quaternion.x(); transform_stamp.transform.rotation.y = quaternion.y(); transform_stamp.transform.rotation.z = quaternion.z(); transform_stamp.transform.rotation.w = quaternion.w(); // 创建发布者 publisher_1 = this->create_publisher<sensor_msgs::msg::PointCloud2>( "/sensing/lidar/top/outlier_filtered/pointcloud", 10); publisher_2 = this->create_publisher<sensor_msgs::msg::PointCloud2>( "/sensing/lidar/concatenated/pointcloud", 10); // 订阅原始点云消息 subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>( "/rslidar_points", 10, std::bind(&LidarTransformNode::pointCloudCallback, this, std::placeholders::_1)); } private: void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { // 过滤掉距离传感器较近的点 pcl::PointCloud<pcl::PointXYZI>::Ptr xyz_cloud(new pcl::PointCloud<pcl::PointXYZI>); pcl::PointCloud<pcl::PointXYZI>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZI>); pcl::fromROSMsg(*msg, *xyz_cloud); for (size_t i = 0; i < xyz_cloud->points.size(); ++i) { if (sqrt(xyz_cloud->points[i].x * xyz_cloud->points[i].x + xyz_cloud->points[i].y * xyz_cloud->points[i].y + xyz_cloud->points[i].z * xyz_cloud->points[i].z) >= RadiusOutlierFilter && !isnan(xyz_cloud->points[i].z)) { pcl_output->points.push_back(xyz_cloud->points.at(i)); } } sensor_msgs::msg::PointCloud2 output; pcl::toROSMsg(*pcl_output, output); output.header = msg->header; // 执行坐标转换 sensor_msgs::msg::PointCloud2 transformed_cloud; pcl_ros::transformPointCloud("base_link", transform_stamp, output, transformed_cloud); // 发布转换后的点云消息 publisher_1->publish(transformed_cloud); publisher_2->publish(transformed_cloud); } rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_; rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_1, publisher_2; double transform_x, transform_y, transform_z, transform_roll, transform_pitch, transform_yaw, RadiusOutlierFilter; // 添加 radius_outlier_filter 成员变量声明 geometry_msgs::msg::TransformStamped transform_stamp; }; int main(int argc, char **argv) { // 初始化节点 rclcpp::init(argc, argv); // 创建实例 auto node = std::make_shared<LidarTransformNode>(); rclcpp::spin(node); rclcpp::shutdown(); return 0; }
然后编译：
1 colcon build --packages-up-to lidar_transform
然后遵从Autoware.Universe官网教程 https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-and-sensor-model/creating-sensor-model/ 更改/home/buaa/autoware_universe/autoware/src/sensor_kit/sample_sensor_kit_launch/sample_sensor_kit_launch/launch 路径下的传感器启动文件
更改lidar.launch.xml 由于只用一个雷达，故不使用点云融合功能包，设置use_concat_filter为false,并发布lidar_transform点云tf转换功能包
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 <launch> <arg name="use_concat_filter" default="false"/> <arg name="vehicle_id" default="$(env VEHICLE_ID default)"/> <arg name="pointcloud_container_name" default="pointcloud_container"/> <arg name="rviz_config" default="$(find-pkg-share rslidar_sdk)/rviz/rviz2.rviz"/> <group> <push-ros-namespace namespace="lidar"/> <group> <push-ros-namespace namespace="top"/> <node pkg="rslidar_sdk" exec="rslidar_sdk_node" output="screen"> <!-- 添加其他必要的参数 --> <param name="queue_size" type="int" value="100"/> <param name="hardware_id" value="none"/> </node> <!-- <node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config)"/> --> </group> <include file="$(find-pkg-share lidar_transform)/launch/points_raw_transform.launch.xml"> <arg name="transform_x" value="0.0"/> <arg name="transform_y" value="0.0"/> <arg name="transform_z" value="1.0"/> <arg name="transform_roll" value="0.0"/> <arg name="transform_pitch" value="0.0"/> <arg name="transform_yaw" value="0.0"/> <arg name="RadiusOutlierFilter" value="1.0"/> </include> <!-- <group> <push-ros-namespace namespace="left"/> <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_VLP16.launch.xml"> <arg name="max_range" value="5.0"/> <arg name="sensor_frame" value="velodyne_left"/> <arg name="sensor_ip" value="192.168.1.202"/> <arg name="host_ip" value="$(var host_ip)"/> <arg name="data_port" value="2369"/> <arg name="scan_phase" value="180.0"/> <arg name="cloud_min_angle" value="300"/> <arg name="cloud_max_angle" value="60"/> <arg name="launch_driver" value="$(var launch_driver)"/> <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/> <arg name="container_name" value="pointcloud_container"/> </include> </group> <group> <push-ros-namespace namespace="right"/> <include file="$(find-pkg-share common_sensor_launch)/launch/velodyne_VLP16.launch.xml"> <arg name="max_range" value="5.0"/> <arg name="sensor_frame" value="velodyne_right"/> <arg name="sensor_ip" value="192.168.1.203"/> <arg name="host_ip" value="$(var host_ip)"/> <arg name="data_port" value="2370"/> <arg name="scan_phase" value="180.0"/> <arg name="cloud_min_angle" value="300"/> <arg name="cloud_max_angle" value="60"/> <arg name="launch_driver" value="$(var launch_driver)"/> <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)"/> <arg name="container_name" value="pointcloud_container"/> </include> </group> --> <include file="$(find-pkg-share sample_sensor_kit_launch)/launch/pointcloud_preprocessor.launch.py"> <arg name="base_frame" value="base_link"/> <arg name="use_intra_process" value="true"/> <arg name="use_multithread" value="true"/> <arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/> <arg name="use_concat_filter" value="$(var use_concat_filter)"/> </include> </group> </launch>
更改imu.launch.xml 添加雷达驱动，并发布雷达话题tamagawa/imu_link到base_link的tf转换
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 <launch> <arg name="launch_driver" default="true"/> <arg name="imu_raw_name" default="tamagawa/imu_raw"/> <!-- add --> <arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/> <group> <push-ros-namespace namespace="imu"/> <group> <push-ros-namespace namespace="fdilink"/> <node pkg="fdilink_ahrs" name="ahrs_driver_node" exec="ahrs_driver_node" if="$(var launch_driver)"> <param name="if_debug_" value="false"/> <param name="serial_port_" value="/dev/imu_usb"/> <param name="serial_baud_" value="921600"/> <param name="imu_topic" value="/sensing/imu/tamagawa/imu_raw"/> <param name="imu_frame_id_" value="tamagawa/imu_link"/> <param name="mag_pose_2d_topic" value="/mag_pose_2d"/> <param name="Magnetic_topic" value="/magnetic"/> <param name="Euler_angles_topic" value="/euler_angles"/> <param name="gps_topic" value="/gps/fix"/> <param name="twist_topic" value="/system_speed"/> <param name="NED_odom_topic" value="/imu_odometry"/> <param name="device_type_" value="1"/> </node> </group> <node pkg="tf2_ros" exec="static_transform_publisher" output="screen" args="0.5 0 0 0 0 0 base_link tamagawa/imu_link"/> <include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml"> <arg name="input_topic" value="$(var imu_raw_name)"/> <arg name="output_topic" value="imu_data"/> <arg name="param_file" value="$(var imu_corrector_param_file)"/> </include> <include file="$(find-pkg-share imu_corrector)/launch/gyro_bias_estimator.launch.xml"> <arg name="input_imu_raw" value="$(var imu_raw_name)"/> <arg name="input_twist" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/> <arg name="imu_corrector_param_file" value="$(var imu_corrector_param_file)"/> </include> </group> </launch>
禁用自带的tf转换 由于上述启动传感器时都发布了对应到baselink的tf转换，故禁用掉官方源码中发布的车体传感器tf转换部分。打开/home/buaa/autoware_universe/autoware/src/universe/autoware.universe/launch/tier4_vehicle_launch/launch 文件夹下的vehicle.launch.xml
启动传感器 单独启动传感器的命令为:
1 ros2 launch tier4_sensing_launch sensing.launch.xml
二、适配底盘 https://autowarefoundation.github.io/autoware-documentation/main/how-to-guides/integrating-autoware/creating-vehicle-interface-package/creating-vehicle-interface/
创建功能包 1 2 cd <your-autoware-dir>/src/vehicle/external ros2 pkg create --build-type ament_cmake my_vehicle_interface
从 Autoware 订阅控制命令主题的一些必要主题以控制车辆，具体话题描述参考官网，其核心是将接收到的autoware控制话题==control/command/control_cmd==提取出控制指令转化为底盘可以接受的==cmd_vel==话题格式，同时将一些必要的车辆状态主题发布。
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 85 86 import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist from autoware_auto_control_msgs.msg import AckermannControlCommand from autoware_auto_vehicle_msgs.msg import ControlModeReport, GearReport, SteeringReport, VelocityReport from hunter_msgs.msg import HunterStatus from tier4_vehicle_msgs.msg import BatteryStatus class VehicleInterface(Node): def __init__(self): super().__init__('vehicle_interface') # 订阅 Autoware 的控制指令 self.subscriber_control_cmd = self.create_subscription( AckermannControlCommand, 'control/command/control_cmd', self.control_cmd_callback, 10 ) # 发布小车的控制指令 self.publisher_cmd_vel = self.create_publisher(Twist, '/cmd_vel', 10) # 订阅小车的状态信息 self.subscriber_hunter_status = self.create_subscription( HunterStatus, '/hunter_status', self.hunter_status_callback, 10 ) # 发布转向状态 self.publisher_steering = self.create_publisher(SteeringReport, '/vehicle/status/steering_status', 10) # 发布速度状态 self.publisher_velocity = self.create_publisher(VelocityReport, '/vehicle/status/velocity_status', 10) # 发布电池状态 self.publisher_battery = self.create_publisher(BatteryStatus, '/vehicle/status/battery_charge', 10) def control_cmd_callback(self, msg): """ 处理从 Autoware 收到的控制命令，并将其转换为 Ackermann 结构小车的控制命令 """ twist = Twist() twist.linear.x = msg.longitudinal.speed # 线速度 twist.angular.z = msg.lateral.steering_tire_angle # 转向角度 self.publisher_cmd_vel.publish(twist) # 发布转换后的控制命令 def hunter_status_callback(self, msg): """ 处理从小车接收到的状态信息，并将其发布到 Autoware 的相关话题 """ # 发布转向报告 steering_report = SteeringReport() steering_report.stamp = self.get_clock().now().to_msg() steering_report.steering_tire_angle = msg.steering_angle self.publisher_steering.publish(steering_report) # 发布速度报告 velocity_report = VelocityReport() velocity_report.header.stamp = self.get_clock().now().to_msg() velocity_report.header.frame_id = "base_link" velocity_report.longitudinal_velocity = msg.linear_velocity # 纵向速度 velocity_report.lateral_velocity = 0.0 # 横向速度（假设为0） velocity_report.heading_rate = msg.steering_angle # 航向变化率 self.publisher_velocity.publish(velocity_report) # 发布电池状态报告 battery_status = BatteryStatus() battery_status.stamp = self.get_clock().now().to_msg() # 时间戳 battery_status.energy_level = msg.battery_voltage # 电池电压 self.publisher_battery.publish(battery_status) def main(args=None): rclpy.init(args=args) node = VehicleInterface() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()
创建launch文件my_vehicle_interface.launch.xml：
1 2 3 4 <launch> <node pkg="my_vehicle_interface" exec="vehicle_interface.py" name="vehicle_interface" output="screen"> </node> </launch>
修改CMakeLists.txt与package.xml后编译：
1 colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select my_vehicle_interface
如果运行遇到找不到py文件的问题，可能是没有赋予其运行权限
1 chmod +x scripts/vehicle_interface.py
然后找到/home/buaa/autoware_universe/autoware/src/universe/autoware.universe/launch/tier4_vehicle_launch/launch 路径下的vehicle.launch.xml 将vehicle_interface部分改成自己的：
1 2 3 4 5 6 7 8 9 10 <!-- vehicle interface --> <group if="$(var launch_vehicle_interface)"> <node pkg="my_vehicle_interface" exec="vehicle_interface.py" name="vehicle_interface" output="screen"> <param name="vehicle_id" value="$(var vehicle_id)"/> <param name="raw_vehicle_cmd_converter_param_path" value="$(var raw_vehicle_cmd_converter_param_path)"/> <param name="initial_engage_state" value="$(var initial_engage_state)"/> </node> </group> </launch>
但是为了调试便利，还是注释掉上述部分，选择单独启动vehicle interface：
1 ros2 launch my_vehicle_interface my_vehicle_interface.launch.xml
