mavros_教程之一（翻译）

1. 关于作者 Masoud Iranmehr

简介：

作者出生在伊朗马赞达兰省的萨里市。我在伊朗德黑兰的夏里夫大学获得了电子科学硕士学位。

我的兴趣

我对嵌入式系统（Linux/Arm）和机器人学特别感兴趣，尤其喜爱ROS（机器人操作系统），因为它非常易于使用，并且可以通过多种编程语言如C++、Python、Javascript等进行开发。我发现Ardupilot是一个非常专业的开源软件包，可以驱动各种类型的机器人，包括飞机、直升机和巡航器。因此，我决定使用这个工具来驱动简单的机器人，并通过ROS对其进行控制。

此外，我精通在机器人领域使用ARM微控制器（STM32），并能够直接在这些处理器与ROS之间进行通信。这种技术可以用来设计在ROS功能下运行的简单机器人。

我在这里愿意分享我的机器人学知识给大家。
如果你对我的示例程序及其文档感兴趣，欢迎加入我的git仓库并成为贡献者。
你可以通过以下方式免费向我询问有关我的程序包的问题：

邮箱地址：masoud.iranmehr@gmail.com

诚挚地，

Masoud Iranmehr

2. 主页

（1）欢迎来到 MAVROS 教程

Author: Masoud Iranmehr

Github Page: https://github.com/masoudir/mavros_tutorial

C++ Examples on Github: https://github.com/masoudir/mavros_cpp_examples

Python Examples on Github: https://github.com/masoudir/mavros_python_examples

Web Page: https://masoudir.github.io/mavros_tutorial/

（2）介绍

该项目展示了一些关于使用 python 和 C++ 语言以及 MAVROS（机器人操作系统的 MAV 链接协议）通过 Ardupilot-SITL 模拟机器人的示例。

（3）安装要求

A.安装一些必要的软件包

只需执行以下命令：
安装python3和pip3

sudo apt-get install python-dev python-pip python3-dev python3-pip python3-rospkg

B.安装 Ardupilot-SITL

Ardupilot-SITL 是用于 ArduPilot 开发和测试的模拟环境。它允许开发人员在计算机上模拟飞行器（如飞机、直升机、无人机和车辆）的运行，而无需实际飞行硬件。通过 Ardupilot-SITL，开发人员可以测试飞行控制算法、自动飞行模式和传感器集成，以及对飞行器行为进行调试和验证，而无需飞行实际飞行器。这对于飞行控制软件的开发和调试非常有用，因为它提供了一个安全且成本效益高的方法来测试新的功能和算法。
参考链接：https://ardupilot.org/dev/docs/building-setup-linux.html

（4）安装 MAVProxy（辅助工具）

我建议在需要 MAVlink 协议的情况下使用外部代理。MAVProxy 可以将来自输入端口的任何 MAVLink 消息转发到其他 TCP/UDP/串行端口。如果您要为两个或更多的 GCS 或控制器节点使用一个车辆，这将非常有用。只需执行以下命令安装：

pip install MAVProxy - 使用pip在Python2中安装 MAVProxy

（5）安装 ROS

ROS是面向机器人的半操作系统。ROS通常支持多种编程语言，如C++、Python、Javascript等。它是免费且易于使用的。到目前为止，ROS有三个支持的版本（kinetic、melodic和neotic），我在本文档中更喜欢使用melodic版本。要安装ROS-melodic，有两种方法：从预构建的二进制文件安装或从源代码安装。我倾向于安装完整版本的ros-melodic。如果你不是使用Ubuntu，可能会在按照ROS教程中提到的二进制文件安装ROS时遇到一些问题，因为这个过程适用于任何类型的Linux平台。

要从源代码安装ROS-melodic，你可以参考这个链接。

要从二进制文件安装ROS-melodic，你可以参考这个链接。

（6）安装 MAVROS

如果您想从二进制文件安装 ROS，建议也从二进制文件安装 MAVROS。

A.从二进制文件安装 MAVROS

对于这一部分，你可以参考这个链接。
为了方便起见，以下是从二进制文件安装MAVROS所需执行的命令：
(还包括安装各种依赖)

sudo apt-get install ros-melodic-mavros ros-melodic-mavros-extras
wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
chmod a+x install_geographiclib_datasets.sh
./install_geographiclib_datasets.sh

chmod a+x 是一个命令，用于修改文件或目录的权限，使其具有可执行权限。

• chmod: 是用于修改文件或目录权限的命令。

•   a+x
  

