使用myAGV、Jetson Nano主板和3D摄像头，实现了RTAB-Map的三维建图功能！

引言
在现代机器人技术中，高精度的环境感知与建图是实现自主导航的关键。本文将展示如何使用myAGV Jetson Nano移动平台搭载Jetson Nano BO1主板，结合RTAB-Map和3D相机，实现更加立体和细致的环境建图。myAGV Jetson Nano具备SLAM雷达导航功能，Jetson Nano提供了强大的计算能力，适合处理复杂的SLAM任务。通过引入3D摄像头，我们能够将摄像头采集的深度信息融入到地图中，使其不仅具有平面数据，还包含了丰富的立体信息。在本文中，我们将详细介绍这一过程中使用的技术，以及解决实施中遇到的问题。

背景与需求分析
在机器人自主导航中，精确的环境感知和地图构建至关重要。传统的二维SLAM技术虽然能够实现实时定位和建图，但在复杂的三维空间中，往往无法充分描述环境的立体结构。

为了解决这一问题，我们选择了myAGV Jetson Nano，该产品具备高性能的SLAM雷达导航能力和强大的计算处理能力，非常适合复杂环境下的自主任务。然而，二维SLAM在描述立体空间时仍显不足。因此，我们引入了3D摄像头，通过捕捉环境的深度信息，生成更加立体和细致的三维地图，提升机器人的环境感知能力。

为了实现这一目标，我们采用了RTAB-Map作为建图工具，它能够处理RGB-D数据并支持实时的三维建图与定位。通过将RTAB-Map与3D摄像头结合在这款产品上使用，我们希望在复杂环境中实现高精度的三维SLAM建图，满足实际应用的需求。

产品
myAGV Jetson Nano
myAGV Jetson Nano 2023采用NVIDIA® Jetson Nano B01 4GB核心主板，搭配大象机器人专为机器人定制的Ubuntu Mate 20.04 操作系统，流畅易用；myAGV 2023具备2D建图与导航、3D建图与导航、图形化编程、可视化软件、ROS仿真、手柄键盘控制等多钟功能，是科研教育、个人创客的首选。

Astra Pro2
Astra Pro2深度相机是基于3D 结构光成像技术获取物体的深度图像，同时利用彩色相机采集物体的彩色图像，适用于0.6m-6m 距离进行3D物品和空间扫描的智能产品，可实现测量距离内的物体深度数据测量。作为Astra系列的迭代升级产品，Astra Pro 2配置MX6000自研深度感知芯片，最高支持1280x1024深度图像，自带多分辨率下深度图像与彩色图像空间对齐功能，可广泛应用于机器人避障、低精度3D测量、体感交互等场景。具备RGB-D功能，能够捕捉彩色图像和深度信息，用于生成三维地图。

所有所需要的依赖的功能包，在myAGV所安装的额ubuntu 20.04系统中已经按照好，我们可以直接使用ROS当中的rtabmap以及astra pro2的功能包。

rtabmap 实现
myagv 已经打包好了一些基本的功能我们直接使用，在这个过程中分析一下他们的功能。

启动程序
首先得启动里程记和雷达。

roslaunch myagv_odometry myagv_active.launch

myagv_active.launch启动文件负责初始化和启动与机器人运动估计和传感器数据获取相关的核心组件。

< launch >

   < node pkg="myagv_odometry" type="myagv_odometry_node" name="myagv_odometry_node" output="screen" / >
   < param name = "robot_description" textfile = "$(find myagv_urdf)/urdf/myAGV.urdf"/ > 
   
   < node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" / >
   < node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" / >
   < !--node name="base2lodom_frame" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 1 /odom /base_footprint 50"/-- >
   < node name="base2camera_link" pkg="tf" type="static_transform_publisher" args="0.13 0 0.131 0 0 0 /base_footprint /camera_link 50"/ >
   < node name="base2imu_link" pkg="tf" type="static_transform_publisher" args="0 0 0 0 3.14159 3.14159 /base_footprint /imu 50"/ >
       
   < node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen" >
     < param name="output_frame" value="odom"/ >
     < param name="base_footprint_frame" value="base_footprint"/ >
     < param name="freq" value="30.0"/ >
     < param name="sensor_timeout" value="2.0"/ >
     < param name="odom_used" value="true"/ >
     < param name="odom_data" value="odom"/ >
     < param name="imu_used" value="true"/ >
     < param name="vo_used" value="false"/ >
