How Cartographer publishes Map to base_link transform?
I am using google's cartographer to build a map, but I am unable to understand how it publishes map->base_link
transform. I am providing cartographer with imu
and velodyne VLP-16
LIDAR data, here is the result of rosbag info
:
path: out2.bag
version: 2.0
duration: 1:19s (79s)
start: Aug 08 2019 07:25:58.85 (1565229358.85)
end: Aug 08 2019 07:27:18.42 (1565229438.42)
size: 1.9 GB
messages: 29535
compression: none [1855/1855 chunks]
types: sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics: /imu 26871 msgs : sensor_msgs/Imu
/points2 2664 msgs : sensor_msgs/PointCloud2
The issue I am facing is that the robot remains close to its original position and rotates, and sometimes it just shifts its position, the map->base_link
transform does not seem to be continuous
Here are .lua
and launch
files:
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "imu_link",
published_frame = "base_link",
odom_frame = "odom",
provide_odom_frame = true,
publish_frame_projected_to_2d = false,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 0,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 1,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 100
MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
return options
launch file:
<launch>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_stack)/rviz/newCarto.rviz"/>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
<param name="robot_description" command="$(find xacro)/xacro '$(find dlive_description)/urdf/dlive_low_poly.xacro'" />
<node name="cartographer_offline_node" pkg="cartographer_ros"
type="cartographer_offline_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files/trials
-configuration_basenames dlive_offline.lua
-urdf_filenames $(find cartographer_ros)/urdf/dlive_3d.urdf
-bag_filenames $(arg bag_filenames)"
output="screen">
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>
Here is the trajectory generated:
The trajectory should be partitioned rectangles though, also the length of each of these lines is around 1m it should be in 100m
I installed cartographer ros from the official documentation here, and I am running ubuntu 16.04 with ROS kinetic.
If I can understand how map->base_link
transform is generated I should be able to understand reason behind the issues I am getting