: 表示将所有用户（包括所有者、组和其他用户）赋予可执行权限。其中：

- `a` 表示所有用户。
- `+x` 表示添加可执行权限。

B.从源安装 MAVROS

如果你不使用Ubuntu，强烈建议从源代码安装MAVROS，因为各种Linux发行版之间的相似性。你可以参考这个链接获取完整的文档。以下是摘要命令：

source <ROS_INSTALL_PATH>/devel/setup.bash - 定义ROS的安装文件夹（你可以将此命令插入到~/.bashrc文件的底部，以便在打开新shell时自动运行此命令。可以使用以下命令进行此操作：sudo nano ~/.bashrc）

roscore - 启动ros核心以访问其功能和内置包

sudo apt-get install python-catkin-tools python-rosinstall-generator -y - 安装与ROS相关的python包

cd ~/my_catkin_ws/src - 跳转到catkin src文件夹

git clone https://github.com/ros-geographic-info/unique_identifier.git - 克隆unique_identifier包

git clone https://github.com/ros-geographic-info/geographic_info.git - 克隆geographic_info包

cd ~/my_catkin_ws - 跳转到catkin文件夹

catkin init - 初始化catkin工作空间

wstool init src - 初始化ros包安装程序

rosinstall_generator --rosdistro melodic mavlink | tee /tmp/mavros.rosinstall - 安装MAVLink（我们使用melodic参考来处理所有ROS发行版，因为它不是特定于发行版的，并且是最新的）

rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall - 安装MAVROS：获取源代码（上游 - 已发布）

wstool merge -t src /tmp/mavros.rosinstall - 创建工作空间和依赖关系

wstool update -t src -j4 - 构建ros工作空间

rosdep install --from-paths src --ignore-src -y - 安装ros包

./src/mavros/mavros/scripts/install_geographiclib_datasets.sh - 安装GeographicLib数据集

catkin build - 构建源代码

source devel/setup.bash - 使用工作空间的setup.bash或setup.zsh，以便rosrun可以从该工作空间找到节点

（7）启动 Rover 机器人

启动 Ardupilot-SITL 模拟 Rover

在进行 Ardupilot 安装文件夹的 ardupilot/Tools/autotest/ 目录下：

cd <你的Ardupilot安装文件夹>/ardupilot/Tools/autotest/

启动 Rover 车辆：

python sim_vehicle.py -v Rover

请注意，不要使用 python3 来运行或构建 MAVProxy，因为在该包中存在与 MAVProxy 模块（如 “map” 和 “console”）不匹配的问题。

如果要显示其他 SITL 模块，如地图（map）或控制台（console），可以使用以下命令：

python sim_vehicle.py -v Rover --map --console

或者，您可以在 SITL 终端中加载它们：

module load map

module load console

注意：当 Ardupilot 首次编译机器人时，请耐心等待。这需要 2-3 分钟来完成。

您会注意到 ardupilot-sitl 创建了一些输出，例如 “127.0.0.1:14550” 和 “127.0.0.1:14551”。这些是用于将此车辆与另一个 MAVProxy 控制台进行通信的辅助 UDP 端口。我们在示例中使用这些端口。

（9）连接 Rover 到 MAVROS

首先，您需要配置 ROS 和 MAVROS。
如果您已经从二进制文件安装了它们，请按照以下命令操作：

source /opt/ros/melodic/setup.bash - 定义 ROS 和 MAVROS 的安装文件夹路径

或者，如果您是从源代码安装的，请执行以下命令：

source <ROS_INSTALL_PATH>/devel/setup.bash - 定义 ROS 的安装文件夹路径（您可以将此命令插入到 ~/.bashrc 文件的末尾，以便在打开新的 shell 时自动运行此命令。可以使用以下命令打开 ~/.bashrc 文件：sudo nano ~/.bashrc）
source <MAVROS_INSTALL_PATH>/devel/setup.bash - 定义 MAVROS 的安装文件夹路径（您可以将此命令插入到 ~/.bashrc 文件的末尾，以便在打开新的 shell 时自动运行此命令。可以使用以下命令打开 ~/.bashrc 文件：sudo nano ~/.bashrc）

然后，是启动 ROS 核心的时间：

roscore - 启动 ROS 核心，用于访问其功能和构建的包

随后，您需要告诉 MAVROS 如何找到您要使用的车辆：

roslaunch mavros apm.launch fcu_url:=udp://:14550@ - 将来自 UDP:14550 端口的车辆连接到 MAVROS

或者，如果您想通过远程 TCP 端口连接到您的机器人，可以使用以下命令：

