SDK: V开发中

自主导航系统教程

1. 产品介绍

该系统包含了建图、定位、导航和控制模块。机器人可在环境下构建点云地图，并依此实现全局定位和目标点的自主移动和避障。

2. 硬件介绍

2.1 性能参数

定位	描述
定位方式	激光SLAM
定位频率	100 Hz
定位精度	＜0.05 m

运动控制	描述
控制方式	自主导航（路径跟踪）
最大行驶速度	0.6 m/s
避障方式	绕障
避障频率	10-20 Hz

网络	描述
有线网络	支持
WiFi	支持

2.2 传感器配置

Galaxea R1配备了多种传感器，其中包括9个高清摄像头和2个激光雷达，使其能够全方位感知周围环境。

R1_FOV

传感器	描述
相机	头部：1 x 双目相机腕部：2 x 单目深度相机底盘：5 x 单目相机
激光雷达	1 x 360° (可选配两个激光雷达)

2.2.1 相机

规格	头部	腕部	底盘
类型	双目相机	单目深度相机	单目相机
数量	1	2	5
输出分辨率	1920 x 1080 @30FPS	1280 x 720 @30FPS	1920 x 1080 @30FPS
视场角	110°H x 70°V	87°H x 58°V x 95°D	118°H x 62°V
深度范围	0.3 m ~ 20 m	0.2 m ~ 3 m	\
工作温度	-10 °C ~ +45°C	0 ~ +85°C	-40 ~ +85°C
尺寸	175L x 30W x 32H mm	90L x 25W x 25H mm	30L x 30W x 23H mm
重量	164 g	75 g	<50 g

2.2.2 激光雷达

底盘配备360°激光雷达*，精度高且抗干扰能力强。

雷达	说明
数量	1 ~ 2
视场角	360°H x 59°V
激光波长	905 nm
检测范围	40 m @10% 反射率 70 m @80% 反射率
近距离盲区	0.1 m
数据端口	100 BASE-TX 以太网
IMU	内置IMU
工作温度范围	-20 ~ +55°C
尺寸	65L x 65W x 60H mm
重量	265 g

* 标配1个激光雷达，可根据用户需求选择激光雷达配置数量。

3. 软件介绍

请确保您的环境满足以下软件依赖要求。

硬件依赖：R1计算单元
操作系统依赖：Ubuntu 20.04 LTS
中间件依赖：ROS Noetic

请添加企业客服或邮件至support@galaxea-dynamics.com获取SDK文件。

如果需要首次部署导航系统，请按照以下步骤安装相关依赖：

sudo apt install libsdl1.2-dev
sudo apt install libsdl-image1.2-dev
sudo apt install libsdl2-image-dev
sudo apt install ros-noetic-grid-map-core
sudo apt install ros-noetic-grid-map-ros
sudo apt install libgoogle-glog-dev

4. 定位导航操作流程

地图构建是机器人自主导航的基础步骤，通过遥控机器人录制传感器数据（bag文件）并将数据回传。地图导出后，将地图部署在机器人本体，再设置目标位姿，实现定点导航。

4.1 构建地图

4.1.1 启动R1

通过SSH登录至R1 ECU。

ssh nvidia@robot_ip
# Enter the password  (default: nvidia)

运行以下指令，启动R1的相关节点。

cd work/galaxea/install/share/startup_config/script/
./ota_script.sh boot

4.1.2 录制数据包

运行以下指令，开始录制bag文件。

cd ~
rosbag record /hdas/imu_chassis /hdas/lidar_chassis_left /hdas/feedback_chassis

通过遥控器控制机器人在所需建图空间内移动，确保覆盖所有需要导航的区域。

遥控器操控机器人底盘方式请点击此处查阅。

当完成地图数据录制后，按下Ctrl + C结束录制。

注意：

请将机器人移动到准备建图的区域，再启动录制。
在录制开始时，机器人需保持静止状态，并持续5秒以上，以保证数据质量。
在录制数据时，应确保环境中没有动态目标（如移动的人员或物体），以避免干扰地图构建。请勿跟随机器人移动。

4.1.3 获取建图运行环境Docker

下载并安装Docker镜像。

自主导航系统为付费启用功能，如需深入了解及购买试用，请联系product@galaxea-dynamics.com或致电4008780980。购买后，我们将提供镜像文件。

