LoadMapAndLocalize fails and crashes node when enable_mapping_and_localization: False

[MrOCW]

Hi, I am testing the localization function I mapped a small area, ran ros2 action send_goal /visual_slam/save_map isaac_ros_visual_slam_interfaces/action/SaveMap "{map_url: /elbrus/map} and it saved into /elbrus/map/data.mdb I brought the robot back to the initial point and relaunched elbrus again, this time with enable_mapping_and_localization: False, then in another terminal: ros2 action send_goal /visual_slam/load_map_and_localize isaac_ros_visual_slam_interfaces/action/LoadMapAndLocalize "{map_url: /elbrus/map}

Tried multiple times and it always returns:

robot-elbrus-1     | [component_container-1] [WARN] [1675336342.806457768] [visual_slam_node]: CallbackLoadMapAndLocalizeAccepted()
robot-elbrus-1     | [component_container-1] [INFO] [1675336342.808872973] [visual_slam_node]: elbrus: LoadMapAndLocalize /elbrus/map [0.000000, 0.000000, 0.000000]
robot-elbrus-1     | [component_container-1] [WARN] [1675336342.820364284] [visual_slam_node]: Localization failed. Status=6
robot-elbrus-1     | [component_container-1] [WARN] [1675336342.827145923] [visual_slam_node]: ELBRUS_LocalizeInExistDb Error 6
robot-elbrus-1     | [component_container-1] terminate called after throwing an instance of 'rclcpp::exceptions::RCLError'
robot-elbrus-1     | [component_container-1]   what():  goal_handle attempted invalid transition from state ABORTED with event ABORT, at /opt/ros/humble/src/rcl/rcl_action/src/rcl_action/goal_handle.c:95
robot-elbrus-1     | [ERROR] [component_container-1]: process has died [pid 78, exit code -6, cmd '/opt/ros/humble/install/lib/rclcpp_components/component_container --ros-args -r __node:=visual_slam_launch_container -r __ns:=/'].

it only works when enable_mapping_and_localization: True but i would like it to be purely localization #58 . Is this possible?

[hemalshahNV]

enable_mapping_and_localization: True may be the "pure" localization mode you're looking if I understood. When set to false, this means that only "smooth" visual odometry poses are published with no loop closure/bundle adjustment, only frame-to-frame with drift. When set to true, it will use its "map" (=pose graph of landmarks which you tried to load) to localize as "rectified" poses which could cause "localization jumps" (discontinuities when visual landmarks are recognized, loops closed, and previously reported poses are corrected).

The node itself should not crash of course in this case. We'll get a fix for this into the queue. Thanks!

[MrOCW]

enable_mapping_and_localization: True causes loop closures which changes the pointcloud map. This makes it incompatible with navigation stacks like nav2 when a static 2D map is required

what would be really helpful would be an option for something like enable_localization_only: True

[hemalshahNV]

The "map" in Isaac ROS Visual SLAM is in fact a visual landmark pose graph and is not necessarily related directly to the static 2D occupancy grid map Nav2 is working with as a floor plan. If we could register this visual landmark pose graph frame (which starts where the robot began) with an occupancy grid map (perhaps with a V-pattern board with a mosaic Apriltag pattern on the ground so we could see this in the OGM and get the pose of it relative to the robot when it starts), we could feed in localization poses from VSLAM in the frame of the OGM Nav2 is using.

[MrOCW]

but wouldnt bundle adjustment from VSLAM cause the localization to have a mismatch with the OGM used in Nav2

[abylikhsanov]

@MrOCW @hemalshahNV Isn't this? https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox

[hemalshahNV]

but wouldnt bundle adjustment from VSLAM cause the localization to have a mismatch with the OGM used in Nav2

Not exactly, no. The OGM is fixed in Nav2 and VSLAM is localizing to its own landmark map frame origin. As long as you have the fixed, static transform between the OGM's origin frame and VSLAM's landmark map frame origin, VSLAM will be performing bundle adjustment to provide better poses in the OGM's origin frame.

[hemalshahNV]

@MrOCW @hemalshahNV Isn't this? https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nvblox

nvblox can rely on VSLAM to provide poses relative to a common origin frame (the landmark map frame in this case).

[abylikhsanov]

@hemalshahNV Will that common origin frame will awlays be static and VSLAM will not cause saved positions (that are saved originally upon initial map) to shift?

[hemalshahNV]

This frame should always be static. The origin of the VSLAM landmap pose graph remains static as a fixed point while it later "bundle adjusts" the other poses along the way relative to that frame.