roslaunch mavros apm.launch fcu_url:=tcp://<REMOTE_HOST>:<TCP_PORT>@ - 将来自 REMOTE_HOST:TCP_PORT 端口的车辆连接到 MAVROS

（10）查看 ROS 主题

source <ROS_INSTALL_PATH>/devel/setup.bash

这条命令用于设置 ROS 的安装路径（请将 <ROS_INSTALL_PATH> 替换为实际的 ROS 安装路径）。您可以将此命令插入到 ~/.bashrc 文件的末尾，以便在打开新的 shell 时自动运行此命令。可以使用以下命令编辑 ~/.bashrc 文件：

sudo nano ~/.bashrc

接着执行以下命令来启动 ROS 的插件监控窗口：

rqt

然后，在显示的 rqt 窗口中，从菜单中选择 “Plugins” > “Topics” > “Topic Monitor” 来开始主题监视。这将显示如下底部的图形界面：

3.第1章 ArduRover 使用命令行界面（CLI）

（1）步骤1 如何更改模式

A.初始化

请按照此链接中提到的步骤操作。

B-1.使用 MAVProxy 或 Ardupilot-SITL 更改车辆模式

获取模式：
如果要查看车辆的最后模式，您只需查看这些终端中的 “>” 字符之前的名称。

设置模式：
如果要更改模式，您可以直接从由 MAVProxy 或 SITL 提供的终端发送命令。例如，如果要将车辆模式更改为 “GUIDED”，可以输入以下命令：

GUIDED - 更改模式为 GUIDED

您还可以通过以下命令查看车辆的所有可用模式：

mode - 显示所有可用模式

B-2.使用 ROS rqt 更改车辆模式：

首先，您需要从 rqt 中添加两个插件：

Plugins -> Topics -> Topic Monitor
Plugins -> Services -> Service Caller

然后在 “Topic Monitor” 部分，您需要勾选 “/mavros/state” 主题以查看其内容，从而可以获取车辆模式以及其他参数，如 arm status 和 Guided status。

接下来，在 “Service Caller” 部分，选择 /mavros/set_mode，然后在 “custom_mode” 字段中设置您的车辆模式。然后点击 “call” 按钮发送此消息。结果应如下所示：

B-3.使用 ROS 命令行更改车辆模式

获取模式
如果您想查看车辆的最后模式，只需键入以下命令：
rostopic echo /mavros/state - 定期返回 /mavros/state 主题的所有内容
结果应类似于以下内容

header:
  stamp:
    secs: 1595198641
    nsecs: 651358095
  frame_id: ''
connected: True
guided: False
manual_input: True
mode: "MANUAL"
system_status: 4

您可以在结果中的 “mode” 字段中找到模式。

设置模式
您需要调用一个 ROS 服务来执行此操作。只需按照以下命令操作：
rosservice info /mavros/set_mode - 查看其参数和消息类型
此命令将返回以下结果:

Node: /mavros
URI: rosrpc://ubuntu:42571
Type: mavros_msgs/SetMode
Args: base_mode custom_mode

mavros_msgs/SetMode 是此服务的 srv 文件类型，“Args” 是我们的输入参数，但是我们需要知道它们的类型，因此我们输入以下命令：

rossrv show mavros_msgs/SetMode- 获取所述 srv 文件的详细信息
结果如下所示：

uint8 MAV_MODE_PREFLIGHT=0
uint8 MAV_MODE_STABILIZE_DISARMED=80
uint8 MAV_MODE_STABILIZE_ARMED=208
uint8 MAV_MODE_MANUAL_DISARMED=64
uint8 MAV_MODE_MANUAL_ARMED=192
uint8 MAV_MODE_GUIDED_DISARMED=88
uint8 MAV_MODE_GUIDED_ARMED=216
uint8 MAV_MODE_AUTO_DISARMED=92
uint8 MAV_MODE_AUTO_ARMED=220
uint8 MAV_MODE_TEST_DISARMED=66
uint8 MAV_MODE_TEST_ARMED=194
uint8 base_mode
string custom_mode
---
bool mode_sent

在这种情况下，custom_mode是需要配置的参数，其类型为 string。例如，如果您想将车辆模式更改为 GUIDED，请执行以下命令：

rosservice call /mavros/set_mode "custom_mode: 'GUIDED'" - 将车辆模式更改为 “GUIDED”

另一种方法是使用 “rosrun” 命令：
rosrun mavros mavsys mode -c MANUAL - 如果要更改模式为 “MANUAL”