< !--      < remap from="imu_data" to="imu" / >-->
   < /node >
 
   < include file="$(find ydlidar_ros_driver)/launch/X2.launch" / >
< /launch >

myagv_odometry_node：启动里程计节点，用于计算机器人在环境中的位置和姿态。

robot_description参数：加载机器人的URDF文件（统一机器人描述格式），描述机器人的物理结构。

joint_state_publisher和robot_state_publisher：发布机器人的关节状态和机器人的整体状态信息。

static_transform_publisher：定义固定的坐标变换，用于将机器人基座和传感器（如相机、IMU）之间的相对位置和姿态联系起来。

robot_pose_ekf：使用扩展卡尔曼滤波器（EKF）融合里程计、IMU等传感器数据，提供更精确的机器人位姿估计。

ydlidar_ros_driver：启动激光雷达（LiDAR）驱动节点，用于获取环境的激光扫描数据。

然后是启动astra pro2 深度相机

roslaunch orbbec_camera astra_pro2.launch

它设置了必要的ROS节点来处理相机的RGB-D数据流，包括初始化相机、设置图像和深度处理的各种参数，并将相机数据发布到ROS主题，以供其他节点（如SLAM或物体检测）使用。

里面已经默认设置好了一些必要的参数，如果需要修改的话请按照官方文档提供的sdk进行修改：3D视觉AI开放平台

例如一下参数：

