`offboard_control` causes immediate blind land — EKF2 converges on vision data but drone lands down immediately or drone crashes

[thagachess]

Branch: ros2 | Commit: 9cf1089 | Setup: Fresh Docker image rebuilt from .devcontainer/humble/Dockerfile, VSCode devcontainer, Ubuntu, no GPU (AMD)

Could you help me in resolving this

What I did

Terminal 1 — Build and launch PX4 + Gazebo:

cd ~/ros2_ws
cp -r /workspaces/rtabmap_drone_example src/
colcon build --symlink-install
source install/setup.bash
cd ~/PX4-Autopilot
make px4_sitl gz_x500_depth_apt

Terminal 2 — XRCE DDS Agent:

docker exec -it drone_sim bash
MicroXRCEAgent udp4 -p 8888

Terminal 3 — ROS2 bridge:

docker exec -it drone_sim bash
source ~/ros2_ws/install/setup.bash
ros2 launch rtabmap_drone_example ros2_bridge.launch.py

Terminal 4 — Offboard control (after Terminal 1 says Ready for takeoff!):

docker exec -it drone_sim bash
source ~/ros2_ws/install/setup.bash
ros2 run rtabmap_drone_example offboard_control --ros-args -p use_sim_time:=true

---

What Happens

Terminal 1 — PX4 output (every attempt):

WARN  [health_and_arming_checks] Preflight Fail: No valid data from Baro 0
WARN  [health_and_arming_checks] Preflight Fail: ekf2 missing data
WARN  [health_and_arming_checks] Preflight Fail: High Gyro Bias
WARN  [health_and_arming_checks] Preflight Fail: heading estimate not stable
WARN  [timesync] time jump detected. Resetting time synchroniser.
WARN  [uxrce_dds_client] time sync no longer converged
INFO  [uxrce_dds_client] time sync converged
INFO  [commander] Ready for takeoff!
INFO  [commander] Armed by external command
WARN  [mc_pos_control] invalid setpoints
WARN  [mc_pos_control] Failsafe: blind land
WARN  [failsafe] Failsafe activated
ERROR [flight_mode_manager] Matching flight task was not able to run, Nav state: 2, Task: 1
INFO  [commander] Landing detected
INFO  [commander] Disarmed by landing

Terminal 4 — offboard_control output:

Offboard could not be set, make sure the vehicle position is published
or check if there are some preflight checks failing... will try again in 1 second.
Offboard could not be set, make sure the vehicle position is published
or check if there are some preflight checks failing... will try again in 1 second.

---

Diagnostic Output

ros2 topic list | grep vehicle_local_position

/fmu/out/vehicle_local_position

Topic is correct, not _v1. The 9cf1089 rename fix is present. This is not a topic name issue.

ros2 topic echo /fmu/out/vehicle_local_position --once

xy_valid: true
z_valid: true
dead_reckoning: true
xy_global: false
z_global: false
heading_var: 0.014
heading_good_for_control: true

dead_reckoning: true means EKF2 is running on IMU only — vision is never fused. heading_var never stabilizes below 0.01.

ros2 topic echo /fmu/out/vehicle_status_v1 --once

arming_state: 1
pre_flight_checks_pass: false
nav_state: 14
gcs_connection_lost: true

ros2 node info /offboard_control

Subscribers:
  /fmu/out/vehicle_local_position: px4_msgs/msg/VehicleLocalPosition
  /fmu/out/vehicle_status_v1: px4_msgs/msg/VehicleStatus
Publishers:
  /fmu/in/offboard_control_mode: px4_msgs/msg/OffboardControlMode
  /fmu/in/trajectory_setpoint: px4_msgs/msg/TrajectorySetpoint
  /fmu/in/vehicle_command: px4_msgs/msg/VehicleCommand

pxh> commander check

INFO  [commander] Preflight check: FAILED

pxh> commander status

INFO  [commander] Disarmed
INFO  [commander] navigation mode: Offboard
INFO  [commander] in failsafe: no

---

[matlabbe]

I've followed the instructions in the README and here the expected outputs. You may compare them.

1. Launch simulator (from a new terminal in dev container after rebuilding the dev container from scratch):

cd ~/PX4-Autopilot
make px4_sitl gz_x500_depth_apt

Output:

~/ros2_ws/src/rtabmap_drone_example$ cd ~/PX4-Autopilot
~/PX4-Autopilot$ make px4_sitl gz_x500_depth_apt
 
[0/1] cd /home/vscode/PX4-Autopilot/build/px4_sitl_default/src/modules/simulat...=apt GZ_IP=127.0.0.1 /home/vscode/PX4-Autopilot/build/px4_sitl_default/bin/px4

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] startup script: /bin/sh etc/init.d-posix/rcS 0
INFO  [init] found model autostart file as SYS_AUTOSTART=4002
INFO  [param] selected parameter default file parameters.bson
INFO  [param] selected parameter backup file parameters_backup.bson
  SYS_AUTOCONFIG: curr: 0 -> new: 1
  SYS_AUTOSTART: curr: 0 -> new: 4002
  CAL_ACC0_ID: curr: 0 -> new: 1310988
  CAL_GYRO0_ID: curr: 0 -> new: 1310988
  CAL_ACC1_ID: curr: 0 -> new: 1310996
  CAL_GYRO1_ID: curr: 0 -> new: 1310996
  CAL_ACC2_ID: curr: 0 -> new: 1311004
  CAL_GYRO2_ID: curr: 0 -> new: 1311004
  CAL_MAG0_ID: curr: 0 -> new: 197388
  CAL_MAG0_PRIO: curr: -1 -> new: 50
  CAL_MAG1_ID: curr: 0 -> new: 197644
  CAL_MAG1_PRIO: curr: -1 -> new: 50
  SENS_BOARD_X_OFF: curr: 0.0000 -> new: 0.0000
  SENS_DPRES_OFF: curr: 0.0000 -> new: 0.0010