rosrun mavros mavsys mode -b <ENUM VALUE> - 如果要使用 “base_mode” 更改模式

（2）步骤2 如何解锁和上锁（arm）

请按照此链接中提到的步骤操作

A.使用 MAVProxy 或 Ardupilot-SITL 进行武装和解除武装

您可以直接从 MAVProxy 或 SITL 提供的终端发送命令。
为此，只需输入以下命令：
arm throttle - 此命令用于解锁机器人
disarm - 此命令用于锁定机器人

B.使用 ROS rqt 进行武装和解除武装

首先你需要从 rqt 添加两个插件:

Plugins -> Topics -> Topic Monitor
Plugins -> Services -> Service Caller
首先在Topic Monitor选项卡中，勾选/mavros/state话题以查看其内容，这样您就可以获取飞行器的模式以及其他参数，比如arm状态和引导状态。

然后在Service Caller选项卡中，选择/mavros/cmd/arming服务，在value字段中设置您的arm状态为布尔类型变量（False/True）。然后点击“call”按钮发送此消息。结果应如下所示：

C.使用 ROS 命令行进行武装和解除武装

1- 监控 Rover 的总体状态（武装状态和模式名称）

如果您想查看MAVROS中您的ardupilot机器人的总体状态
您需要了解MAVROS创建的所有话题：

rostopic list - 返回所有可用的话题
然后您可以看到机器人的每个部分都被映射到一个特定的话题上。为了显示机器人的解锁状态，您必须使用/mavros/state话题：

rostopic echo /mavros/state - 定期返回/mavros/state话题的所有内容
结果应该类似于这样：

---                                                                                                                                                                    
header:                                                                                                                                                                
  stamp: 09                                                                                                                                                            
    secs: 1595198641

    nsecs: 651358095                                                                                                                                                   
  frame_id: ''                                                                                                                                                         
connected: True                                                                                                                                                        
guided: False                                                                                                                                                          
manual_input: True

mode: "MANUAL"                                                                                                                                                         
system_status: 4                                                                                                                                                       
---

命令rostopic echo将每隔一秒返回主题的所有内容。在这种情况下，“已连接”字段显示您的 MAVROS 仍然连接到机器人。 “模式”还显示当时车辆模式的名称。

2- 向机器人发送武装/解除武装命令

这里是翻译的内容：

只需按照以下命令操作：

rosservice call /mavros/cmd/arming True - 发送解锁命令给机器人
rosservice call /mavros/cmd/arming False - 发送锁定命令给机器人

另一种方法是使用 “rosrun” 命令：
rosrun mavros mavsafety arm- 发送解锁命令给机器人
rosrun mavros mavsafety disarm - 发送锁定命令给机器人

（3）步骤3 如何进行移动

A.初始化

请按照这个链接中提到的步骤操作。然后，您需要按照这个链接中的说明将您的车辆模式更改为GUIDED，然后您可以使用其中一个链接中提到的方法对机器人进行解锁。然后，根据这里描述的步骤移动您的车辆。

B.使用 MAVProxy 或 Ardupilot-SITL 移动您的车辆

我们假设您的车辆处于GUIDED模式，如果不是，请按照此页面上“初始化”部分提到的链接进行操作。然后，使用 MAVProxy 或 SITL 中的以下命令移动您的车辆：

GUIDED 纬度 经度 高度 - 将车辆移动到指定的目的地
参数 (纬度, 经度, 高度) 是用来确定车辆目的地的位置参数。

如果您想要停止车辆移动，您需要使用以下命令：

mode HOLD - 停止车辆运动
此外，还有其他一些模式可以使车辆移动，例如“RTL”、“MANUAL”等。请参考此链接获取更多信息。

C.使用 ROS rqt 移动您的车辆

我们假设您的车辆处于GUIDED模式，如果不是，请按照此页面上“初始化”部分提到的链接进行操作。然后，您需要从rqt中添加以下插件：

Plugins -> Topics -> Message Publisher
然后，您需要添加一个话题，地址为/mavros/setpoint_raw/global，点击“+”进行添加。接着，您需要更改位置的数值（“纬度”、“经度”、“海拔”），以设定车辆的目标位置。然后勾选该话题以便每隔1秒发布一次（此间隔可以更改），或者右键点击该话题，然后选择Publish Selected Once仅发布一次。然后您就可以看到您的车辆移动到指定的目的地了。

D.使用 ROS 命令行更改车辆模式

您需要通过以下命令发布目标位置：
指定经度和纬度：

