Problem performing 3D SLAM using cartographer

Hi!

I am using Vortex Studio as my simulation environment, and have followed this tutorial https://vortexstudio.atlassian.net/wiki/spaces/VSD221/pages/3234199789/ROS2+Tutorial+1+Running+the+Turtlebot3+Nav2+demo on how to perform SLAM and use Nav2 along with Vortex Studio.

I am trying to perform now a 3d SLAM on my own robot using Cartographer, so I configured the following .lua file:

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "Body",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_odometry = true,
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 = 160

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

However, I have encountered some problems:
First of all, I changed the Rooth frame id of the ROS2 module from “map” to “odom”, as I advertised in the example I provided. So my TF tree is “odom”->“Body”->Rest of the parts of the robot. At first I didn’t get why should I do this, but if I don’t change it, it doesn’t work in the example. However, I don’t get where the “map” frame declared in the map_frame field comes from. And I think the issue I have could be related with it.

The map I am trying to build is shown in the Top-left corner of the image attached.

If I launch Rviz with just the simulation launched, it can be seen that my “odom” frame has the z-axis in the same direction as the “Body” of the robot. And the TF tree is just “odom”->“Body”->rest of the parts. (Right-Top part of the image attached)

However, when I launch the cartographer with the configuration of the .lua file mentioned above, I get what is now shown in the Down-Left corner.

The plane where the map is being drawed is different from the one of the robot, whose coordinate frame appears rotated. Now my TF tree is “map”->“odom”->“Body”->rest of the parts. (I don’t know how to change the orientation of that “map” frame it appears because of the .lua configuration!). Look to the Down-Right corner.

I have asked in other forums, but I didn’t get any answer… Hope you can provide me with the solution or at least a hint of what the problem could be!