功能包下载
首先是安装功能包,官方现在默认是ros2的版本,所以需要自己选择合适的版本。实际使用发现
官方版本地址:
https://github.com/swri-robotics/mapviz.git
这是用于ros2的foxy使用,然后master的branch适合ubuntu20的noetic版本。我用的就是这个版本的
接下来安装依赖项:
sudo apt-get install ros-$ROS_DISTRO-mapviz ros-$ROS_DISTRO-mapviz-plugins ros-$ROS_DISTRO-tile-map ros-$ROS_DISTRO-multires-image
然后编译,ros1中使用catkin_make,ros2中使用colcon build
启动程序
启动mapviz中的launch文件即可:
source devel/setup.bash
roslaunch mapviz mapviz.launch
这里面主要是关于显示的一些配置,我修改的主要是初始的定位:
<launch>
<node pkg="mapviz" type="mapviz" name="mapviz"></node>
<node pkg="swri_transform_util" type="initialize_origin.py" name="initialize_origin" >
<param name="local_xy_frame" value="/map"/>
<param name="local_xy_origin" value="swri"/>
<rosparam param="local_xy_origins">
[{ name: swri,
latitude: 38.9973678,
longitude: 117.2978343,
altitude: 0.38214111328,
heading: 0.0},
{ name: back_40,
latitude: 38.9973678,
longitude: 117.2978343,
altitude: 0.38299942016499,
heading: 0.0}]
</rosparam>
</node>
<node pkg="tf" type="static_transform_publisher" name="swri_transform" args="0 0 0 0 0 0 /map /origin 100" />
</launch>
也就是将我的位置的大致的经纬度写入launch中。
创建tile_map
添加一下我的卫星地图(类似于rviz添加话题)
接下来选择source:
选择常规地图资源
然后输入URL:
这个url主要从天图获取API,可以去天图注册一下:
官网:
https://sso.tianditu.gov.cn/login?service=https%3A%2F%2Fconsole.tianditu.gov.cn%2Fapi%2Fkey
创建自己的api即可。
接下来需要填写对应格式的url,我这里直接提供几个可以使用的URL
http://t0.tianditu.gov.cn/img_w/wmts?SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=1cfe06711bc22d31762c8884d41d68f0
http://t0.tianditu.gov.cn/img_w/wmts?SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=375f936acc7c22013f7b7b96559094a7
此时启动launch应该就能显示地图了:
显示轨迹
显示车辆轨迹需要标准的gps的数据格式,也就是发布sensor_msgs/NavSatFix格式的数据
接下来我们自己创建一下这个轨迹,首先需要找到的是起点的经纬度,然后根据这个经纬度推算UTM坐标,再以此为基准建立一条轨迹,然后再把这条轨迹以经纬度的格式发布出来即可实现整个过程。
参考程序pub_gps.py:
#!/usr/bin/env python3
'''
Author: CYUN && cyun@tju.enu.cn
Date: 2024-12-13 20:12:55
LastEditors: CYUN && cyun@tju.enu.cn
LastEditTime: 2024-12-13 20:22:35
FilePath: /undefined/home/cyun/learn_ws/mapviz_ws/src/mapviz/mapviz/scrips/pub_gps.py
Description:
Copyright (c) 2024 by Tianjin University, All Rights Reserved.
'''
import rospy
from sensor_msgs.msg import NavSatFix
import utm
import math
# 假设天津的一个经纬度坐标示例(你可以替换为具体准确的坐标)
latitude = 38.9973678 # 纬度
longitude = 117.2978343 # 经度
# 定义生成轨迹的点数
num_points = 1000
# 定义轨迹点之间的距离间隔(单位:米,这里只是示例值,可按需调整)
distance_interval = 1
# 用于将经纬度转换为UTM坐标
def latlon_to_utm(lat, lon):
return utm.from_latlon(lat, lon)
# 用于将UTM坐标转换回经纬度
def utm_to_latlon(easting, northing, zone_number, zone_letter):
return utm.to_latlon(easting, northing, zone_number, zone_letter)
# 基于起始坐标生成轨迹(在UTM坐标下简单模拟,这里是直线轨迹示例,可按需扩展更复杂逻辑)
def generate_utm_trajectory(lat, lon, num_points, distance_interval):
utm_start = latlon_to_utm(lat, lon)
zone_number = utm_start[2]
zone_letter = utm_start[3]
utm_trajectory = []
x = utm_start[0]
y = utm_start[1]
for _ in range(num_points):
utm_trajectory.append((x, y, zone_number, zone_letter))
# 简单地在x方向上按照距离间隔增加,模拟轨迹移动(可按实际需求调整轨迹生成逻辑)
x += distance_interval
return utm_trajectory
# 将UTM轨迹转换回经纬度轨迹
def utm_trajectory_to_latlon(utm_trajectory):
latlon_trajectory = []
for point in utm_trajectory:
lat, lon = utm_to_latlon(point[0], point[1], point[2], point[3])
latlon_trajectory.append((lat, lon))
return latlon_trajectory
# 模拟发布经纬度轨迹信息(通过sensor_msgs/NavSatFix消息类型,实际需在ROS环境运行)
def publish_latlon_trajectory(latlon_trajectory):
rospy.init_node('trajectory_publisher', anonymous=True)
pub = rospy.Publisher('trajectory', NavSatFix, queue_size=10)
rate = rospy.Rate(10) # 1Hz的发布频率,可按需调整
for lat, lon in latlon_trajectory:
navsatfix_msg = NavSatFix()
navsatfix_msg.latitude = lat
navsatfix_msg.longitude = lon
navsatfix_msg.altitude = 0.0 # 这里简单设为0,可按实际补充准确高度信息
pub.publish(navsatfix_msg)
rate.sleep()
if __name__ == "__main__":
utm_trajectory = generate_utm_trajectory(latitude, longitude, num_points, distance_interval)
latlon_trajectory = utm_trajectory_to_latlon(utm_trajectory)
publish_latlon_trajectory(latlon_trajectory)
rospy.spin()
然后添加轨迹显示:
话题是/trajectory,类型是navsat的,然后勾选上se latest transtorm。
正常显示轨迹。这个软件还可以显示相机画面,雷达数据等等,感觉还是很不错的。
除了mapviz之外,还有一种在rviz中显示地图的工具——rviz_satelite,很明显这是一个rviz的插件工具,实际使用下来其实并不咋样(主要是网络连接不上去),这里也记录一下怎么安装和使用。
首先是安装:
git clone https://github.com/nobleo/rviz_satellite
这个包里面的配置文件demo.gps:
header:
seq: 999
stamp:
secs: 1435607489
nsecs: 924811809
frame_id: base_link
status:
status: 0
service: 1
latitude: 38.9973678
longitude: 117.2978343
altitude: 0.551
position_covariance: [3.9561210000000004, 0.0, 0.0, 0.0, 3.9561210000000004, 0.0, 0.0, 0.0, 7.650756]
position_covariance_type: 2
主要是将当地的经纬度和时间戳修改一下。
然后就可以启动了:
roslaunch rviz_satellite demo.launch
然后需要修改rviz的URI接口:
OpenStreetMap: https://tile.openstreetmap.org/{z}/{x}/{y}.png
TomTom: https://api.tomtom.com/map/1/tile/basic/main/{z}/{x}/{y}.png?tileSize=512&key=[TOKEN]
Mapbox: https://api.mapbox.com/styles/v1/mapbox/satellite-v9/tiles/256/{z}/{x}/{y}?access_token=[TOKEN]
GoogleMaps: https://maps.googleapis.com/maps/api/staticmap?maptype=satellite¢er={lat},{lon}&zoom={z}&size=256x256&key=[TOKEN]
以上是几种常用的接口,我试了一下,还都比较麻烦,最后还是用openstreetmap的:
https://tile-a.openstreetmap.fr/hot/{z}/{x}/{y}.png
https://tile-b.openstreetmap.fr/hot/{z}/{x}/{y}.png
https://tile-c.openstreetmap.fr/hot/{z}/{x}/{y}.png
https://tile.openstreetmap.fr/hot/{z}/{x}/{y}.png
http://{s}.tile.osm.org/{z}/{x}/{y}.png
试试这几种,感觉最后效果都一般,没有mapviz好用。
只能一点点显示。