#!/usr/bin/env bash
# For usage instructions, see: https://docs.mola-slam.org/latest/

if [ "$#" -eq 0 ]; then
    echo "Error: At least 1 rosbag file is required. Split rosbags are also supported by providing the path to the directory."
    echo "Usage: $0 /path/to/dataset.mcap"
    echo "Usage: $0 /path/to/dataset_directory    (It must contain a metadata.yaml)"
    echo ""
    echo "Common environment variables:"
    echo "  MOLA_LIDAR_TOPIC       LiDAR topic name (default: '/ouster/points')"
    echo "  MOLA_IMU_TOPIC         IMU topic name (default: '/imu')"
    echo "  MOLA_GNSS_TOPIC        GNSS NavSatFix topic name (default: '/gps')"
    echo "  MOLA_ODOMETRY_TOPIC    Wheels 2D odometry (optional)"
    echo "  MOLA_TF_BASE_LINK      Robot base /tf frame id (default: 'base_link')"
    echo "  MOLA_TF_TOPIC          /tf topic in the bag (default: '/tf')"
    echo "  MOLA_TF_STATIC_TOPIC   /tf_static topic in the bag (default: '/tf_static')"
    echo "                         Override both for namespaced bags (e.g. /robot1/tf)."
    exit 1
fi

SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )

MOLA_LAUNCHS_DIR=$SCRIPT_DIR/../mola-cli-launchs/
if [ -d $SCRIPT_DIR/../share/mola_lidar_odometry/mola-cli-launchs ]; then
  MOLA_LAUNCHS_DIR=$SCRIPT_DIR/../share/mola_lidar_odometry/mola-cli-launchs
fi

FILE=$1
shift 1

MOLA_ODOMETRY_PIPELINE_YAML="${PIPELINE_YAML:-$MOLA_LAUNCHS_DIR/../pipelines/lidar3d-default.yaml}" \
MOLA_INPUT_ROSBAG2="$FILE" \
  mola-cli \
    $MOLA_LAUNCHS_DIR/lidar_odometry_from_rosbag2.yaml \
    $@