rostopic pub /mavros/setpoint_raw/global mavros_msgs/GlobalPositionTarget -1 "{'longitude': 14.51218, 'latitude': 10.15785}" 

如果您只想指定纬度（经度为0）：

rostopic pub /mavros/setpoint_raw/global mavros_msgs/GlobalPositionTarget -1 "{'latitude': 10.15785}"

这里的 “-1” 表示该命令只发布一次。如果您想定期发布这条消息，您可以删除 “-1” 参数，并使用 -r RATE参数来指定发布频率：

rostopic pub /mavros/setpoint_raw/global mavros_msgs/GlobalPositionTarget -r 1 "{'longitude': 14.51218, 'latitude': 10.15785}"

这将以1 Hz的频率发布指定目的地的消息，使车辆移动到指定位置。

（4）步骤4 如何设置和获取参数

A.初始化

请按照此链接中提到的步骤操作

B.使用 MAVProxy 或 Ardupilot-SITL 获取/设置 MAVROS 参数

如果您想设置参数，可以使用以下命令：

param set PARAMETER VALUE - 设置参数的新值

如果您想获取参数的值，可以使用以下命令之一：

param fetch PARAMETER - 获取参数的值
param show PARAMETER - 获取参数的值

例如，如果您想将CRUISE_SPEED作为您的参数，并设置或获取它的值，可以使用以下命令：

设置参数值：

param set CRUISE_SPEED 20 - 设置参数的新值

获取参数值：

param fetch CRUISE_SPEED - 获取参数的值
或者
param show CRUISE_SPEED - 获取参数的值

这些命令允许您设置和获取特定参数的值。

C.使用 ROS rqt 获取/设置 MAVROS 参数

首先，您需要从 rqt 中添加两个插件：

Plugins -> Services -> Service Caller

如果您想设置参数，然后在 Service Caller部分，您需要选择 /mavros/param/set，然后在param_id字段中输入您要设置的参数名称，在integer或 real字段中设置该参数的新值。然后点击 call按钮发送此消息。以参数CRUISE_SPEED 为例，设置参数的结果应如下所示：

正如您所看到的输出，包含一个名为 success的布尔类型字段（False/True），表示此过程是否成功完成。此外，新值将显示在屏幕上，如您所见。

请注意，当您的值为整数格式时，integer和 real 之间没有任何区别。

如果您想设置参数，则在Service Caller 部分，您需要选择 /mavros/param/get，然后在 param_id字段中输入您要设置的参数名称，在integer" 或 "real字段中设置该参数的新值。然后点击 call按钮发送此消息。以参数 CRUISE_SPEED为例，设置参数的结果应如下所示：

正如您所看到的，输出构成了一个 bool 类型（False/True）的“成功”字段，并指示此过程是否成功完成。如您所见，该参数的新值也将显示在该屏幕上。

D.使用 ROS 命令行更改车辆模式

如果您想设置参数值，您需要调用一个ROS服务来执行此操作。请按照以下命令操作：
查看服务的详细信息：

rosservice info /mavros/param/set - 查看此服务的详细信息

这个命令将给出如下结果：

Node: /mavros
URI: rosrpc://ubuntu:58491
Type: mavros_msgs/ParamSet
Args: param_id value

mavros_msgs/ParamSet 是此服务的服务消息类型，Args是我们的输入参数，但我们需要知道它们的类型，因此我们执行以下命令：

rossrv show mavros_msgs/ParamSet - 获取所述服务文件的详细信息

这将得到以下结果：

string param_id
mavros_msgs/ParamValue value
  int64 integer
  float64 real
---
bool success
mavros_msgs/ParamValue value
  int64 integer
  float64 real

在这种情况下，param_id是需要配置的参数名称，其类型为string。此外，integer和 real是描述此参数值的字段。

要使用新值设置参数，请运行以下命令：

rosservice call /mavros/param/set "{'param_id':'PARAM', 'value':[INTEGER_VALUE, REAL_VALUE]}" - 更改参数的值

例如，对于参数 CRUISE_SPEED：

rosservice call /mavros/param/set "{'param_id':'CRUISE_SPEED', 'value':[20, 0]}" - 更改参数的值

如果您想获取参数的值，请运行以下命令：

rosservice call /mavros/param/get "{'param_id':'PARAM'}" - 更改车辆模式为 "GUIDED"

例如，对于参数 CRUISE_SPEED：

rosservice call /mavros/param/get "{'param_id':'CRUISE_SPEED'}" - 获取参数的值

使用以下 rosrun 命令更改车辆模式：

rosrun mavros mavparam set PARAM VALUE - 将 PARAM 的值更改为 VALUE