推荐查看Docker安装教程进行安装。
执行以下命令在本地电脑上加载Docker文件。
```
sudo docker load -i galaxea-main-mapping-image.tar.gz
```
docker默认挂载在根目录，请预留20G以上存储空间。如需更改挂载路径，请参考: bash # 1. 在 /etc/docker/ 目录下创建一个新的 daemon.json 文件： sudo vim /etc/docker/daemon.json # 2. 在打开的文件中，添加以下内容，将 Docker 存储目录改为希望挂载的路径 { "data-root": "/path/to/target_dir" } # 3. 保存并退出。按下'shift'+ ':'，输入'w'+'q'，然后按下回车键。 # 4. 修改配置后，重启 Docker 服务使配置生效： sudo systemctl restart docker
下载默认标定文件。如已购买，我们将会以官方邮件的形式发送给您。

4.1.4 在环境中建图

在本地电脑终端中，运行以下命令将录制的bag文件从R1端拉取到本地。

scp nvidia@{robot_ip}:~/{xxx.bag} .
# [robot_ip]为R1的IP地址;
# [xxx.bag]为录制的bag文件名。

准备好bag文件和标定参数文件。

mkdir -p ~/mapping_data
cp /path/to/xxx.bag ~/mapping_data
cp /path/to/robot_calibration.json ~/mapping_data

启动Docker并开始建图。

sudo docker run --rm  -v ~/mapping_data:/mapping_data galaxea-mapping:v0.0.1 bash -c "./root/run_mapping_app.sh /mapping_data"

查看建图结果。

cd ~/mapping_data/map
# map.obj 文件即为地图结果.  可以使用meshlab打开查看点云. 
# sudo apt-get install meshlab
meshlab map.obj

4.1.5 导入地图和标定文件

执行以下命将地图和标定文件导入至R1。