INFO  [dataman] data manager file './dataman' size is 1208528 bytes
INFO  [init] Gazebo simulator
INFO  [init] Starting gazebo with world: /home/vscode/PX4-Autopilot/Tools/simulation/gz/worlds/apt.sdf
INFO  [init] Starting gz gui
INFO  [init] Gazebo world is ready
INFO  [init] Spawning model
Warning [Utils.cc:132] [/sdf/model[@name="OakD-Lite"]/link[@name="camera_link"]/sensor[@name="IMX214"]/gz_frame_id:/home/vscode/PX4-Autopilot/Tools/simulation/gz/models/OakD-Lite/model.sdf:L38]: XML Element[gz_frame_id], child of element[sensor], not defined in SDF. Copying[gz_frame_id] as children of [sensor].
Warning [Utils.cc:132] [/sdf/model[@name="OakD-Lite"]/link[@name="camera_link"]/sensor[@name="StereoOV7251"]/gz_frame_id:/home/vscode/PX4-Autopilot/Tools/simulation/gz/models/OakD-Lite/model.sdf:L56]: XML Element[gz_frame_id], child of element[sensor], not defined in SDF. Copying[gz_frame_id] as children of [sensor].
libEGL warning: egl: failed to create dri2 screen
libEGL warning: egl: failed to create dri2 screen
INFO  [gz_bridge] world: apt, model: x500_depth_0
INFO  [commander] LED: open /dev/led0 failed (22)
INFO  [uxrce_dds_client] init UDP agent IP:127.0.0.1, port:8888
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2026-04-04/20_35_51.ulg
INFO  [logger] Opened full log file: ./log/2026-04-04/20_35_51.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
pxh> INFO  [lockstep_scheduler] setting initial absolute time to 1452000 us
INFO  [tone_alarm] home set
error: XDG_RUNTIME_DIR not set in the environment.
INFO  [uxrce_dds_client] synchronized with time offset 1775334950342193us
INFO  [uxrce_dds_client] successfully created rt/fmu/out/register_ext_component_reply data writer, topic id: INFO  [uxrce_dds_client] successfully created rt/fmu/out/arming_check_request data writer, topic id: 17
INFO  [uxrce_dds_client] successfully created rt/fmu/out/mode_completed data writer, topic id: 150
INFO  [uxrce_dds_client] successfully created rt/fmu/out/battery_status data writer, topic id: 20
INFO  [uxrce_dds_client] successfully created rt/fmu/out/collision_constraints data writer, topic id: 27
INFO  [uxrce_dds_client] successfully created rt/fmu/out/estimator_status_flags data writer, topic id: 85
INFO  [uxrce_dds_client] successfully created rt/fmu/out/failsafe_flags data writer, topic id: 91
INFO  [uxrce_dds_client] successfully created rt/fmu/out/manual_control_setpoint data writer, topic id: 141
INFO  [uxrce_dds_client] successfully created rt/fmu/out/message_format_response data writer, topic id: 147
INFO  [uxrce_dds_client] successfully created rt/fmu/out/position_setpoint_triplet data writer, topic id: 185INFO  [uxrce_dds_client] successfully created rt/fmu/out/sensor_combined data writer, topic id: 217
INFO  [uxrce_dds_client] successfully created rt/fmu/out/timesync_status data writer, topic id: 240
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_land_detected data writer, topic id: 268
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_attitude data writer, topic id: 255
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_control_mode data writer, topic id: 262
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_command_ack data writer, topic id: 259
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_global_position data writer, topic id: 263
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_gps_position data writer, topic id: 265
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_local_position data writer, topic id: 269
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_odometry data writer, topic id: 274
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vehicle_status_v1 data writer, topic id: 279
INFO  [uxrce_dds_client] successfully created rt/fmu/out/airspeed_validated data writer, topic id: 14
INFO  [uxrce_dds_client] successfully created rt/fmu/out/vtol_vehicle_status data writer, topic id: 288
INFO  [uxrce_dds_client] successfully created rt/fmu/out/home_position data writer, topic id: 123
INFO  [uxrce_dds_client] time sync converged
WARN  [health_and_arming_checks] Preflight Fail: heading estimate not stable
INFO  [commander] Ready for takeoff!
WARN  [timesync] time jump detected. Resetting time synchroniser.
WARN  [uxrce_dds_client] time sync no longer converged
INFO  [uxrce_dds_client] time sync converged
INFO  [commander] Armed by external command
INFO  [tone_alarm] arming warning
INFO  [commander] Takeoff detected
WARN  [timesync] time jump detected. Resetting time synchroniser.
WARN  [uxrce_dds_client] time sync no longer converged
INFO  [uxrce_dds_client] time sync converged
WARN  [timesync] time jump detected. Resetting time synchroniser.
WARN  [uxrce_dds_client] time sync no longer converged
INFO  [uxrce_dds_client] time sync converged
WARN  [timesync] time jump detected. Resetting time synchroniser.
WARN  [uxrce_dds_client] time sync no longer converged
INFO  [uxrce_dds_client] time sync converged

2. Second terminal:

ros2 launch rtabmap_drone_example ros2_bridge.launch.py

Output:

~/ros2_ws/src/rtabmap_drone_example$ ros2 launch rtabmap_drone_example ros2_bridge.launch.py