rosrun mavros mavparam get PARAM - 返回 PARAM 的值

这些命令可以帮助您设置和获取参数的值，以及更改车辆模式。

4.第2章 ArduRover 使用 Python

（1）步骤1 驾驶 rover

A.初始化：

请按照此链接中提到的步骤操作

B.需求：

您只需使用以下命令克隆 mavros_python_examples：

git clone https://www.github.com/masoudir/mavros_python_examples
cd mavros_python_examples

或者您可以通过 pip 轻松安装此包

pip3 install mavros_python_examples

C.如何使用：

“mavros_python_examples” 包含一个测试文件，展示了使用该软件包驾驶小车的最简单方法。要访问此文件，请参考 “/test/rover.py”，如果您尚未克隆此项目，也可以通过以下命令下载该代码：

wget https://github.com/masoudir/mavros_tutorial/test/rover.py

该文件的内容如下：
**
在驾驶您的Rover并使其移动之前，您需要按照这里的说明进行操作，然后运行以下命令：

如果您已经克隆了此软件包：
python3 test/rover.py

如果您通过 pip3 安装了此软件包：
python3 rover.py

您将看到您的车辆模式更改为 GUIDED，然后机器人将被解锁并移动到指定点。

要了解更多关于此软件包的详细信息，请参考此链接。

（2）步骤2 详细信息-Python

A.初始化

该包由四个不同的 python 类组成，可以轻松地使用 Ardupilot 车辆。
此处描述了这些类。

B-1.解释细节——定义TOPICS和SERVICES类

首先，我们创建了一个名为TopicService的类，如下所示，以便更轻松地定义主题和服务：

class TopicService:
def __init__(self, name: str, classType):
    self.__name = name
    self.__classType = classType
    self.__data = None
def set_data(self, data):
    self.__data = data
def get_data(self):
    return self.__data
def get_type(self):
    return self.__classType
def get_name(self):
    return self.__name

正如您所看到的，在这个类中定义了三个参数：

• name指向 TOPIC 或 SERVICE 的名称，
• classType指向所使用的类的类型，
• data指向此 TOPIC 或 SERVICE 的内容。

B-2. 定义处理 ROS 主题和服务的类

我们 定义了一个名为 “RosHandler” 的第二个类，用于在 ROS 环境中更轻松地处理 TOPICS 和 SERVICES。该类的内容如下所示：

class RosHandler:
    def __init__(self):
        self.rate = 1
        self.connected = False

    def connect(self, node: str, rate: int):
        rospy.init_node(node, anonymous=True)
        self.rate = rospy.Rate(rate)
        self.connected = True
        rospy.loginfo("Rospy is up ...")
        rospy.spin()

    def disconnect(self):
        if self.connected:
            rospy.loginfo("shutting down rospy ...")
            rospy.signal_shutdown("disconnect")
            self.connected = False

    @staticmethod
    def topic_publisher(topic: TopicService):
        pub = rospy.Publisher(topic.get_name(), topic.get_type(), queue_size=10)
        pub.publish(topic.get_data())
        print("edfgedge")

    @staticmethod
    def topic_subscriber(topic: TopicService):
        rospy.Subscriber(topic.get_name(), topic.get_type(), topic.set_data)

    @staticmethod
    def service_caller(service: TopicService, timeout=30):
        try:
            srv = service.get_name()
            typ = service.get_type()
            data = service.get_data()

            rospy.loginfo("waiting for ROS service:" + srv)
            rospy.wait_for_service(srv, timeout=timeout)
            rospy.loginfo("ROS service is up:" + srv)
            call_srv = rospy.ServiceProxy(srv, typ)
            return call_srv(data)
        except rospy.ROSException as e:
            print("ROS ERROR:", e)
        except rospy.ROSInternalException as e:
            print("ROS ERROR:", e)
        except KeyError as e:
            print("ERROR:", e)
        return None

在使用 MAVROS 连接到车辆之前，请小心执行topic_subscriber()函数。此函数创建一个用于从 ROS 接收数据的事件处理程序。

然后，您可以使用connect()函数连接到您的车辆。该函数接受两个参数name和 rate，分别是由此代码建立的 ROS 节点的名称以及从 ROS TOPICS 接收数据的速度。

请注意，从每个 topic 接收数据的实际速率直接关联到该 topic 中定义的速率。这意味着，如果特定 topic 的频率为 40 Hz，您可以在 “connect()” 函数中使用的速率之下更新其数据。

