GPS 基础概念
GPS 数据有一些限制：
- GPS 数据不是非常精确：GPS 数据的值会波动，你的位置会经常移动。这是不可避免的，你需要以自己的方式减轻这种误差。里程计数据会减少一些这些误差，但不是全部，因为 Summit XL 的轮子会滑动，这会在里程计中产生误差。请记住这一点。 GPS
- 不提供方向信息：GPS 仅提供一个空间中的点，没有方向。这意味着你需要用某种方式的指南针来补充。IMU 在这里很有用。 考虑到这些，你将能够拥有一个粗略但有用的系统，在没有地图的情况下在户外地形上定位和移动你的机器人。当然，你可以使用 GPS 移动，同时生成一张新地图，扩大可以在下次使用更精确地图定位的区域。
启动 GPS 导航
首先，请确保你有一个可以保存和启动所有文件的包，命名为 my_summitxl_tools。
步骤 0：加载一个空地图
因为我们希望有地图，即使我们将根据 GPS 移动，我们仍然需要将一个空地图加载到地图服务器中。为此，只需复制你在第1单元中创建的地图，移除所有黑色区域，留下完全白色（即空白）的地图。
创建这个名为 mymap_empty.yaml 的空版本。您可以根据需要创建它，这是一个页面，您可以在其中将图像完全转换为白色或单色……它为您提供了一些选项：
当你拥有它时，还创建一个同名并引用图像的 yaml 文件：
image: mymap_empty.pgm resolution: 0.050000 origin: [-50.000000, -50.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196创建map server launch启动以便测试其是否正常工作：
start_map_server.launch
<launch><arg name="map_file" default="$(find my_sumit_xl_tools)/maps/mymap_empty.yaml"/><node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" /> </launch>现在地图服务器会将新地图加载到参数服务器中，并且应该可以在 RVIZ 中看到它。启动地图服务器时，您应该会看到类似以下内容：
步骤 1：断开 AMCL
因此，我们需要做的第一件事是断开上一章中使用的 AMCL 定位系统。而且，由于我们的 EKF 节点与 AMCL 节点相关，我们也将删除它。
检索 start_navigation.launch 并创建一个名为 start_navigation_with_gps_ekf.launch 的副本。然后，添加刚刚创建的 start_map_server.launch，并断开所有定位系统。
start_navigation_with_gps_ekf.launch
<launch><!-- Run the map server --><include file="$(find my_sumit_xl_tools)/launch/start_map_server.launch" /><!--- Run Move Base --><include file="$(find my_sumit_xl_tools)/launch/move_base_map.launch" /></launch>那么，删除 AMCL 意味着什么？
AMCL 的主要功能是将变换从 /map 坐标发布到 /odom 坐标。在 amcl 运行时查看此坐标树：
在这里，您可以看到负责连接地图坐标和 odom 坐标的节点是 AMCL。那么，您现在将如何操作呢？
当然，您将使用 ekf_localization 节点！我们还将使用它来发布从地图坐标到 odom 坐标的转换，以及合并的 GPS、IMU 和 odom 数据。
步骤 2：将 GPS 数据转换为 Odometry 类型数据
navsat_transform_node 是一个 ROS 节点，它是 robot_localization 包的一部分。基本上，它允许您将 GPS 纬度和经度数据转换为可以在空间中表示的 XY 坐标。以下是启动它所需的启动文件示例：
start_navsat.launch
<launch><!-- --><node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" respawn="true"><param name="magnetic_declination_radians" value="0"/><param name="yaw_offset" value="0"/><param name="zero_altitude" value="true"/><param name="broadcast_utm_transform" value="false"/><param name="publish_filtered_gps" value="false"/><param name="use_odometry_yaw" value="false"/><param name="wait_for_datum" value="false"/><remap from="/imu/data" to="/imu/data" /><remap from="/gps/fix" to="/gps/fix" /><remap from="/odometry/filtered" to="/odom" /></node></launch>我们来解释一下这个启动文件中出现的一些参数：
<param name="magnetic_declination_radians" value="0"/> <param name="yaw_offset" value="0"/> <param name="zero_altitude" value="true"/>
- magnetic_declination_radians: 如果你的 IMU 提供相对于磁北的方向，则需要此参数。它表示你所在位置的磁偏角。如果你不知道，可以在这里计算：http://www.ngdc.noaa.gov/geomag-web（确保将值转换为弧度）。
- yaw_offset: 当 IMU 面朝东时，其偏航角应读为 0。如果不是，请在此输入偏移量（desired_value = offset + sensor_raw_value）。
- zero_altitude: 如果设置为 true，则此节点生成的里程计消息的姿态 Z 值将被设置为 0。
<param name="broadcast_utm_transform" value="false"/> <param name="publish_filtered_gps" value="false"/>
- broadcast_utm_transform: 这是为了在 TF 树中发布 UTM 静态变换（GPS 系统的原点）用的。
- publish_filtered_gps: 没有什么神秘的，如果你想发布改进后的odometry和 IMU GPS 数据。如果设置为 true，navsat_transform_node 将在 /gps/filtered 主题上发布 sensor_msgs/NavSatFix 消息。
<param name="use_odometry_yaw" value="false"/> <param name="wait_for_datum" value="false"/>
- use_odometry_yaw: 这是用于当你有一个非常可靠的里程计转向系统时。在户外机器人情况下，里程计中的转向数据通常足够可靠，因为它们只是偶尔打滑，特别是在崎岖地形上。这就是为什么这里关闭了这个选项。
- wait_for_datum: 这是为了直接提供系统的基准，即我们 GPS 定位的原点。通常，GPS 会直接提供这些数据，所以通常是关闭的。如果你出于某种原因需要提供它，你需要在启动文件中添加以下参数设置：
<rosparam param="datum">[55.944904, -3.186693, 0.0, map, base_link]</rosparam>或者通过名为
set_datum的 ROS 服务，并使用robot_localization/SetDatum服务消息。<remap from="/imu/data" to="/imu/data" /> <remap from="/gps/fix" to="/gps/fix" /> <remap from="/odometry/filtered" to="/robotnik_base_control/odom" />这些是最重要的参数。它们是获取 GPS、IMU 和里程计数据的主题。通过这些数据，它生成了 Pose 等效值。
你如何知道 GPS 数据是什么意思以及它在现实生活中的位置？
在这种模拟 GPS 的情况下，它起始坐标为：纬度 = 39.5080331， 经度 = -0.4619816。这些是 Summit XL 制造商 Robotnik 的办公室坐标。
在现实生活中，它当然会读取实际地点的数据。你可以在这个 Google 页面上获取地址到经纬度的转换，反之亦然。
一旦你准备好这个启动文件，启动时，它将发布到主题 /odometry/gps。
roslaunch my_sumit_xl_tools start_navsat.launchrostopic info /odometry/gps正如你在这里看到的，消息类型是 nav_msgs/Odometry
## nav_msgs/Odometry std_msgs/Header header uint32 seq time stamp string frame_id string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[36] covariance geometry_msgs/TwistWithCovariance twist geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z float64[36] covariance如你所见，这是一条标准的里程计消息。这意味着现在你有了仅基于 GPS、里程计和 IMU 的 Summit 位置。因此，显而易见的下一步是将这些数据转换成一个 TF，将机器人定位到正确的位置。
创建一个 TF 发布器，将 MAP 帧转换为 Odom 帧，使用 EKF 节点
现在你有了一个基于 GPS 的新的里程计信息源，是时候将这些数据与来自 IMU 和传统里程计的额外数据合并，以生成 TF map -> odom。
让我们更新我们的
start_navigation_with_gps_ekf.launch启动文件版本。start_navigation_with_gps_ekf.launch
<launch><include file="$(find my_sumit_xl_tools)/launch/start_navsat.launch" /><!-- Run the ekf for map to odom config --><node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_with_gps"> <rosparam command="load" file="$(find my_sumit_xl_tools)/config/robot_localization_with_gps.yaml" /></node><!-- Run the map server --><include file="$(find my_sumit_xl_tools)/launch/start_map_server.launch" /><!--- Run Move Base --><include file="$(find my_sumit_xl_tools)/launch/move_base_map.launch" /></launch>现在让我们集中在
ekf_localization节点的启动上：<!-- Run the ekf for map to odom config --> <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_with_gps"> <rosparam command="load" file="$(find my_sumit_xl_tools)/config/robot_localization_with_gps.yaml" /> </node>如你所见，我们正在加载一个新的参数文件，名为
robot_localization_with_gps.yamlExercise 3.1
实现所有启动文件和配置文件，以便能够启动整个系统，并使 Summit XL 完全定位并能够自主导航。它应该能够使用 RVIZ 中的 2D Pose 信号移动，就像在上一章中一样。
遵循的步骤：
- 创建 start_map_server.launch 并检查您创建的空地图是否正常。
- 创建 start_navsat.launch 并检查它是否正常工作并进行转换。移动 Summit XL 机器人以查看值是否相应变化。
- 创建 start_navigation_with_gps_ekf.launch 启动文件的更新版本。
- 创建包含所有必需参数的 robot_localization_with_gps.yaml 文件。
- 启动它并检查 Summit XL 是否可以导航。
尝试到达外面的建筑桶。离开建筑物时要小心。
END Exercise 3.1
你应该在 RVIZ 中获得与此类似的结果：
你成功了吗？好吧，如果你没有，让我们看看配置文件现在应该是什么样子：
robot_localization_with_gps.yaml
#Configuation for robot odometry EKF # frequency: 50two_d_mode: truepublish_tf: trueodom_frame: summit_xl_a_odom base_link_frame: summit_xl_a_base_footprint world_frame: map map_frame: mapodom0: /robotnik_base_control/odom odom0_config: [false, false, false,false, false, false,true, true, false,false, false, true,false, false, false] odom0_differential: trueimu0: /imu/data imu0_config: [false, false, false,false, false, true,false, false, false,false, false, true,true, false, false] imu0_differential: falseodom1: /odometry/gps odom1_config: [true, true, false,false, false, false,false, false, false,false, false, false,false, false, false] odom1_differential: falseprocess_noise_covariance": [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]如您所见，此配置文件的主要区别在于现在我们有了 GPS 传感器：
odom1: /odometry/gps odom1_config: [true, true, false,false, false, false,false, false, false,false, false, false,false, false, false] odom1_differential: false此 /odometry/gps 主题包含 navsat_transform_node 进行的 GPS 到 Odometry 转换的数据。
还请注意，我们将 publish_tf 变量设置为 true。这非常重要，因为现在我们需要发布从 odom 到地图的转换。
您可以检查您拥有的 TF 树。您应该看到您的 ekf_localization_with_gps 节点发布了从 odom 到地图的地图。
在RVIZ中表示MAP位置
因为现在有了 GPS 数据，所以您可以将机器人表示在 GPS 定位的实际位置。得到的结果如下：
但这并不是您应该看到的，因为如果您根据原点经纬度（olat = 49.9 olon = 8.9）从 Google 页面检索地址，您应该会得到类似 Google 地图中显示的地图：
因此，要在 RViz 中实现这一点，您必须：
- 使用插件在 RViz 中表示 GPS 当前位置：此插件允许您根据 /gps/fix 主题中发布的 GPS 数据将机器人定位在地球上相应的真实位置：这是插件：https://github.com/gareth-cross/rviz_satellite。
- 将插件下载到您的工作区并使用以下命令进行编译：
cd ~/catkin_ws/src git clone https://github.com/nobleo/rviz_satellite.git cd rviz_satellite git checkout 3.0.1 cd ~/catkin_ws catkin_make source devel/setup.bash编译完成后，再次运行RViz：
rviz现在，您需要配置 RViz 以正确显示地图。您可以将 RViz 配置文件复制到已在以下路径中为您设置的工作区：/home/simulations/public_sim_ws/src/all/summit_xl/sumit_xl_tools/rviz_config/gps.rviz
cp /home/simulations/public_sim_ws/src/all/summit_xl/sumit_xl_tools/rviz_config/gps.rviz ~/catkin_ws/src/现在，在 RViz 中，加载此 gps.rviz 配置文件并将固定坐标更改为 summit_xl_a_base_link。您将看到类似以下内容：
- 或者，您也可以手动设置，如下所示。首先，将固定坐标设置为 summit_xl_a_base_link。
然后，将AerialMapDisplay显示添加到RViz。
在 AerialMapDisplay 选项中，设置发布 GPS 数据的主题（即：/gps/fix），并设置应获取地图图块的 URL。此 URL 必须具有以下结构：
http://a.tiles.mapbox.com/v4/mapbox.satellite/{z}/{x}/{y}.jpg?access_token=YOUR_ACCESS_TOKEN这里唯一需要更改的是 YOUR_ACCESS_TOKEN，它由 MapBox 提供。您可以在此处获取：https://www.mapbox.com/
至于机器人固定坐标，summit_xl_a_odom 是一个不错的选择。
还请注意，您必须选择地图坐标作为固定坐标，才能查看机器人何时在地图上移动。
如果一切顺利，您应该会看到类似以下内容：
最后，记得将您的 RViz 配置保存为 gps.rviz。
Exercise 3.2
尝试 RViz AerialMapDisplay 的不同设置，看看它如何改变其可视化效果。
END Exercise 3.2
如何将 GPS 坐标转换为位姿数据
这非常重要，因为您需要能够将位姿发送到 move_base 节点才能移动机器人。但通常情况下，当您在户外移动时，您需要 GPS 坐标。因此，您必须使用 geonav_transform 包。
此包允许您根据您设置的 GPS 原点将纬度和经度转换为 XYZ 坐标。在本例中，GPS 原点位于：
- olat = 49.9 olon = 8.9
gps_to_xyz.py
#!/usr/bin/env python # Import geonav tranformation module import geonav_transform.geonav_conversions as gc reload(gc) # Import AlvinXY transformation module import alvinxy.alvinxy as axy reload(axy) import rospy import tf from nav_msgs.msg import Odometrydef get_xy_based_on_lat_long(lat,lon, name):# Define a local orgin, latitude and longitude in decimal degrees# GPS Originolat = 49.9olon = 8.9xg2, yg2 = gc.ll2xy(lat,lon,olat,olon)utmy, utmx, utmzone = gc.LLtoUTM(lat,lon)xa,ya = axy.ll2xy(lat,lon,olat,olon)rospy.loginfo("######### "+name+" ###########") rospy.loginfo("LAT COORDINATES ==>"+str(lat)+","+str(lon)) rospy.loginfo("COORDINATES XYZ ==>"+str(xg2)+","+str(yg2))rospy.loginfo("COORDINATES AXY==>"+str(xa)+","+str(ya))rospy.loginfo("COORDINATES UTM==>"+str(utmx)+","+str(utmy))return xg2, yg2if __name__ == '__main__':rospy.init_node('gps_to_xyz_node')xg2, yg2 = get_xy_based_on_lat_long(lat=49.9,lon=8.9, name="MAP")xg2, yg2 = get_xy_based_on_lat_long(lat=50.9,lon=8.9, name="MAP")请注意，根据使用的模型，转换方式会有所不同。您将在此处使用标准 geonav 变换 gn。但您也可以使用 AlvinXY 转换器（这是一种简单的直线转换）或获取 UTM 值。
查看两个测试的输出。只需更改纬度的一个单位，米数的影响就非常大。这是显而易见的，因为两条纬线之间的距离约为 111 公里。请注意，在 AvinXY 中，结果正是：111227.305636 米或 111.227305636 公里。GN 并非完全如此，因为它考虑了更多因素。
[INFO] [WallTime: 1505572823.286398] [0.000000] ######### MAP ########### [INFO] [WallTime: 1505572823.286697] [0.000000] LAT COORDINATES ==>49.9,8.9 [INFO] [WallTime: 1505572823.286876] [0.000000] COORDINATES XYZ ==>0.0,0.0 [INFO] [WallTime: 1505572823.287080] [0.000000] COORDINATES AXY==>0.0,0.0 [INFO] [WallTime: 1505572823.287267] [0.000000] COORDINATES UTM==>492818.438732,5527517.13624 [INFO] [WallTime: 1505572823.287644] [0.000000] ######### MAP ########### [INFO] [WallTime: 1505572823.287837] [0.000000] LAT COORDINATES ==>50.9,8.9 [INFO] [WallTime: 1505572823.289169] [0.000000] COORDINATES XYZ ==>149.529522407,111192.232732 [INFO] [WallTime: 1505572823.289821] [0.000000] COORDINATES AXY==>0.0,111227.305636 [INFO] [WallTime: 1505572823.290013] [0.000000] COORDINATES UTM==>492967.968254,5638709.36898Exercise 3.3
现在，创建一个程序，将机器人移动到空间中的三个不同 GPS 位置。步骤如下：
- 创建 GPS 到位姿转换器：这可以通过复制您刚刚学到的如何使用 GPS 到坐标转换器 gps_to_xyz.py 来完成。
- 创建 move_base 节点的客户端：此客户端将负责将目标位姿发送到 move_base 操作服务器。创建一个系统，让您知道它何时到达目的地。
请记住，您的原点与示例不同。GPS 的原点是：
- origin latitude = 39.5080331
- origin longitude = -0.4619816
END Exercise 3.3
这里有一个可能有用的 move_base 客户端示例。
move_base_client.py
#! /usr/bin/env pythonimport rospy import time import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult, MoveBaseFeedback""" 定义了一个简单的目标状态类，用于表示目标的不同状态 class SimpleGoalState:PENDING = 0 # 目标等待中ACTIVE = 1 # 目标激活中DONE = 2 # 目标完成WARN = 3 # 警告状态ERROR = 4 # 错误状态 """# 我们创建了一些常量，表示 SimpleGoalState 类中的相应值 PENDING = 0 ACTIVE = 1 DONE = 2 WARN = 3 ERROR = 4""" /move_base/goal ### PYTHON MESSAGErosmsg show move_base_msgs/MoveBaseGoal geometry_msgs/PoseStamped target_pose std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w /move_base/cancel /move_base/cmd_vel /move_base/current_goal /move_base/feedback """# 定义反馈回调函数。当从动作服务器接收到反馈时，会调用此函数 # 它只是打印一条消息，表示接收到新的反馈消息 def feedback_callback(feedback):rospy.loginfo(str(feedback))# 初始化动作客户端节点 rospy.init_node('move_base_gps_node')action_server_name = '/move_base' client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)# 等待动作服务器启动 rospy.loginfo('等待动作服务器 '+action_server_name) client.wait_for_server() rospy.loginfo('找到动作服务器...'+action_server_name)# 创建一个目标并发送到动作服务器 goal = MoveBaseGoal()goal.target_pose.header.frame_id = "/map" # 设置目标的坐标系 goal.target_pose.header.stamp = rospy.get_rostime() # 设置时间戳 goal.target_pose.pose.position.x = 0.0 # 设置目标位置的 x 坐标 goal.target_pose.pose.orientation.z = 0.0 # 设置目标姿态的 z 分量 goal.target_pose.pose.orientation.w = 1.0 # 设置目标姿态的 w 分量client.send_goal(goal, feedback_cb=feedback_callback) # 发送目标并设置反馈回调函数# 可以访问 SimpleAction 变量 "simple_state"，当状态为 1 时表示激活中，2 表示完成 # 这是一个变量，最好使用像 get_state 这样的函数来获取状态 # state = client.simple_state # state_result 将给出最终状态。激活时为 1，完成且无错误时为 2，有警告时为 3，有错误时为 4 state_result = client.get_state()rate = rospy.Rate(1) # 设置循环频率为 1 Hzrospy.loginfo("当前状态: "+str(state_result))while state_result < DONE:rospy.loginfo("等待服务器返回结果期间执行其他操作....")rate.sleep()state_result = client.get_state()rospy.loginfo("当前状态: "+str(state_result))rospy.loginfo("[结果] 状态: "+str(state_result)) if state_result == ERROR:rospy.logerr("服务器端出现问题") if state_result == WARN:rospy.logwarn("服务器端出现警告")# rospy.loginfo("[结果] 状态: "+str(client.get_result())) # 可以打印最终结果