[INFO] [launch]: All log files can be found below /home/vscode/.ros/log/2026-04-04-20-37-26-714533-mathieu-R12-3069
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [imu_filter_madgwick_node-1]: process started with pid [3071]
[INFO] [imu_to_tf-2]: process started with pid [3073]
[INFO] [rgbd_sync-3]: process started with pid [3075]
[INFO] [rgbd_odometry-4]: process started with pid [3077]
[INFO] [rtabmap-5]: process started with pid [3079]
[INFO] [rviz2-6]: process started with pid [3081]
[INFO] [parameter_bridge-7]: process started with pid [3083]
[INFO] [static_transform_publisher-8]: process started with pid [3085]
[INFO] [static_transform_publisher-9]: process started with pid [3087]
[INFO] [static_transform_publisher-10]: process started with pid [3089]
[INFO] [ros_odometry_to_vehicle_odometry-11]: process started with pid [3091]
[INFO] [joy_node-12]: process started with pid [3093]
[INFO] [teleop_node-13]: process started with pid [3095]
[INFO] [point_cloud_xyz-14]: process started with pid [3097]
[INFO] [voxel_marker-15]: process started with pid [3099]
[INFO] [controller_server-16]: process started with pid [3101]
[INFO] [smoother_server-17]: process started with pid [3103]
[INFO] [planner_server-18]: process started with pid [3105]
[INFO] [behavior_server-19]: process started with pid [3107]
[INFO] [bt_navigator-20]: process started with pid [3109]
[INFO] [waypoint_follower-21]: process started with pid [3111]
[INFO] [velocity_smoother-22]: process started with pid [3113]
[INFO] [lifecycle_manager-23]: process started with pid [3115]
[static_transform_publisher-8] [WARN] [1775335046.889717253] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-9] [WARN] [1775335046.890286555] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-10] [WARN] [1775335046.890863651] []: Old-style arguments are deprecated; see --help for new-style arguments
[rviz2-6] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-vscode'
[lifecycle_manager-23] [INFO] [1775335046.937609161] [lifecycle_manager_navigation]: Creating
[static_transform_publisher-10] [INFO] [1775335046.944498355] [static_transform_publisher_g0Ls9U3I6pjVInmp]: Spinning until stopped - publishing transform
[static_transform_publisher-10] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-10] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-10] from 'base_link' to 'x500_depth_0/base_link/imu_sensor'
[lifecycle_manager-23] [INFO] [1775335046.951701605] [lifecycle_manager_navigation]: Creating and initializing lifecycle service clients
[imu_filter_madgwick_node-1] [INFO] [1775335046.965718611] [imu_filter_madgwick]: Starting ImuFilter
[imu_filter_madgwick_node-1] [INFO] [1775335046.965966373] [imu_filter_madgwick]: Using dt computed from message headers
[imu_filter_madgwick_node-1] [INFO] [1775335046.965981687] [imu_filter_madgwick]: The gravity vector is kept in the IMU message.
[imu_filter_madgwick_node-1] [INFO] [1775335046.966037383] [imu_filter_madgwick]: Imu filter gain set to 0.100000
[imu_filter_madgwick_node-1] [INFO] [1775335046.966050661] [imu_filter_madgwick]: Gyro drift bias set to 0.000000
[imu_filter_madgwick_node-1] [INFO] [1775335046.966055774] [imu_filter_madgwick]: Magnetometer bias values: 0.000000 0.000000 0.000000
[static_transform_publisher-9] [INFO] [1775335046.969627740] [static_transform_publisher_vopwyZy1FyvxrwVH]: Spinning until stopped - publishing transform
[static_transform_publisher-9] translation: ('0.000000', '0.000000', '0.140000')
[static_transform_publisher-9] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-9] from 'base_link' to 'middle'
[bt_navigator-20] [INFO] [1775335046.975593435] [bt_navigator]: 
[bt_navigator-20]       bt_navigator lifecycle node launched. 
[bt_navigator-20]       Waiting on external lifecycle transitions to activate
[bt_navigator-20]       See https://design.ros2.org/articles/node_lifecycle.html for more information.
[bt_navigator-20] [INFO] [1775335046.975713748] [bt_navigator]: Creating
[teleop_node-13] [INFO] [1775335046.980232240] [TeleopTwistJoy]: Teleop enable button 9.
[teleop_node-13] [INFO] [1775335046.980294405] [TeleopTwistJoy]: Linear axis x on 3 at scale 1.000000.
[teleop_node-13] [INFO] [1775335046.980302845] [TeleopTwistJoy]: Linear axis y on 2 at scale 0.500000.
[teleop_node-13] [INFO] [1775335046.980306604] [TeleopTwistJoy]: Linear axis z on 1 at scale 1.000000.
[teleop_node-13] [INFO] [1775335046.980310332] [TeleopTwistJoy]: Angular axis yaw on 0 at scale 1.000000.
[smoother_server-17] [INFO] [1775335046.988418081] [smoother_server]: 
[smoother_server-17]    smoother_server lifecycle node launched. 
[smoother_server-17]    Waiting on external lifecycle transitions to activate
[smoother_server-17]    See https://design.ros2.org/articles/node_lifecycle.html for more information.
[velocity_smoother-22] [INFO] [1775335046.989306687] [velocity_smoother]: 
[velocity_smoother-22]  velocity_smoother lifecycle node launched. 
[velocity_smoother-22]  Waiting on external lifecycle transitions to activate
[velocity_smoother-22]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[smoother_server-17] [INFO] [1775335046.991234844] [smoother_server]: Creating smoother server
[planner_server-18] [INFO] [1775335046.993763408] [planner_server]: 
[planner_server-18]     planner_server lifecycle node launched. 
[planner_server-18]     Waiting on external lifecycle transitions to activate
[planner_server-18]     See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-18] [INFO] [1775335046.994739139] [planner_server]: Creating
[static_transform_publisher-8] [INFO] [1775335047.001836998] [static_transform_publisher_37zVmfzUvxfKSy64]: Spinning until stopped - publishing transform
[static_transform_publisher-8] translation: ('0.120000', '0.030000', '0.242000')
[static_transform_publisher-8] rotation: ('-0.500000', '0.500000', '-0.500000', '0.500000')
[static_transform_publisher-8] from 'base_link' to 'camera_link'
[waypoint_follower-21] [INFO] [1775335047.002358730] [waypoint_follower]: 
[waypoint_follower-21]  waypoint_follower lifecycle node launched. 
[waypoint_follower-21]  Waiting on external lifecycle transitions to activate
[waypoint_follower-21]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[waypoint_follower-21] [INFO] [1775335047.003042361] [waypoint_follower]: Creating
[planner_server-18] [INFO] [1775335047.012890989] [global_costmap.global_costmap]: 
[planner_server-18]     global_costmap lifecycle node launched. 
[planner_server-18]     Waiting on external lifecycle transitions to activate
[planner_server-18]     See https://design.ros2.org/articles/node_lifecycle.html for more information.
[planner_server-18] [INFO] [1775335047.013458483] [global_costmap.global_costmap]: Creating Costmap
[behavior_server-19] [INFO] [1775335047.019307958] [behavior_server]: 
[behavior_server-19]    behavior_server lifecycle node launched. 
[behavior_server-19]    Waiting on external lifecycle transitions to activate
[behavior_server-19]    See https://design.ros2.org/articles/node_lifecycle.html for more information.
[lifecycle_manager-23] [INFO] [1775335047.025895316] [lifecycle_manager_navigation]: Starting managed nodes bringup...
[lifecycle_manager-23] [INFO] [1775335047.025947415] [lifecycle_manager_navigation]: Configuring controller_server
[controller_server-16] [INFO] [1775335047.026915883] [controller_server]: 
[controller_server-16]  controller_server lifecycle node launched. 
[controller_server-16]  Waiting on external lifecycle transitions to activate
[controller_server-16]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-16] [INFO] [1775335047.031970733] [controller_server]: Creating controller server
[controller_server-16] [INFO] [1775335047.063328232] [local_costmap.local_costmap]: 
[controller_server-16]  local_costmap lifecycle node launched. 
[controller_server-16]  Waiting on external lifecycle transitions to activate
[controller_server-16]  See https://design.ros2.org/articles/node_lifecycle.html for more information.
[controller_server-16] [INFO] [1775335047.064215608] [local_costmap.local_costmap]: Creating Costmap
[controller_server-16] [INFO] [1775335047.065624990] [controller_server]: Configuring controller interface
[controller_server-16] [INFO] [1775335047.065719266] [controller_server]: getting goal checker plugins..
[controller_server-16] [INFO] [1775335047.065780915] [controller_server]: Controller frequency set to 20.0000Hz
[controller_server-16] [INFO] [1775335047.065805526] [local_costmap.local_costmap]: Configuring
[controller_server-16] [INFO] [1775335047.072645500] [local_costmap.local_costmap]: Using plugin "voxel_layer"
[controller_server-16] [INFO] [1775335047.075980193] [local_costmap.local_costmap]: Subscribed to Topics: cloud
[controller_server-16] [INFO] [1775335047.082946359] [local_costmap.local_costmap]: Initialized plugin "voxel_layer"
[controller_server-16] [INFO] [1775335047.082988549] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[controller_server-16] [INFO] [1775335047.084605550] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[controller_server-16] [INFO] [1775335047.094290791] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[controller_server-16] [INFO] [1775335047.094811225] [controller_server]: Created goal checker : general_goal_checker of type nav2_controller::SimpleGoalChecker
[controller_server-16] [INFO] [1775335047.095195436] [controller_server]: Controller Server has general_goal_checker  goal checkers available.
[controller_server-16] [INFO] [1775335047.096709202] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[controller_server-16] [INFO] [1775335047.097171370] [controller_server]: Setting transform_tolerance to 0.200000
[controller_server-16] [INFO] [1775335047.106743498] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[controller_server-16] [INFO] [1775335047.106997421] [controller_server]: Critic plugin initialized
[controller_server-16] [INFO] [1775335047.107054687] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[controller_server-16] [INFO] [1775335047.107218994] [controller_server]: Critic plugin initialized
[controller_server-16] [INFO] [1775335047.107262651] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[controller_server-16] [INFO] [1775335047.107334798] [controller_server]: Critic plugin initialized
[controller_server-16] [INFO] [1775335047.107382886] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[controller_server-16] [INFO] [1775335047.107521688] [controller_server]: Critic plugin initialized
[controller_server-16] [INFO] [1775335047.107567616] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[controller_server-16] [INFO] [1775335047.107692561] [controller_server]: Critic plugin initialized
[controller_server-16] [INFO] [1775335047.107738505] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[controller_server-16] [INFO] [1775335047.107837921] [controller_server]: Critic plugin initialized
[controller_server-16] [INFO] [1775335047.107881759] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[controller_server-16] [INFO] [1775335047.107966815] [controller_server]: Critic plugin initialized
[controller_server-16] [INFO] [1775335047.107978764] [controller_server]: Controller Server has FollowPath  controllers available.
[lifecycle_manager-23] [INFO] [1775335047.119873067] [lifecycle_manager_navigation]: Configuring smoother_server
[smoother_server-17] [INFO] [1775335047.120058419] [smoother_server]: Configuring smoother server
[smoother_server-17] [INFO] [1775335047.134601498] [smoother_server]: Created smoother : simple_smoother of type nav2_smoother::SimpleSmoother
[smoother_server-17] [INFO] [1775335047.135247448] [smoother_server]: Smoother Server has simple_smoother  smoothers available.
[lifecycle_manager-23] [INFO] [1775335047.145151071] [lifecycle_manager_navigation]: Configuring planner_server
[planner_server-18] [INFO] [1775335047.145564216] [planner_server]: Configuring
[planner_server-18] [INFO] [1775335047.146191477] [global_costmap.global_costmap]: Configuring
[planner_server-18] [INFO] [1775335047.158547954] [global_costmap.global_costmap]: Using plugin "static_layer"
[planner_server-18] [INFO] [1775335047.164013212] [global_costmap.global_costmap]: Subscribing to the map topic (/map) with transient local durability
[planner_server-18] [INFO] [1775335047.168122106] [global_costmap.global_costmap]: Initialized plugin "static_layer"
[planner_server-18] [INFO] [1775335047.168153253] [global_costmap.global_costmap]: Using plugin "inflation_layer"
[planner_server-18] [INFO] [1775335047.172209124] [global_costmap.global_costmap]: Initialized plugin "inflation_layer"
[planner_server-18] [INFO] [1775335047.207308701] [planner_server]: Created global planner plugin GridBased of type nav2_navfn_planner/NavfnPlanner
[planner_server-18] [INFO] [1775335047.207346251] [planner_server]: Configuring plugin GridBased of type NavfnPlanner
[planner_server-18] [INFO] [1775335047.217311992] [planner_server]: Planner Server has GridBased  planners available.
[lifecycle_manager-23] [INFO] [1775335047.284228533] [lifecycle_manager_navigation]: Configuring behavior_server
[behavior_server-19] [INFO] [1775335047.285090713] [behavior_server]: Configuring
[imu_to_tf-2] [INFO] [1775335047.289412720] [imu_to_tf]: fixed_frame_id: base_link_stabilized
[imu_to_tf-2] [INFO] [1775335047.289591719] [imu_to_tf]: base_frame_id: base_link
[imu_to_tf-2] [INFO] [1775335047.289599416] [imu_to_tf]: qos: 0
[rgbd_sync-3] [INFO] [1775335047.290399336] [rtabmap.rgbd_sync]: rgbd_sync: approx_sync = false
[rgbd_sync-3] [INFO] [1775335047.290576149] [rtabmap.rgbd_sync]: rgbd_sync: topic_queue_size  = 10
[rgbd_sync-3] [INFO] [1775335047.290587885] [rtabmap.rgbd_sync]: rgbd_sync: sync_queue_size  = 10
[rgbd_sync-3] [INFO] [1775335047.290593449] [rtabmap.rgbd_sync]: rgbd_sync: qos             = 0
[rgbd_sync-3] [INFO] [1775335047.290598748] [rtabmap.rgbd_sync]: rgbd_sync: qos_camera_info = 0
[rgbd_sync-3] [INFO] [1775335047.290603788] [rtabmap.rgbd_sync]: rgbd_sync: depth_scale = 1.000000
[rgbd_sync-3] [INFO] [1775335047.290619910] [rtabmap.rgbd_sync]: rgbd_sync: decimation = 1
[rgbd_sync-3] [INFO] [1775335047.290625381] [rtabmap.rgbd_sync]: rgbd_sync: compressed_rate = 0.000000
[behavior_server-19] [INFO] [1775335047.324063648] [behavior_server]: Creating behavior plugin spin of type nav2_behaviors/Spin
[behavior_server-19] [INFO] [1775335047.324809532] [behavior_server]: Configuring spin
[rgbd_sync-3] [INFO] [1775335047.332122416] [rtabmap.rgbd_sync]: 
[rgbd_sync-3] rgbd_sync subscribed to (exact sync):
[rgbd_sync-3]    /camera/rgb/image,
[rgbd_sync-3]    /camera/depth/image,
[rgbd_sync-3]    /camera/rgb/camera_info
[rtabmap-5] [INFO] [1775335047.337208652] [rtabmap.rtabmap]: rtabmap(maps): latch                      = true
[rtabmap-5] [INFO] [1775335047.337407482] [rtabmap.rtabmap]: rtabmap(maps): map_filter_radius          = 0.000000
[rtabmap-5] [INFO] [1775335047.337424521] [rtabmap.rtabmap]: rtabmap(maps): map_filter_angle           = 30.000000
[rtabmap-5] [INFO] [1775335047.337430851] [rtabmap.rtabmap]: rtabmap(maps): map_cleanup                = true
[rtabmap-5] [INFO] [1775335047.337435863] [rtabmap.rtabmap]: rtabmap(maps): map_always_update          = false
[rtabmap-5] [INFO] [1775335047.337440763] [rtabmap.rtabmap]: rtabmap(maps): map_empty_ray_tracing      = true
[rtabmap-5] [INFO] [1775335047.337445254] [rtabmap.rtabmap]: rtabmap(maps): cloud_output_voxelized     = true
[rtabmap-5] [INFO] [1775335047.337449574] [rtabmap.rtabmap]: rtabmap(maps): cloud_subtract_filtering   = false
[rtabmap-5] [INFO] [1775335047.337454109] [rtabmap.rtabmap]: rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[rtabmap-5] [INFO] [1775335047.337508641] [rtabmap.rtabmap]: rtabmap(maps): octomap_tree_depth         = 16
[behavior_server-19] [INFO] [1775335047.357835107] [behavior_server]: Creating behavior plugin backup of type nav2_behaviors/BackUp
[behavior_server-19] [INFO] [1775335047.358537241] [behavior_server]: Configuring backup
[behavior_server-19] [INFO] [1775335047.372107740] [behavior_server]: Creating behavior plugin drive_on_heading of type nav2_behaviors/DriveOnHeading
[behavior_server-19] [INFO] [1775335047.372783799] [behavior_server]: Configuring drive_on_heading
[behavior_server-19] [INFO] [1775335047.384042603] [behavior_server]: Creating behavior plugin assisted_teleop of type nav2_behaviors/AssistedTeleop
[behavior_server-19] [INFO] [1775335047.387923758] [behavior_server]: Configuring assisted_teleop
[rtabmap-5] [INFO] [1775335047.393596275] [rtabmap.rtabmap]: rtabmap: frame_id      = base_link
[rtabmap-5] [INFO] [1775335047.393636724] [rtabmap.rtabmap]: rtabmap: map_frame_id  = map
[rtabmap-5] [INFO] [1775335047.393643671] [rtabmap.rtabmap]: rtabmap: log_to_rosout_level  = 4
[rtabmap-5] [INFO] [1775335047.393649275] [rtabmap.rtabmap]: rtabmap: initial_pose  = 
[rtabmap-5] [INFO] [1775335047.393654354] [rtabmap.rtabmap]: rtabmap: use_action_for_goal  = true
[rtabmap-5] [INFO] [1775335047.393659846] [rtabmap.rtabmap]: rtabmap: tf_delay      = 0.050000
[rtabmap-5] [INFO] [1775335047.393674036] [rtabmap.rtabmap]: rtabmap: tf_tolerance  = 0.100000
[rtabmap-5] [INFO] [1775335047.393680990] [rtabmap.rtabmap]: rtabmap: odom_sensor_sync   = false
[rtabmap-5] [INFO] [1775335047.393686256] [rtabmap.rtabmap]: rtabmap: pub_loc_pose_only_when_localizing = false
[rtabmap-5] [INFO] [1775335047.393691130] [rtabmap.rtabmap]: rtabmap: wait_for_transform = 0.500000
[rtabmap-5] [INFO] [1775335047.393696940] [rtabmap.rtabmap]: rtabmap: gen_scan  = false
[rtabmap-5] [INFO] [1775335047.393701926] [rtabmap.rtabmap]: rtabmap: gen_depth  = false
[behavior_server-19] [INFO] [1775335047.413282151] [behavior_server]: Creating behavior plugin wait of type nav2_behaviors/Wait
[behavior_server-19] [INFO] [1775335047.413911927] [behavior_server]: Configuring wait
[point_cloud_xyz-14] [INFO] [1775335047.420339455] [point_cloud_xyz]: Approximate time sync = true
[lifecycle_manager-23] [INFO] [1775335047.425366585] [lifecycle_manager_navigation]: Configuring bt_navigator
[bt_navigator-20] [INFO] [1775335047.425653265] [bt_navigator]: Configuring
[rtabmap-5] [INFO] [1775335047.439651607] [rtabmap.rtabmap]: Setting RTAB-Map parameter "Grid/MapFrameProjection"="true" (rosparam)
[rtabmap-5] [INFO] [1775335047.439804251] [rtabmap.rtabmap]: Setting RTAB-Map parameter "Grid/MaxGroundHeight"="1.15" (rosparam)
[rtabmap-5] [INFO] [1775335047.439875248] [rtabmap.rtabmap]: Setting RTAB-Map parameter "Grid/MaxObstacleHeight"="1.75" (rosparam)
[rtabmap-5] [INFO] [1775335047.446801529] [rtabmap.rtabmap]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="10" (rosparam)
[rtabmap-5] [INFO] [1775335047.458656752] [rtabmap.rtabmap]: Setting RTAB-Map parameter "Optimizer/GravitySigma"="0.1" (rosparam)
[rtabmap-5] [INFO] [1775335047.479794172] [rtabmap.rtabmap]: Setting RTAB-Map parameter "RGBD/StartAtOrigin"="true" (rosparam)
[rtabmap-5] [INFO] [1775335047.525994831] [rtabmap.rtabmap]: Setting RTAB-Map parameter "Vis/FeatureType"="10" (rosparam)
[rtabmap-5] [INFO] [1775335047.529626003] [rtabmap.rtabmap]: RTAB-Map detection rate = 1.000000 Hz
[rtabmap-5] [INFO] [1775335047.529717674] [rtabmap.rtabmap]: rtabmap: Using database from "/home/vscode/.ros/rtabmap.db" (0 MB).
[lifecycle_manager-23] [INFO] [1775335047.587275496] [lifecycle_manager_navigation]: Configuring waypoint_follower
[waypoint_follower-21] [INFO] [1775335047.587545708] [waypoint_follower]: Configuring
[waypoint_follower-21] [INFO] [1775335047.598097553] [waypoint_follower]: Created waypoint_task_executor : wait_at_waypoint of type nav2_waypoint_follower::WaitAtWaypoint
[lifecycle_manager-23] [INFO] [1775335047.598786995] [lifecycle_manager_navigation]: Configuring velocity_smoother
[velocity_smoother-22] [INFO] [1775335047.599088013] [velocity_smoother]: Configuring velocity smoother
[lifecycle_manager-23] [INFO] [1775335047.601883364] [lifecycle_manager_navigation]: Activating controller_server
[controller_server-16] [INFO] [1775335047.602047758] [controller_server]: Activating
[controller_server-16] [INFO] [1775335047.602074941] [local_costmap.local_costmap]: Activating
[controller_server-16] [INFO] [1775335047.602083108] [local_costmap.local_costmap]: Checking transform
[controller_server-16] [INFO] [1775335047.602101162] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist
[rviz2-6] [INFO] [1775335047.719697780] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-6] [INFO] [1775335047.719771391] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rtabmap-5] [INFO] [1775335047.746777215] [rtabmap.rtabmap]: rtabmap: Database version = "0.22.1".
[rtabmap-5] [INFO] [1775335047.746831595] [rtabmap.rtabmap]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[rtabmap-5] [INFO] [1775335047.766587585] [rtabmap.rtabmap]: Setup callbacks
[rtabmap-5] [WARN] [1775335047.766654902] [rtabmap.rtabmap]: rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.
[rtabmap-5] [INFO] [1775335047.766804373] [rtabmap.rtabmap]: rtabmap: subscribe_depth = false
[rtabmap-5] [INFO] [1775335047.766820844] [rtabmap.rtabmap]: rtabmap: subscribe_rgb = false
[rtabmap-5] [INFO] [1775335047.766826610] [rtabmap.rtabmap]: rtabmap: subscribe_stereo = false
[rtabmap-5] [INFO] [1775335047.766832177] [rtabmap.rtabmap]: rtabmap: subscribe_rgbd = true (rgbd_cameras=1)
[rtabmap-5] [INFO] [1775335047.766839086] [rtabmap.rtabmap]: rtabmap: subscribe_sensor_data = false
[rtabmap-5] [INFO] [1775335047.766844630] [rtabmap.rtabmap]: rtabmap: subscribe_odom_info = true
[rtabmap-5] [INFO] [1775335047.766849355] [rtabmap.rtabmap]: rtabmap: subscribe_user_data = false
[rtabmap-5] [INFO] [1775335047.766854127] [rtabmap.rtabmap]: rtabmap: subscribe_scan = false
[rtabmap-5] [INFO] [1775335047.766903125] [rtabmap.rtabmap]: rtabmap: subscribe_scan_cloud = false
[rtabmap-5] [INFO] [1775335047.766909742] [rtabmap.rtabmap]: rtabmap: subscribe_scan_descriptor = false
[rtabmap-5] [INFO] [1775335047.766913431] [rtabmap.rtabmap]: rtabmap: topic_queue_size = 10
[rtabmap-5] [INFO] [1775335047.766916911] [rtabmap.rtabmap]: rtabmap: sync_queue_size  = 10
[rtabmap-5] [INFO] [1775335047.766920028] [rtabmap.rtabmap]: rtabmap: qos_image       = 0
[rtabmap-5] [INFO] [1775335047.766923231] [rtabmap.rtabmap]: rtabmap: qos_camera_info = 0
[rtabmap-5] [INFO] [1775335047.766926307] [rtabmap.rtabmap]: rtabmap: qos_scan        = 0
[rtabmap-5] [INFO] [1775335047.766929489] [rtabmap.rtabmap]: rtabmap: qos_odom        = 0
[rtabmap-5] [INFO] [1775335047.766932595] [rtabmap.rtabmap]: rtabmap: qos_user_data   = 0
[rtabmap-5] [INFO] [1775335047.766935511] [rtabmap.rtabmap]: rtabmap: approx_sync     = false
[rtabmap-5] [INFO] [1775335047.766946542] [rtabmap.rtabmap]: Setup rgbd callback
[rtabmap-5] [INFO] [1775335047.769563037] [rtabmap.rtabmap]: 
[rtabmap-5] rtabmap subscribed to (exact sync):
[rtabmap-5]    /rtabmap/odom \
[rtabmap-5]    /rtabmap/rgbd_image \
[rtabmap-5]    /rtabmap/odom_info
[rviz2-6] [INFO] [1775335047.875768412] [rviz2]: Stereo is NOT SUPPORTED
[parameter_bridge-7] [INFO] [1775335047.974523198] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/clock (gz.msgs.Clock) -> /clock (rosgraph_msgs/msg/Clock)] (Lazy 0)
[parameter_bridge-7] [INFO] [1775335047.976740623] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/world/apt/model/x500_depth_0/link/camera_link/sensor/IMX214/image (gz.msgs.Image) -> /camera/rgb/image (sensor_msgs/msg/Image)] (Lazy 0)
[parameter_bridge-7] [INFO] [1775335047.979728780] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/world/apt/model/x500_depth_0/link/camera_link/sensor/IMX214/camera_info (gz.msgs.CameraInfo) -> /camera/rgb/camera_info (sensor_msgs/msg/CameraInfo)] (Lazy 0)
[parameter_bridge-7] [INFO] [1775335047.980905453] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/depth_camera (gz.msgs.Image) -> /camera/depth/image (sensor_msgs/msg/Image)] (Lazy 0)
[parameter_bridge-7] [INFO] [1775335047.982696885] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/camera_info (gz.msgs.CameraInfo) -> /camera/depth/camera_info (sensor_msgs/msg/CameraInfo)] (Lazy 0)
[parameter_bridge-7] [INFO] [1775335047.983524693] [ros_gz_bridge]: Creating GZ->ROS Bridge: [/world/apt/model/x500_depth_0/link/base_link/sensor/imu_sensor/imu (gz.msgs.IMU) -> /imu/data_raw (sensor_msgs/msg/Imu)] (Lazy 0)
[imu_filter_madgwick_node-1] [INFO] [1775335047.985601757] [imu_filter_madgwick]: First IMU message received.
[controller_server-16] [INFO] [1775335048.102151350] [local_costmap.local_costmap]: Timed out waiting for transform from base_link to odom to become available, tf error: Invalid frame ID "odom" passed to canTransform argument target_frame - frame does not exist
[rgbd_odometry-4] [ WARN] (2026-04-04 20:37:28.103) Odometry.cpp:324::process() Updated initial pose from xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 to xyz=0.000000,0.000000,0.000000 rpy=0.000412,-0.000689,-0.000001 with IMU orientation
[rtabmap-5] [ WARN] (2026-04-04 20:37:28.127) Memory.cpp:5917::createSignature() Mem/SaveDepth16Format is set to false to use 32bits format but this is not compatible with the compressed depth format chosen (Mem/DepthCompressionFormat=".rvl"), depth images will be compressed in ".png" format instead. Explicitly set Mem/SaveDepth16Format to true to keep using ".rvl" format and images will be converted to 16bits for convenience (warning: that would remove all depth values over 65 meters). Explicitly set Mem/DepthCompressionFormat=".png" to suppress this warning. This warning is only printed once.
[ros_odometry_to_vehicle_odometry-11] [ERROR] [1775335048.130006060] [ros_odometry_to_vehicle_odometry]: "map" passed to lookupTransform argument target_frame does not exist. 
[rtabmap-5] [INFO] [1775335048.184572676] [rtabmap.rtabmap]: rtabmap (1): Rate=1.00s, Limit=0.000s, Conversion=0.0017s, RTAB-Map=0.0648s, Maps update=0.0070s pub=0.0004s delay=0.0880s (local map=1, WM=1)
[planner_server-18] [INFO] [1775335048.184637550] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 119 X 85 at 0.050000 m/pix
[ros_odometry_to_vehicle_odometry-11] [ERROR] [1775335048.239866123] [ros_odometry_to_vehicle_odometry]: Lookup would require extrapolation at time 97.484000, but only time 97.640000 is in the buffer, when looking up transform from frame [odom] to frame [map]
[ros_odometry_to_vehicle_odometry-11] [ERROR] [1775335048.339429662] [ros_odometry_to_vehicle_odometry]: Lookup would require extrapolation into the past.  Requested time 97.584000 but the earliest data is at time 97.640000, when looking up transform from frame [odom] to frame [map]
[controller_server-16] [INFO] [1775335048.602241345] [local_costmap.local_costmap]: start
[rviz2-6] [INFO] [1775335048.838086853] [rviz2]: Trying to create a map of size 80 x 80 using 1 swatches
[controller_server-16] [INFO] [1775335048.853152263] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[lifecycle_manager-23] [INFO] [1775335048.956593331] [lifecycle_manager_navigation]: Server controller_server connected with bond.
[lifecycle_manager-23] [INFO] [1775335048.956707928] [lifecycle_manager_navigation]: Activating smoother_server
[smoother_server-17] [INFO] [1775335048.956933117] [smoother_server]: Activating
[smoother_server-17] [INFO] [1775335048.956957164] [smoother_server]: Creating bond (smoother_server) to lifecycle manager.
[lifecycle_manager-23] [INFO] [1775335049.060265944] [lifecycle_manager_navigation]: Server smoother_server connected with bond.
[lifecycle_manager-23] [INFO] [1775335049.060301518] [lifecycle_manager_navigation]: Activating planner_server
[planner_server-18] [INFO] [1775335049.060600776] [planner_server]: Activating
[planner_server-18] [INFO] [1775335049.060637536] [global_costmap.global_costmap]: Activating
[planner_server-18] [INFO] [1775335049.060646860] [global_costmap.global_costmap]: Checking transform
[planner_server-18] [INFO] [1775335049.060730383] [global_costmap.global_costmap]: start
[planner_server-18] [INFO] [1775335049.110841126] [planner_server]: Activating plugin GridBased of type NavfnPlanner
[planner_server-18] [INFO] [1775335049.111880058] [planner_server]: Creating bond (planner_server) to lifecycle manager.
[lifecycle_manager-23] [INFO] [1775335049.214597176] [lifecycle_manager_navigation]: Server planner_server connected with bond.
[lifecycle_manager-23] [INFO] [1775335049.214642011] [lifecycle_manager_navigation]: Activating behavior_server
[behavior_server-19] [INFO] [1775335049.214949708] [behavior_server]: Activating
[behavior_server-19] [INFO] [1775335049.214969673] [behavior_server]: Activating spin
[behavior_server-19] [INFO] [1775335049.214979220] [behavior_server]: Activating backup
[behavior_server-19] [INFO] [1775335049.214985060] [behavior_server]: Activating drive_on_heading
[behavior_server-19] [INFO] [1775335049.214990298] [behavior_server]: Activating assisted_teleop
[behavior_server-19] [INFO] [1775335049.214995337] [behavior_server]: Activating wait
[behavior_server-19] [INFO] [1775335049.215003096] [behavior_server]: Creating bond (behavior_server) to lifecycle manager.
[rtabmap-5] [INFO] [1775335049.217228641] [rtabmap.rtabmap]: rtabmap (2): Rate=1.00s, Limit=0.000s, Conversion=0.0013s, RTAB-Map=0.0362s, Maps update=0.0043s pub=0.0014s delay=0.0840s (local map=1, WM=1)
[lifecycle_manager-23] [INFO] [1775335049.318212911] [lifecycle_manager_navigation]: Server behavior_server connected with bond.
[lifecycle_manager-23] [INFO] [1775335049.318247239] [lifecycle_manager_navigation]: Activating bt_navigator
[bt_navigator-20] [INFO] [1775335049.318485094] [bt_navigator]: Activating
[bt_navigator-20] [INFO] [1775335049.366442987] [bt_navigator]: Creating bond (bt_navigator) to lifecycle manager.
[lifecycle_manager-23] [INFO] [1775335049.471440400] [lifecycle_manager_navigation]: Server bt_navigator connected with bond.
[lifecycle_manager-23] [INFO] [1775335049.471535780] [lifecycle_manager_navigation]: Activating waypoint_follower
[waypoint_follower-21] [INFO] [1775335049.472846422] [waypoint_follower]: Activating
[waypoint_follower-21] [INFO] [1775335049.472876478] [waypoint_follower]: Creating bond (waypoint_follower) to lifecycle manager.
[lifecycle_manager-23] [INFO] [1775335049.578749916] [lifecycle_manager_navigation]: Server waypoint_follower connected with bond.
[lifecycle_manager-23] [INFO] [1775335049.578794852] [lifecycle_manager_navigation]: Activating velocity_smoother
[velocity_smoother-22] [INFO] [1775335049.579069856] [velocity_smoother]: Activating
[velocity_smoother-22] [INFO] [1775335049.579114795] [velocity_smoother]: Creating bond (velocity_smoother) to lifecycle manager.
[lifecycle_manager-23] [INFO] [1775335049.682219640] [lifecycle_manager_navigation]: Server velocity_smoother connected with bond.
[lifecycle_manager-23] [INFO] [1775335049.682256082] [lifecycle_manager_navigation]: Managed nodes are active
[lifecycle_manager-23] [INFO] [1775335049.682262758] [lifecycle_manager_navigation]: Creating bond timer...
[rtabmap-5] [INFO] [1775335050.237209113] [rtabmap.rtabmap]: rtabmap (3): Rate=1.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.0335s, Maps update=0.0042s pub=0.0008s delay=0.0800s (local map=1, WM=1)
[rtabmap-5] [INFO] [1775335051.291588904] [rtabmap.rtabmap]: rtabmap (4): Rate=1.00s, Limit=0.000s, Conversion=0.0010s, RTAB-Map=0.0316s, Maps update=0.0052s pub=0.0007s delay=0.0800s (local map=1, WM=1)
[...]