您可以通过 “topic_publisher()” 函数发布您的 topics，也可以通过 “service_caller” 函数发送您的服务命令。

B-3. 定义使用 Rover 功能的类

为了使用Rover车辆，我们需要定义它的主题和服务，并定义如何使用它们的规则。这些是在此类上完成的，如下所示：

class RoverHandler(RosHandler):
def __init__(self):
    super().__init__()
    self.armed = False
    self.mode = ""

    self.TOPIC_STATE = TopicService("/mavros/state", mavros_msgs.msg.State)
    self.SERVICE_ARM = TopicService("/mavros/cmd/arming", mavros_msgs.srv.CommandBool)
    self.SERVICE_SET_MODE = TopicService("/mavros/set_mode", mavros_msgs.srv.SetMode)
    self.SERVICE_SET_PARAM = TopicService("/mavros/param/set", mavros_msgs.srv.ParamSet)
    self.SERVICE_GET_PARAM = TopicService("/mavros/param/get", mavros_msgs.srv.ParamGet)
    self.TOPIC_SET_POSE_GLOBAL = TopicService('/mavros/setpoint_raw/global', mavros_msgs.msg.GlobalPositionTarget)

    self.thread_param_updater = threading.Timer(0, self.update_parameters_from_topic)
    self.thread_param_updater.daemon = True
    self.thread_param_updater.start()

def enable_topics_for_read(self):
    self.topic_subscriber(self.TOPIC_STATE)

def arm(self, status: bool):
    data = mavros_msgs.srv.CommandBoolRequest()
    data.value = status
    self.SERVICE_ARM.set_data(data)
    result = self.service_caller(self.SERVICE_ARM, timeout=30)
    return result.success, result.result

def change_mode(self, mode: str):
    data = mavros_msgs.srv.SetModeRequest()
    data.custom_mode = mode
    self.SERVICE_SET_MODE.set_data(data)
    result = self.service_caller(self.SERVICE_SET_MODE, timeout=30)
    return result.mode_sent

def move(self, lat: float, lon: float, alt: float):
    data = mavros_msgs.msg.GlobalPositionTarget()
    data.latitude = lat
    data.longitude = lon
    data.altitude = alt
    self.TOPIC_SET_POSE_GLOBAL.set_data(data)
    self.topic_publisher(topic=self.TOPIC_SET_POSE_GLOBAL)

def get_param(self, param: str):
    data = mavros_msgs.srv.ParamGetRequest()
    data.param_id = param
    self.SERVICE_GET_PARAM.set_data(data)
    result = self.service_caller(self.SERVICE_GET_PARAM, timeout=30)
    return result.success, result.value.integer, result.value.real

def set_param(self, param: str, value_integer: int, value_real: float):
    data = mavros_msgs.srv.ParamSetRequest()
    data.param_id = param
    data.value.integer = value_integer
    data.value.real = value_real
    self.SERVICE_SET_PARAM.set_data(data)
    result = self.service_caller(self.SERVICE_SET_PARAM, timeout=30)
    return result.success, result.value.integer, result.value.real

def update_parameters_from_topic(self):
    while True:
        if self.connected:
            data = self.TOPIC_STATE.get_data()
            self.armed = data.armed
            self.mode = data.mode

这是一个名为 “RoverHandler” 的类，继承自 “RosHandler” 类，用于处理在 ROS 环境中更容易操作的 TOPICS 和 SERVICES。
该类包含以下方法：

• enable_topics_for_read()：启用对 “/mavros/state” 主题的订阅，以便读取车辆状态数据。
• arm(status: bool)：控制车辆的武器状态，将参数 status 设置为 True 表示解锁车辆，设置为 False 表示锁定车辆。
• change_mode(mode: str)：更改车辆的模式，将参数 mode 设置为所需的模式名称（例如 “GUIDED”）。
• move(lat: float, lon: float, alt: float)：移动车辆到指定的全局位置，参数 lat、lon、alt 分别表示目标位置的纬度、经度和海拔高度。
• get_param(param: str)：获取指定参数的值，参数 param 是要获取值的参数名称。
• set_param(param: str, value_integer: int, value_real: float)：设置指定参数的值，参数 param 是要设置值的参数名称，value_integer 是整数值，value_real 是浮点数值。
• update_parameters_from_topic()：持续监听 “/mavros/state” 主题，更新车辆的武器状态和模式信息。
  此类提供了方便的方法来操作与车辆状态、模式、参数和位置相关的 ROS TOPICS 和 SERVICES。

正如您所看到的，使用 “TopicService” 类定义了一些 TOPICS 和 SERVICES：