/camera/color/camera_info : 彩色相机信息（CameraInfo）话题。
/camera/color/image_raw: 彩色数据流图像话题。
/camera/depth/camera_info: 深度数据流图像话题。
/camera/depth/image_raw: 红外数据流图像话题。
/camera/depth/points : 点云话题，仅当 enable_point_cloud 为 true 时才可用`.
/camera/depth_registered/points: 彩色点云话题，仅当 enable_colored_point_cloud 为 true 时才可用。
/camera/ir/camera_info: 红外相机信息（CameraInfo）话题。
/camera/ir/image_raw: 红外数据流图像话题。

紧接着启动rtabmap启动文件建图就可以开始建图了。

roslaunch myagv_navigation rtabmap_mapping.launch

< launch >
 < group ns="rtabmap" >
 
   < !-- Choose visualization -- >
 < arg name="rtabmap_viz"             default="true" / >

   < !-- Use RGBD synchronization -- >
   < !-- Here is a general example using a standalone nodelet, 
        but it is recommended to attach this nodelet to nodelet 
        manager of the camera to avoid topic serialization -- >
   < node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_sync/rgbd_sync" output="screen" >
     < remap from="rgb/image"       to="/camera/color/image_raw"/ >
     < remap from="depth/image"     to="/camera/depth/image_raw"/ >
     < remap from="rgb/camera_info" to="/camera/color/camera_info"/ >
     < remap from="rgbd_image"      to="rgbd_image"/ > < !-- output -- >
     
     < !-- Should be true for not synchronized camera topics 
          (e.g., false for kinectv2, zed, realsense, true for xtion, kinect360)-- >
     < param name="approx_sync"       value="true"/ > 
   < /node >

   < node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="--delete_db_on_start" >
         < param name="frame_id" type="string" value="base_footprint"/ >
         < param name="subscribe_rgb" type="bool" value="false"/ >
         < param name="subscribe_depth" type="bool" value="false"/ >
         < param name="subscribe_rgbd" type="bool" value="true"/ >
         < param name="subscribe_scan" type="bool" value="true"/ >

         < remap from="odom" to="/odom"/ >
         < remap from="scan" to="/scan"/ >
         < remap from="rgbd_image" to="rgbd_image"/ >

         < param name="queue_size" type="int" value="100"/ >

         < !-- RTAB-Map's parameters -- >
         < param name="RGBD/NeighborLinkRefining" type="string" value="true"/ >
         < param name="RGBD/ProximityBySpace"     type="string" value="true"/ >
         < param name="RGBD/AngularUpdate"        type="string" value="0.01"/ >
         < param name="RGBD/LinearUpdate"         type="string" value="0.01"/ >
         < param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/ >
         < param name="Grid/FromDepth"            type="string" value="false"/ > < !-- occupancy grid from lidar -- >
         < param name="Reg/Force3DoF"             type="string" value="true"/ >
         < param name="Reg/Strategy"              type="string" value="1"/ > < !-- 1=ICP -- >
         
         < !-- ICP parameters -- >
         < param name="Icp/VoxelSize"                 type="string" value="0.05"/ >
         < param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/ >
   < /node >
 < node pkg="rviz" type="rviz" name="rviz" args="-d $(find myagv_navigation)/rviz/rtabmap.rviz" output="screen"/ >
     
 < node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
   args="0.0 0.0 0.2 3.1415 0.0 0 /base_footprint /laser_frame 40" / >
 < /group >
< /launch >

启动组（group）：

● 将RTAB-Map相关的节点分组，使得它们共享相同的命名空间（rtabmap），方便管理和数据处理。

RGB-D同步节点（rgbd_sync）：

● 启动一个用于同步RGB图像和深度图像的节点，将摄像头的原始图像数据转换成RTAB-Map可以处理的格式。

RTAB-Map SLAM节点：

● 启动RTAB-Map SLAM算法，配置SLAM相关的参数，如订阅的传感器数据、队列大小、以及优化和ICP（迭代最近点算法）相关的参数。该节点负责实时处理传感器数据，生成环境地图并估计机器人的位姿。

RViz可视化：

● 启动RViz，用于实时可视化RTAB-Map生成的地图和机器人的位姿。

静态变换发布（tf）：

● 定义并发布激光雷达和机器人体框架之间的固定坐标变换，确保SLAM算法能够正确地将传感器数据对齐到相同的坐标系中。

接下来看看效果如何。

效果也不是特别特别的流畅

问题
实现是实现了基本的建图，但是从图片中看，即使是Jetson Nano的主板，在性能上还是有所欠缺，在建图的时候还是会有所卡顿。

所以有没有办法，来解决这个问题呢，能够保证相对完整的建图。

答案是当然有。

那就是ROS的多机通讯！

解决办法
ROS多机通讯
ROS多机通信指的是在多个计算设备之间通过ROS网络共享信息和任务的能力。这在处理复杂机器人应用时特别有用，比如当单个设备（如Jetson Nano）无法处理所有计算任务时，可以通过网络将部分任务分担给其他设备（如一台性能更强的PC）。

简而言之就是，Jetson Nano主板负责处理slam的一些计算，用一台性能强的PC来处理深度相机得到图形处理。

1. 配置网络

● 确保PC和Jetson Nano在同一个网络下，并可以互相通信。

● 设置每台设备的ROS环境变量，主要是ROS_MASTER_URI和ROS_IP或ROS_HOSTNAME。

PC:
export ROS_MASTER_URI=http://< PC_IP >:11311
export ROS_IP=< 192.168.1.100 >

Jetson
export ROS_MASTER_URI=http://< PC_IP >:11311
export ROS_IP=< 192.168.1.121 >

2. 启动核心节点

在PC端上启动核心节点，这样Jetson Nano可以通过多机通信与PC的ROS核心进行通信

3. 节点分布

● PC端（SLAM建图）：在PC上运行RTAB-Map节点，订阅来自Jetson Nano的传感器数据，并进行SLAM建图。

● Jetson Nano端（图形处理）：Jetson Nano运行传感器驱动节点（如深度相机），并发布图像和深度数据。

● 同时，可以运行图形处理节点，处理订阅的SLAM结果或地图数据。

4. 数据传输

通过ROS topics在PC和Jetson Nano之间传递数据。例如，Jetson Nano可以将相机的RGB-D数据发布到/camera/color/image_raw和/camera/depth/image_raw等主题，PC端的RTAB-Map节点则订阅这些主题。

来看建图的效果，是不是比之前的流畅了许多。

总结
在本次技术案例中，我们成功地使用这款产品结合Jetson Nano主板和3D摄像头，实现了RTAB-Map的三维建图功能。

然而，在实施过程中，我们遇到了性能瓶颈的问题，特别是在Jetson Nano主板上运行复杂的SLAM算法时，计算负荷较重，导致实时性和稳定性受到一定影响。为了解决这一问题，我们引入了多机通讯技术，将部分计算任务分配到另一台计算机上进行处理，从而减轻了Jetson Nano的负担。通过这一优化方案，不仅提高了系统的整体性能，还确保了SLAM建图过程的流畅和高效。

审核编辑 黄宇