2024年睿抗机器人大赛智能侦查省赛（预选赛）总结下篇

引言

在上篇和中篇里，我们回顾了省赛任务书前三个任务解决的思路和解决的demo，这一篇我们主要针对余下的最后一部分，也是省赛任务中分值占比很高的自主导航部分展开分析。

任务5：ROS建图与导航

任务描述

利用仿真环境，使用gmapping建图，利用使用说明中保存地图的方法保存地图。由于官方提供的虚拟机已经搭建好大部分的环境，因此我们可以直接开始操作。

完成步骤

2D建图

首先打开终端，启动ROS核心。

bash 复制代码

roscore

新建一个终端启动仿真环境Gazebo，加载仿真世界，启动相关的传感器和机器人模型。具体代码如下：

bash 复制代码

roslaunch turtlebot3_gazebo turtlebot3_world.launch

再打开键盘控制程序：

bash 复制代码

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

之后启动gmapping节点，在终端中输入以下指令：

bash 复制代码

roslaunch turtlebot3_slam turtlebot3_slam.launch

启动gmapping节点，将用于根据传入的激光扫描数据（通常来自激光雷达）和里程计数据（来自机器人的运动模型）进行建图；gmapping节点将会订阅/scan话题（激光扫描数据）和/odom话题（里程计数据）；gmapping根据订阅到的数据，实时进行SLAM（同时定位与地图构建）计算，生成地图。这一过程在可视化工具rviz里均可看到。

最后保存地图文件，完成slam建图之后可以新建终端并在终端输入以下指令保存地图：

bash 复制代码

rosrun map_server map_saver -f ~/catkin_ws/src/turtlebot3/turtlebot3_navigation/maps/map     
#将地图文件命名为map，存储位置为/home/catkin_ws/src/turtlebot3/turtlebot3_navigation/maps

这一段指令是使用ros地图服务器进行保存，实际上我们需要自行编写launch文件实现这一个功能。

总结一下执行流程如下：

bash 复制代码

在第一个终端输入：
roscore
在第二个终端输入：
roslaunch turtlebot3_gazebo turtlebot3_world.launch                                          
在第三个终端输入：
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch                                     
在第四个终端输入：
roslaunch turtlebot3_slam turtlebot3_slam.launch                                             建图完成之后，在第五个终端输入：
rosrun map_server map_saver -f ~/catkin_ws/src/turtlebot3/turtlebot3_navigation/maps/map

修改gazebo仿真环境

通过上一步我们已经建立好了地图，接下来就是修改gazebo仿真环境，保存修改之后的地图。

首先需要找到Gazebo仿真环境的描述文件（通常是.world文件），在这个文件中添加两个物体，一个圆柱体和一个球体，在原有world文件中添加以下代码实现：

bash 复制代码

<!-- 假设长方形空间的中心为(0, 0, 0)，长x=10, 宽y=5, 高z=2 -->
 <!-- 添加圆柱体 --> 
<model name="cylinder_model"> <pose>5 2.5 1 0 0 0</pose> 
<!-- 放在对角线的一端，x和y坐标根据长方形空间调整 --> 
<static>true</static> <!-- 模型是静态的，设置为true --> 
<link name="cylinder_link"> 
<inertial>
 <mass>1.0</mass> 
<inertia> 
<ixx>1.0</ixx> 
<iyy>1.0</iyy> 
<izz>1.0</izz> 
<ixy>0.0</ixy> 
<ixz>0.0</ixz> 
<iyz>0.0</iyz> 
</inertia>
 </inertial> 
<collision name="cylinder_collision"> 
<geometry> 
<cylinder radius="1.0" length="2.0"/> 
</geometry> 
</collision> 
<visual name="cylinder_visual"> 
<geometry> 
<cylinder radius="1.0" length="2.0"/> 
</geometry> 
<material> 
<script> 
<name>Gazebo/Orange</name> 
 <uri>file://media/materials/scripts/gazebo.material</uri> 
</script> 
</material> 
</visual> 
</link> 
</model> 
添加球体的原理和步骤类似

编写launch文件

然后编写launch文件，要实现一键完成gazebo环境启动，slam建图和保存地图的功能需要重新编写launch文件在launch文件中，需要包含对Gazebo的引用、gmapping节点的配置以及地图保存的命令。具体代码如下：

bash 复制代码

<launch>
<!-- 启动Gazebo仿真环境 --> 
<include file="$(find gazebo_ros)/launch/empty_world.launch"> 
<arg name="world_name" value="$(find your_package)/worlds/your_world.world"/> 
<!-- 如果有需要，可以添加其他Gazebo参数 --> 
</include> 
<!-- 启动gmapping节点 --> 
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen"> 
<param name="base_frame" value="base_link"/> 
<param name="odom_frame" value="odom"/> 
</node> 
<!-- 启动地图保存节点 --> 
<node pkg="map_server" type="map_saver" name="map_saver" output="screen" args="-f /home/catkin_ws/src/turtlebot3/turtlebot3_navigation/maps/map" /> 
<!-- 如果需要运行RViz进行可视化，同理添加rviz的启动配置 --> 
</launch>