• Topic /mavros/state 用于读取车辆的整体状态，其类类型为 “mavros_msgs.msg.State”（Python 中的类型）。

• Topic /mavros/setpoint_raw/global用于发布车辆移动的目标位置，其类类型为 “mavros_msgs.msg.GlobalPositionTarget”（Python 中的类型）。

• Service/mavros/cmd/arming用于解锁/锁定车辆，其类类型为 “mavros_msgs.srv.CommandBool”（Python 中的类型）。

• Service /mavros/set_mode用于更改车辆模式，其类类型为 “mavros_msgs.srv.SetMode”（Python 中的类型）。

• Service/mavros/param/set用于设置车辆参数，其类类型为 “mavros_msgs.srv.ParamSet”（Python 中的类型）。

• Service/mavros/param/get 用于获取车辆参数，其类类型为 “mavros_msgs.srv.ParamGet”（Python 中的类型）。

这些 TOPICS 和 SERVICES 允许您通过 ROS 与车辆进行交互，从而读取状态、控制动作以及设置参数等。

（3）步骤3 如何使用-Python

C-1.如何使用——（初始化）：

请按照此链接中提到的步骤操作

C-2.如何使用——（需求）：

import threading
import time
from mavHandler.roverHandler import *


class MyRoverHandler(RoverHandler):
    def __init__(self):
        super().__init__()

        self.user_thread = threading.Timer(0, self.user)
        self.user_thread.daemon = True
        self.user_thread.start()

    def user(self):
        while True:
            time.sleep(1)
            print("arm:", self.armed, "mode:", self.mode)
            print("set param:", self.set_param("CRUISE_SPEED", 2, 0))
            if self.connected:
                print("get param:", self.get_param("CRUISE_SPEED"))
                print("set param:", self.set_param("CRUISE_SPEED", 10, 0))
                self.change_mode(MODE_GUIDED)
                self.arm(True)
                self.move(50.15189, 10.484885, 0)

if __name__ == "__main__":
    v = MyRoverHandler()
    v.enable_topics_for_read()
    v.connect("node1", rate=10)

这段代码定义了一个名为 MyRoverHandler 的类，它继承自 RoverHandler 类，并添加了一个名为 user 的方法，用于模拟用户操作。
在 __init__ 方法中，通过调用 super().__init__() 来初始化基类 RoverHandler。然后创建了一个名为 user_thread 的定时器线程，在定时器线程中启动了 user 方法。
在 user 方法中，通过一个无限循环（while True）定期执行以下操作：

• 每隔一秒打印当前车辆的武器状态 (armed) 和模式 (mode)。
• 调用 set_param 方法来设置名为 “CRUISE_SPEED” 的参数值为 2（整数）和 0（浮点数），并打印设置结果。
• 如果已连接到车辆（self.connected 为 True）：
  • 调用 get_param 方法获取名为 “CRUISE_SPEED” 的参数值，并打印获取结果。
  • 再次调用 set_param 方法将 “CRUISE_SPEED” 参数值设置为 10（整数）和 0（浮点数），并打印设置结果。
  • 调用 change_mode 方法将车辆模式设置为 MODE_GUIDED。
  • 调用 arm 方法将车辆解锁（True）。
  • 调用 move 方法将车辆移动到指定的全局位置（纬度为 50.15189，经度为 10.484885，海拔高度为 0）。
    在 __main__ 块中：
• 创建了一个名为 v 的 MyRoverHandler 实例。
• 调用 enable_topics_for_read 方法以启用对 “/mavros/state” 主题的订阅。
• 调用 connect 方法连接到 ROS 节点（节点名称为 “node1”），并设置数据接收速率为 10 Hz。

这段代码的主要作用是模拟用户操作，通过调用 RoverHandler 类中定义的方法来控制车辆的状态、参数和移动。在后台定时打印和设置车辆状态和参数，以及执行移动操作。

要运行您的代码，请按照此链接中提到的说明将 ROS 函数引入到此代码中（请确保在当前终端中设置 ROS 和 MAVROS 的路径以使用该代码）。然后，如果您使用 git 下载了此项目，就可以运行您的代码。

此代码将创建一个名为 “node1” 的节点，然后将其连接到您通过之前步骤启动的 ArduRover 车辆。接着，它将把 “CRUISE_SPEED” 参数的值设置为 2，然后将车辆模式更改为 “GUIDED”。然后，它将解锁车辆，最后强制车辆移动到目标位置 {“lat”: 50.15189, “lon”: 10.484885}。