google cartographer参数配置和话题转发

为了对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

需要配置自己的.lua文件

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设置为真表示实时的闭环检测！

ROSLAUNCH文件

<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()

最终效果如下：