运行测试

在终端里输入以下指令,之后便完成了修改gazebo仿真环境并保存修改之后的地图。

bash 复制代码

roslaunch rospag mapping_and_saving.launch

任务6：自主导航写"车"字

任务描述

利用上述任务保存的地图，小车定点导航，给小车发点，让小车的轨迹写一个汉字"车"。

任务分析

首先需要获取地图数据：从上述任务保存的地图数据中读取地图信息，包括地图大小、路障位置等。再设置小车起点：确定小车的起点坐标。
接着定义汉字"车"的轨迹：根据汉字"车"的形状，设计小车的运行轨迹，确定小车需要经过的坐标点。
小车定点导航：使用导航算法，将小车从起点导航至每个目标点，并控制小车按照指定的轨迹运动。
判定是否到达终点：每到达一个目标点，判断小车是否到达轨迹的最后一个点，如果是，则表示小车已形成一个汉字"车"。
结束任务：小车形成汉字"车"后，任务结束。

这里需要自己编写一个python脚本实现，其主要目的是生成导航目标并发送到move_base，主要包括：

导入必要的库
定义一个包含目标位置（x, y, yaw）的列表作为路径点
用rospy.init_node()初始化ROS节点
创建一个actionlib.SimpleActionClient对象，连接到move_base的MoveBaseAction
使用client.wait_for_server()等待move_base服务就绪
对于每个路径点，创建一个MoveBaseGoal对象并设置目标位置
使用client.send_goal(goal)发送目标到move_base、等待结果或使用回调函数来处理结果。

这里给出demo以帮助大家实现：

bash 复制代码

#! /usr/bin/env python
#coding=utf-8
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
points = [
        [(0.0747399330139, 1.63215446472, 0.0), (0.0, 0.0, 0.0039991690347, 0.999992003292)],
        [(0.662498235703, 1.61071753502, 0.0), (0.0, 0.0, -0.704229044006, 0.709972854114)],
        #其他导航点需要大家自行调整
         ]
#points里面是各目标点信息
current_point = 0

def move_to_point(x, y, z, quat):
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()

    goal = MoveBaseGoal()
    goal.target_pose.header.frame_id = 'map'#map为地图名
    goal.target_pose.pose.position.x = x
    goal.target_pose.pose.position.y = y
    goal.target_pose.pose.position.z = z
    goal.target_pose.pose.orientation.x = quat[0]
    goal.target_pose.pose.orientation.y = quat[1]
    goal.target_pose.pose.orientation.z = quat[2]
    goal.target_pose.pose.orientation.w = quat[3]

    client.send_goal(goal)
    rospy.loginfo('Moving to point: ({}, {}, {})'.format(x, y, z))
    client.wait_for_result()

    # 发送下一个目标点
    global current_point
    current_point += 1
    if current_point < len(points):
        x, y, z = points[current_point][0]
        quat = points[current_point][1]
        move_to_point(x, y, z, quat)
    else:
        rospy.loginfo('All points reached')

def goal_reached_callback(msg):
    if msg.status.status == 3: 
        rospy.loginfo('Goal reached')

if __name__ == '__main__':
    rospy.init_node('navigation_demo')

    move_to_point(*points[current_point][0], quat=points[current_point][1])

    rospy.spin()

最后我们可以启动终端检验我们的小车了，具体流程如下：

bash 复制代码

#在第一个终端里面执行
roslaunch turtlebot3_gazebo turtlebot3_world.launch 
#启动gazebo仿真环境   
                               
#在第二个终端执行
roslaunch turtlebot3_navigation turtlebot3_navigation.launch
#开启导航功能，注意在进行写"车"字之前要检查下该功能是否正常

#在第三个终端里执行
rosrun show_trajectory by_marker
#开启轨迹记录和显示功能  

#在第四个终端里执行
rosrun turtlebot3_navigation nav.py
#实现写"车"字

通过上述流程，我们较好的完成了任务，下面是小车自己写出来的"车"字。

小结

回顾上篇、中篇和下篇，由于笔者是自己第一次尝试写博客，因此有些表述不到位的地方还请大家见谅，2024年睿抗机器人大赛已经告一段落，写这三篇文章也算是对我们在比赛过程中解决问题的一个总结，希望可以帮助大家更加了解睿抗机器人大赛智能侦查赛道具体是做什么，要怎么去解决仿真中遇到的问题，大赛从省赛到国赛，从仿真到实物应用，也是比较适合对ros感兴趣的小伙伴学习ros系统，提高编程能力的一个平台，比赛中遇到的各种小问题会让我们解决实际问题的能力获得较大的提升。对睿抗机器人大赛希望有更多了解的小伙伴可以私信我，对我上述表达有问题的地方也请大家积极指正。