3. Third terminal:

ros2 run rtabmap_drone_example offboard_control --ros-args -p use_sim_time:=true

Output:

~/ros2_ws/src/rtabmap_drone_example$ ros2 run rtabmap_drone_example offboard_control --ros-args -p use_sim_time:=true

Starting node...
[INFO] [1775335092.730696589] [offboard_control]: Setting offboard mode...
[INFO] [1775335092.737928327] [offboard_control]: Command accepted!
[INFO] [1775335093.730686345] [offboard_control]: Vehicle arming..
[INFO] [1775335093.730727257] [offboard_control]: Take off at 1.3 meter... to position=(0.000082,-0.001075,-1.300011) heading=1.570424
[INFO] [1775335093.746007999] [offboard_control]: Command accepted!

Drone takes off and stays stable:

---

Debugging logs:

~/ros2_ws/src/rtabmap_drone_example$ ros2 topic list | grep vehicle_local_position
/fmu/out/vehicle_local_position


~/ros2_ws/src/rtabmap_drone_example$ ros2 topic echo /fmu/out/vehicle_local_position --once
timestamp: 1775335328722622
timestamp_sample: 1775335328718622
xy_valid: true
z_valid: true
v_xy_valid: true
v_z_valid: true
x: 0.0028671876061707735
y: -0.0014097703387960792
z: -1.2993130683898926
delta_xy:
- 0.00016849691746756434
- 0.00018221803475171328
xy_reset_counter: 2
delta_z: -0.055788133293390274
z_reset_counter: 2
vx: -0.00335889239795506
vy: 0.0010286944452673197
vz: -0.00063140201382339
z_deriv: -9.017133561428636e-05
delta_vxy:
- 0.007389651145786047
- 0.004426497034728527
vxy_reset_counter: 1
delta_vz: -0.20074929296970367
vz_reset_counter: 1
ax: 0.004113011062145233
ay: -0.00553693063557148
az: -0.0009988433448597789
heading: 1.5704175233840942
heading_var: 0.0003820574202109128
unaided_heading: -0.006096605211496353
delta_heading: 1.5729012489318848
heading_reset_counter: 1
heading_good_for_control: true
tilt_var: 2.816267078742385e-05
xy_global: false
z_global: false
ref_timestamp: 0
ref_lat: .nan
ref_lon: .nan
ref_alt: .nan
dist_bottom_valid: false
dist_bottom: 1.3994537591934204
dist_bottom_var: 46.463096618652344
delta_dist_bottom: -0.055788133293390274
dist_bottom_reset_counter: 2
dist_bottom_sensor_bitfield: 0
eph: 0.05424315109848976
epv: 0.0339827686548233
evh: 0.06849058717489243
evv: 0.03642598167061806
dead_reckoning: false
vxy_max: .inf
vz_max: .inf
hagl_min: .inf
hagl_max_z: .inf
hagl_max_xy: .inf
---

~/ros2_ws/src/rtabmap_drone_example$ ros2 topic echo /fmu/out/vehicle_status_v1 --once
timestamp: 1775335379258550
armed_time: 143056000
takeoff_time: 144832000
arming_state: 2
latest_arming_reason: 4
latest_disarming_reason: 0
nav_state_timestamp: 142060000
nav_state_user_intention: 14
nav_state: 14
executor_in_charge: 0
valid_nav_states_mask: 2147411071
can_set_nav_states_mask: 8307839
failure_detector_status: 0
hil_state: 0
vehicle_type: 1
failsafe: false
failsafe_and_user_took_over: false
failsafe_defer_state: 0
gcs_connection_lost: true
gcs_connection_lost_counter: 0
high_latency_data_link_lost: false
is_vtol: false
is_vtol_tailsitter: false
in_transition_mode: false
in_transition_to_fw: false
system_type: 2
system_id: 1
component_id: 1
safety_button_available: true
safety_off: true
power_input_valid: true
usb_connected: false
open_drone_id_system_present: false
open_drone_id_system_healthy: false
parachute_system_present: false
parachute_system_healthy: false
rc_calibration_in_progress: false
calibration_enabled: false
pre_flight_checks_pass: true
---