mola stops publishing map->odom when the robot is stopped

[limor666]

I'm running it using standard yaml files and overriding defaults in command line.
Invoking in the following way works as expected but I would like it not to stop publishing map->odom once there is no movement of the robot

MOLA_IMU_TOPIC=/Tugbot1/imu \
MOLA_LO_PUBLISH_REFERENCE_FRAME=map \
MOLA_LO_PUBLISH_VEHICLE_FRAME=Tugbot1/base_link_180 \
MOLA_TF_BASE_LINK=Tugbot1/base_link_180 \
MOLA_TF_ESTIMATED_ODOMETRY=Tugbot1/odom \
MOLA_TF_MAP=map \
ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py \
  lidar_topic_name:=/Tugbot1/scan_omni_cloud \
  base_link_frame:=Tugbot1/base_link_180 \
  mola_tf_base_link:=Tugbot1/base_link_180 \
  odom_frame:=Tugbot1/odom \
  reference_frame:=map \
  publish_localization_following_rep105:=True \
  forward_ros_tf_as_mola_odometry_observations:=True \
  start_active:=True \
  use_mola_gui:=False \
  \
  start_mapping_enabled:=False \
  mola_initial_map_mm_file:=warehouse.mm

[jlblancoc]

Hi @limor666 !

I just double checked it, and MOLA-LO (via BridgeRos2) publishes Odometry messages at the sensor rate, no matter the robot moves or not...

I don't know how to help you debug this w/o further details... As a first idea, check (ros2 topic echo /tf, tf_monitor,...) if the pose is being updated via /tf. Since you use rep105, remember that MOLA will publish the tf between map -> odom.

Do you have any warning in the terminal?

You can also try setting the environment variables MOLA_VERBOSITY_BRIDGE_ROS2=DEBUG or MOLA_VERBOSITY_MOLA_LO=DEBUG and piping them to a file for posterior analysis (there'll be too many messages) to look for something suspicious.

[limor666]

Thank you for the reply. There were no warnings when it stopped broadcasting.
After a lot of experimentation, reading source code and AI help, I found out that adding state_estimator_config_yaml parameter to my previous command line (and the yaml file below), solves the problem. I let the system run for many hours and so far Mola continuously sends the map->odom TF.

Command Line:

MOLA_IMU_TOPIC=/Tugbot1/imu \
MOLA_LO_PUBLISH_REFERENCE_FRAME=map \
MOLA_LO_PUBLISH_VEHICLE_FRAME=Tugbot1/base_link_180 \
MOLA_TF_BASE_LINK=Tugbot1/base_link_180 \
MOLA_TF_ESTIMATED_ODOMETRY=Tugbot1/odom \
MOLA_TF_MAP=map \
ros2 launch mola_lidar_odometry ros2-lidar-odometry.launch.py \
  lidar_topic_name:=/Tugbot1/scan_omni_cloud \
  base_link_frame:=Tugbot1/base_link_180 \
  mola_tf_base_link:=Tugbot1/base_link_180 \
  odom_frame:=Tugbot1/odom \
  reference_frame:=map \
  publish_localization_following_rep105:=True \
  forward_ros_tf_as_mola_odometry_observations:=True \
  start_active:=True \
  use_mola_gui:=False \
  start_mapping_enabled:=False \
  mola_initial_map_mm_file:=$PWD/warehouse.mm \
  state_estimator_config_yaml:=$PWD//my_smoother.yaml

my_smoother.yaml

execution_rate: 20.0
params:
  kinematic_model: "KinematicModel::ConstantVelocity"
  vehicle_frame_name: "Tugbot1/base_link_180"
  reference_frame_name: "map"
  # Allow the smoother to wait longer for data
  max_time_to_use_velocity_model: 10.0
  enforce_planar_motion: true
  sliding_window_length: 10
  # Force the filter to initialize even with 0 velocity
  initial_uncertainty_pos: 0.5
  initial_uncertainty_ang: 0.1
  sigma_v_stationary: 0.01
  sigma_w_stationary: 0.01