正确的TF关系map----odom----base_link----laser
base_link是固定在机器人本体上的坐标系,通常选择飞控
其中map–odom 的链接是由cartographer中lua文件配置完成的
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
base_link----laser的链接是在启动rplidar节点的时候有tf_static完成的
<node pkg="tf" name="tf_lidar" type="static_transform_publisher" args="0 0 0 0 0 0 base_link laser 10"/>
下面分两种情况在剖析TF关系的变化
1、cartographer不使用px4飞控的imu消息 /mavros/imu/data
因为在建图过程中没有引入px4所以base_link,TF关系中暂时没有加入base_link
lua中配置关系,cartographer追踪的的laser的位置,发布pos使用的也是laser的id。但是此时有一个问题,节点中报错**Could not compute submap fading: Could not find a connection between ‘map’ and ‘base_link’ because they are not part of the same tree.Tf has two or more unconnected trees.**base_link和map链接关系是断开的,地图中飞机的位置是不动的。
map_frame = "map",
tracking_frame = "laser",
published_frame = "laser",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
TRAJECTORY_BUILDER_2D.use_imu_data = false
需要单独做一下base_link和odom的静态链接,在通过监听laser和odom的tf关系就可以把定位信息发送给px4了。
rosrun tf static_transform_publisher 0 0 0 0 0 0 /odom /base_link 100
2、cartographer使用px4飞控的imu消息 /mavros/imu/data
因为激光雷达和飞控属于固连关系,在启动rplidar节点时建立tf静态关联
<node pkg="tf" name="tf_lidar" type="static_transform_publisher" args="0 0 0 0 0 0 base_link laser 10"/>
此时,laser的位置信息同步到了base_link,所以cartographer只需要去追踪base_link即可。在通过监听base_link和odom的tf关系就可以把定位信息发送给px4了。
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_pose_extrapolator = true,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
TRAJECTORY_BUILDER_2D.use_imu_data = true
还有第三种情况
机器人中本身存在 odom 坐标系, 比如里程计或者vins。而 provide_odom_frame 又设置成了 true, 就会导致 odom 坐标系重复发布,会导致机器人位姿发生来回的跳动。
map_frame:一般为“map”.用来发布submap的ROS帧ID.
tracking_frame:SLAM算法要跟踪的ROS 帧ID.?
published_frame:用来发布pose的帧ID.?
odom_frame:只要当有里程计信息的时候才会使用。即provide_odom_frame=true.
provide_odom_frame:如果为true,the local, non-loop-closed, continuous pose将会在map_frame里以odom_frame发布?
publish_frame_projected_to_2d:如果为true,则已经发布的pose将会被完全成2D的pose,没有roll,pitch或者z-offset?
use_odometry:如果为true,需要提供里程计信息,并话题/odom会订阅nav_msgs/Odometry类型的消息,在SLAM过程中也会使用这个消息进行建图
use_nav_sat:如果为true,会在话题/fix上订阅sensor_msgs/NavSatFix类型的消息,并且在globalSLAM中会用到
use_landmarks:如果为true,会在话题/landmarks上订阅cartographer_ros_msgs/LandmarkList类型的消息,并且在SLAM过程中会用到
num_laser_scans:SLAM可以输入的/scan话题数目的最大值
num_muti_echo_laser_scans:SLAM可以输入sensor_msgs/MultiEchoLaserScan话题数目的最大值
num_subdivisions_per_laser_scan:将每个接收到的(multi_echo)激光scan分割成的点云数。 细分scam可以在扫描仪移动时取消scanner获取的scan。 有一个相应的trajectory builder option可将细分扫描累积到将用于scan_matching的点云中。
num_point_clouds:SLAM可以输入的sensor_msgs/PointCloud2话题数目的最大值
lookup_transform_timeout_sec:使用tf2查找transform的超时时间(秒)。
submap_publish_period_sec:发布submap的时间间隔(秒)
pose_publish_period_sec:发布pose的时间间隔,值为5e-3的时候为200HZ
trajectory_publish_period_sec:发布trajectory markers(trajectory的节点)的时间间隔,值为30e-3为30ms
rangefinder_samping_ratio:测距仪的固定采样ratio
imu_sampling_ratio:IMU message的固定采样ratio
landmarks_sampling_ratio:landmarks message的固定采样ratio,(单位是啥?)