ssh nvidia@{rorbot_ip} "mkdir -p ~/galaxea/calib ~/galaxea/maps"
scp -r ~/mapping_data/map/* nvidia@{robot_ip}:~/galaxea/maps/
scp -r ~/mapping_data/robot_calibration.json nvidia@{robot_ip}:~/galaxea/calib/

4.2 启动定位功能

启动定位功能时，确保机器人在已知地图中。

启动R1节点

在R1端执行以下命令，启动相关节点。

ssh nvidia@{robot_ip}
cd work/galaxea/install/share/startup_config/script/
./ota_script.sh boot

获取定位

将遥控器拨到底盘控制模式，操作机器人已知地图环境中2m范围内绕圈，确保机器人能够成功定位。

在R1端终端中，运行以下命令检查定位状态：
```
 source ~/work/galaxea/install/setup.bash
 rosrun tf tf_echo map body
```
若正常返回以下数据，则定位成功：
```
- Translation: [3.280, -0.743, 0.008]
- Rotation: in Quaternion [0.000, -0.004, -0.147, 0.989] # xx y z w
```

4.3 设置目标位姿

遥控机器人到目标点
启动定位成功后，将机器人遥控到客户想设定的目标点处，确保R1中心远离障碍物至少45cm。

记录位姿信息
每遥控至一个目标点，记录该位置的位姿信息：

- Translation: [3.280, -0.743, 0.008]
- Rotation: in Quaternion [0.000, -0.004, -0.147, 0.989] # x y z w

更新导航目标点下发脚本
重复上述过程，记录所有目标点的位姿信息后，将所有目标点位姿信息更新到导航目标点下发脚本中。脚本示例如下：

point_nav.py

#!/usr/bin/env python
import geometry_msg.msg
import rospy
from geometry_msgs.msg import PoseStamped
from system_manager_msg.msg import TaskRequest, TaskResponse
from geometry_msg.msg import PoseStamped
class NavigationEngager:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('navigation_engager', anonymous=True)
        # 初始化位置数组
        self.pose_array = self.create_pose_array()
        self.current_index = 0  # 当前目标位置索引

        # 设置Publisher和Subscriber
        self.publisher = rospy.Publisher('/system_manager/task/navigation_enage', TaskRequest, queue_size=10)
        self.subscriber = rospy.Subscriber('/system_manager/task/response', TaskResponse, self.response_callback)

        # 状态变量
        self.engage_pending = False  # 标志当前是否等待响应
        rospy.loginfo("Navigation engager node initialized.")

        # 开始发送第一个 engage
        rospy.sleep(1)  # 确保 Publisher 已初始化
        self.send_engage()

    def create_pose_array(self):
        """创建一个包含目标位置的 PoseStamped 数组"""
        pose_array = []
        pose = geometry_msg.msg.PoseStamped()
        pose.header.frame_id = "map"
        pose.pose.position.x = 3.280 
        pose.pose.position.y = -0.743
        pose.pose.position.z = 0.008
        pose.pose.orientation.x = 0.000 
        pose.pose.orientation.y = -0.004
        pose.pose.orientation.z =  -0.147
        pose.pose.orientation.w = 0.989
        pose_array.append(pose)

        # 新增目标点
        # pose = geometry_msg.msg.PoseStamped()
        # pose.header.frame_id = "map"
        # pose.pose.position.x = 5.761 
        # pose.pose.position.y = -0.146
        # pose.pose.position.z = 0.087
        # pose.pose.orientation.x = -0.008 
        # pose.pose.orientation.y = -0.001
        # pose.pose.orientation.z = 0.551
        # pose.pose.orientation.w = 0.835
        # pose_array.append(pose)

        return pose_array

    def send_engage(self):
        print("index  =", self.current_index)
        """发送 engage 消息"""

        if self.current_index >= len(self.pose_array):
            self.current_index = 0
        if not self.engage_pending and self.current_index < len(self.pose_array):
            msg = TaskRequest()
            # 根据需要设置消息的字段
            msg.task_type = 1
            msg.navigation_task.target_pose = self.pose_array[self.current_index]
            self.publisher.publish(msg)
            self.current_index += 1
            self.engage_pending = True  # 标记为等待响应

    def response_callback(self, msg):
        """处理响应消息"""
        if self.engage_pending:
            self.engage_pending = False  # 重置状态，允许发送新的 engage
            rospy.sleep(0.1)  # 控制发送节奏
            self.send_engage()  # 发送新的 engage

    def run(self):
        """运行节点"""
        rospy.spin()

if __name__ == '__main__':
    try:
        engager = NavigationEngager()
        engager.run()
    except rospy.ROSInterruptException:
        pass

可添加任意多个点，在python文件中，替换和新增python脚本中的如下区域

# 新添加目标点
# pose = geometry_msg.msg.PoseStamped()
# pose.header.frame_id = "map"
# pose.pose.position.x = 5.761 
# pose.pose.position.y = -0.146
# pose.pose.position.z = 0.087
# pose.pose.orientation.x = -0.008 
# pose.pose.orientation.y = -0.001
# pose.pose.orientation.z = 0.551
# pose.pose.orientation.w = 0.835
# pose_array.append(pose)

在目标点设置完成后，执行该脚本。将遥控器切换至Auto模式，可实现机器人设置目标点的循环自主导航。

source ~/work/galaxea/install/setup.bash
python3 point_nav.py

5. 软件接口

5.1 系统框图

R1_navigation_system_diagram_CN

5.2 驱动接口

R1提供了多种驱动接口，用于与硬件设备进行通信和控制。以下是主要的驱动接口及其说明：

5.2.1 底盘驱动接口

/motion_control/chassis_speed：用于控制机器人底盘的运动，包括速度控制、方向控制等。请前往R1软件介绍查看底盘驱动接口章节获取更多详细信息。

5.2.2 激光雷达接口

/hdas/lidar_chassis_left：激光雷达用于环境感知和距离测量，为机器人提供实时的环境信息。请前往R1软件介绍查看激光雷达接口章节获取更多详细信息。

5.2.3 IMU接口

/hdas/imu_chassis：IMU用于测量机器人的加速度、角速度等信息，为导航和姿态控制提供数据支持。请前往R1软件介绍查看IMU接口章节获取更多详细信息。

5.3 运控接口

R1机器人提供了多种运动控制接口，用于实现对机器人运动的精确控制。以下是主要的运控接口及其说明：

5.3.1 底盘控制接口

/motion_target/target_speed_chassis：用于控制机器人底盘的运动，包括速度控制、方向控制等。请前往R1软件手册的“底盘控制接口”章节获取更多详细信息。

5.4 定位接口

定位（Localization）接口是R1机器人实现自主导航和环境感知的核心组件。通过这些接口，机器人能够接收来自多种传感器的数据，如IMU（惯性测量单元）和激光雷达，从而实现精确的多传感器融合定位。这些接口确保机器人能够在复杂环境中准确地感知自身位置和姿态，为后续的路径规划和导航提供可靠的数据支持。本章节详细介绍了定位接口的各个话题，包括输入和输出数据的类型及其用途。

话题名称	I/O	描述	消息类型
/hdas/imu_chassis	Input	IMU数据，用于多传感器融合定位	sensor_msgs::Imu
/hdas/lidar_chassis_left	Input	多线激光雷达点云，用于定位	sensor_msgs/PointCloud2
/localization/localization_results	Output	SLAM定位状态	localization_msg/LocLocalization

5.5 导航话题接口

导航（Navigation）接口是R1机器人实现自主路径规划和运动控制的关键部分。这些接口允许机器人根据输入的传感器数据（如激光雷达点云和SLAM定位状态）进行全局和局部路径规划，并输出控制指令以驱动机器人底盘运动。导航接口不仅支持避障功能，还能够实时更新机器人的运动轨迹和任务状态，确保机器人能够高效、安全地完成导航任务。本章节详细介绍了导航接口的各个话题，包括输入和输出数据的类型及其用途。

话题名称	I/O	描述	消息类型
/hdas/lidar_chassis_left	Input	多线激光雷达点云，用于避障	sensor_msgs/PointCloud2
/localization/localization_results	Input	SLAM定位状态	localization_msg/LocLocalization
/system_manager/task/request	Input	导航任务接口	system_manager_msg/TaskRequest
/nav/local_path	Output	局部路径规划器规划的局部路径	sensor_msgs/PointCloud2
/nav/global_path	Output	全局路径规划器规划的全局路径	sensor_msgs/PointCloud2
/nav/robot_global_traj	Output	机器人行驶的全局轨迹	nav_msgs/Path
/nav/global_map	Output	导航全局代价地图，用于规划全局路径	nav_msgs/OccupancyGrid
/nav/local_map	Output	导航局部代价地图，用于规划局部路径	nav_msgs/OccupancyGrid
/nav/global_goal	Output	导航接收的目标点，用于可视化	geometry_msgs/PoseStamped
/motion_target/target_speed_chassis	Output	导航输出控制速度	geometry_msgs/Twist
/system_manager/task/response	Output	导航任务完成情况	system_manager_msg/TaskResponse

5.6 导航服务接口

5.6.1 开启导航服务

执行以下命令可开启导航服务。

service name: /nav_service  # 用于导航系统开关
Type: std_srvs/SetBool
示例:  rosservice call /nav_service "data: true"
      rosservice call /nav_service "data: false"

注意：导航服务在机器人启动时默认开启。

注意：确保R1机器人在导航服务开启时处于已建图的环境中，且周围环境与建图时保持一致，以确保定位和导航的准确性

5.6.2 关闭导航服务

执行以下命令可关闭导航服务。

service name: /nav_service  # 用于导航系统开关
Type: std_srvs/SetBool
示例:  rosservice call /nav_service "data: true"
      rosservice call /nav_service "data: false"

5.7 状态机

状态机（System Manager）是R1机器人系统的核心管理模块，负责协调和管理机器人的各项任务和服务。通过System Manager，用户可以触发导航任务、监控任务状态，并接收任务完成的反馈。本章节详细介绍了System Manager相关的ROS话题，包括输入和输出数据的类型及其用途。

话题名称	I/O	描述	消息类型
/system_manager/task/navigation_enage	Input	根据用户给出的target_pose触发system_manager构建，并向导航模块发送任务的topic	system_manager_msg/TaskRequest
/system_manager/task/response	Input	导航模块完成导航任务后，向system_manager回复成功或失败的topic	system_manager_msg/TaskResponse
/system_manager/task/request	Output	system_manager向导航模块发送任务的topic	system_manager_msg/TaskRequest

1. 产品介绍​

2. 硬件介绍​

2.1 性能参数​

2.2 传感器配置​

2.2.1 相机​

2.2.2 激光雷达​

3. 软件介绍​

4. 定位导航操作流程​

4.1 构建地图​

4.1.1 启动R1​

4.1.2 录制数据包​

4.1.3 获取建图运行环境Docker​

4.1.4 在环境中建图​

4.1.5 导入地图和标定文件​

4.2 启动定位功能​

4.3 设置目标位姿​

5. 软件接口​

5.1 系统框图​

5.2 驱动接口​

5.2.1 底盘驱动接口​

5.2.2 激光雷达接口​

5.2.3 IMU接口​

5.3 运控接口​

5.3.1 底盘控制接口​

5.4 定位接口​

5.5 导航话题接口​

5.6 导航服务接口​

5.6.1 开启导航服务​

5.6.2 关闭导航服务​

5.7 状态机​