为了对google cartographer进行实验仿真：
安装完成后首先用官方rosbag进行实验没问题后再尝试用自己的rosbag文件！
重要的参考资料：
https://google-cartographer-ros.readthedocs.io/en/latest/ros_api.html
https://google-cartographer-ros.readthedocs.io/en/latest/configuration.html
https://www.guyuehome.com/19781
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = true,
use_pose_extrapolator = true,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 50e-3,
trajectory_publish_period_sec = 50e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.min_range=0.1
TRAJECTORY_BUILDER_2D.max_range=16.0
TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.05
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
return options重要参数解释：
map_frame 创建的地图参考系地址默认map
tracking_frame 根据自己情况选择，如果有imu最好选择imu_link，其他情况可以选择base_link
published_frame发布主题的参考系，因为我的bag文件里odom系零点与map不一样，我这里选择相对odom
odom_frame:这里设置为odom，如果provide_odom_frame设置成真就会出现警告，Ignoring transform from authority "unknown_publisher" with frame_id and child_frame_id "odom" because they are the same 因此我这里设置成false
publish_frame_projected_to_2d = true意味着只发布二维pose，没有俯仰角度等
我这里使用laserscan和odom因此设置为真，use_nav_sat表示GPS定位，use_landmarks我这里没有
num_laserscans=1表示只有一个雷达，echoes回波雷达没有
samplling_ratio表示各个数据可信度的权重，可以根据自己的情况调整！
TRAJECTORY_BUILDER_2D中的一些参数根据自己的传感器和需求进行调整，use_online_correlative_scan_matching设置为真表示实时的闭环检测！
<launch>
<arg name = "rosbag_file" value = "/home/x/xxx.bag"/>
<!--machine name="local_alt" address="localhost" default="true" /-->
<param name="/use_sim_time" value="true"/>
<!--node name="stage_ros" pkg="stage_ros" type="stageros" args="$(find stage_ros)/xxxx.world" /-->
<node name="player" pkg="rosbag" type="play" args=" -r 1.0 $(arg rosbag_file)" cwd="node" required="true"/>
<!--param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" /-->
<!--node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /-->
<node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<node name="cartographer_node" pkg="cartographer_ros" type="cartographer_node" args=" -configuration_directory $(find cartographer_ros)/configuration_files -configuration_basename cartographer.lua" output="screen">
<!--remap from="scan" to="/stage/xx/ranger_0/laserscan" /-->
<!--remap from="odom" to="/stage/xx/odometry"/-->
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros" type="cartographer_occupancy_grid_node" args="-resolution 0.1" />
</launch>如果用到urdf文件的话这需要启用robot state publisher
由于本人的bag文件中的坐标系名称不符合实际需要，需要进行修改，因为启用另一个python节点进行转发
#!/usr/bin/env python
import rospy
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped, Twist
from sensor_msgs.msg import LaserScan
from tf2_msgs.msg import TFMessage
import tf2_ros
def tf_frame(x):
return {
'/stage/xx/odom': 'odom',
'/stage/xx/base_footprint': 'base_footprint',
'/stage/xx/base_link':'base_link',
'/stage/xx/base_laser_link':'base_laser_link'
}.get(x,x)
def tf_callback(msg):
tfs = []
for tf in msg.transforms:
t = TransformStamped()
t.header = tf.header
#print 'tf',tf.header.frame_id
#print 'tf2',tf.child_frame_id
t.header.frame_id = tf_frame(tf.header.frame_id)
t.child_frame_id = tf_frame(tf.child_frame_id)
t.transform=tf.transform
tfs.append(t)
tfm = TFMessage(tfs)
pub_tf.publish(tfm)
def odom_callback(msg):
odo = Odometry()
odo.header = msg.header
#print 'odom', msg.header.frame_id
odo.header.frame_id=tf_frame(msg.header.frame_id)
odo.child_frame_id = 'base_link'
odo.pose = msg.pose
odo.twist = msg.twist
pub_odom.publish(odo)
def ls_callback(msg):
ls = LaserScan()
ls.header = msg.header
#print 'laserscan',msg.header.frame_id
ls.header.frame_id = tf_frame(msg.header.frame_id)
ls.angle_min = msg.angle_min
ls.angle_max = msg.angle_max
ls.angle_increment = msg.angle_increment
ls.range_min = msg.range_min
ls.range_max = msg.range_max
ls.ranges = msg.ranges
ls.intensities = msg.intensities
pub_ls.publish(ls)
if __name__ == '__main__':
rospy.init_node('broadcaster_for_cartographer')
#br = tf2_ros.TransformBroadcaster()
pub_tf = rospy.Publisher('/tf', TFMessage, queue_size=1)
rospy.Subscriber('/tf', TFMessage, tf_callback,queue_size=1)
pub_odom = rospy.Publisher('odom', Odometry, queue_size=1)
rospy.Subscriber('/stage/xx/odometry', Odometry, odom_callback,queue_size=100)
pub_ls = rospy.Publisher('scan', LaserScan, queue_size=1)
rospy.Subscriber('/stage/xx/ranger_0/laserscan', LaserScan, ls_callback,queue_size=100)
rospy.spin()最终效果如下：
