Autoware.Universe适配实车教程
|
自动驾驶
|
62

1219 字
|
38 分钟

一、适配传感器

首先声明，踩了那么多坑后，明白一个道理，要想少出错，少踩坑，还是要遵循一切都参控官方文档的原则。不要怕官网文档繁杂又全是英语想省事，就一切都遵从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）。
/sensing/lidar/top/==outlier_filtered==/pointcloud用于==定位==,
/sensing/lidar/==concatenated==/pointcloud用==于感知==；

编写雷达点云转换节点

打开终端并执行以下命令来创建工作空间和功能包：

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

编写玩节点代码:

#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;
}

然后编译：

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转换功能包

<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转换

<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
注释掉vehicle description部分

启动传感器

单独启动传感器的命令为:

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/
根据官网教程一步一步编写vehicle_interface来创建底盘驱动与Autoware之间的联系：

创建功能包

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==话题格式，同时将一些必要的车辆状态主题发布。
以下为脚本内容

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：

<launch>
  <node pkg="my_vehicle_interface" exec="vehicle_interface.py" name="vehicle_interface" output="screen">
  </node>
</launch>

修改CMakeLists.txt与package.xml后编译：

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select my_vehicle_interface

如果运行遇到找不到py文件的问题，可能是没有赋予其运行权限
在 vehicle_interface.py 的开头添加执行指令

#!/usr/bin/env python3

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部分改成自己的：

  <!-- 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：

ros2 launch my_vehicle_interface  my_vehicle_interface.launch.xml
如果觉得本文对您有所帮助，可以支持下博主，一分也是缘😊
暂无评论

发送评论 编辑评论 


				

			

						

				

					

						

							

								
							

							
						

					

				

				

					

						

							

								
							

							
						

					

				

				

					

						

							

								
							

							
							
													

					

				

			

			
			

				

					
				

			

				

											

							
							
						

																

							
							
						

																

							
							
						

										
					
											
						

	

		
|´・ω・)ノ
ヾ(≧∇≦*)ゝ
(☆ω☆)
（╯‵□′）╯︵┴─┴
￣﹃￣
(/ω＼)
∠( ᐛ 」∠)＿
(๑•̀ㅁ•́ฅ)
→_→
୧(๑•̀⌄•́๑)૭
٩(ˊᗜˋ*)و
(ノ°ο°)ノ
(´இ皿இ｀)
⌇●﹏●⌇
(ฅ´ω`ฅ)
(╯°A°)╯︵○○○
φ(￣∇￣o)
ヾ(´･ ･｀｡)ノ"
( ง ᵒ̌皿ᵒ̌)ง⁼³₌₃
(ó﹏ò｡)
Σ(っ °Д °;)っ
( ,,´･ω･)ﾉ"(´っω･｀｡)
╮(╯▽╰)╭
o(*////▽////*)q
＞﹏＜
( ๑´•ω•) "(ㆆᴗㆆ)
😂
😀
😅
😊
🙂
🙃
😌
😍
😘
😜
😝
😏
😒
🙄
😳
😡
😔
😫
😱
😭
💩
👻
🙌
🖕
👍
👫
👬
👭
🌚
🌝
🙈
💊
😶
🙏
🍦
🍉
😣
Source: github.com/k4yt3x/flowerhd
	

	

		
颜文字
Emoji
小恐龙
花!
	


									

			

			
			
		

	






上一篇

                    

                    
			        推荐文章
		            

		            

							
Autoware安装 （踩坑指南）

							
							

							
Autoware.universe完整部署步骤（踩坑版）