[xslocobot_nav] SLAM and Nav2 functionalities

[stephenadhi]

Hi,

I'm further improving nav2 functionalities, hoping to make both RTABMAP and slam_toolbox work as well as adding localizazion mode using nav2_amcl given a map yaml file.

Changes

1. Added sample map interbotix_map.pgm based on the SLAM result using the interbotix.world in gazebo.
2. Fixed nav2 functionalities
   • Static layer receiving data from map_server. Transformation from map to odom published by amcl.
   • Able to produce global plan given a goal pose.
   • add missing use_sim_time in param file. This has to be explicitly written in the yaml file, so the launch file can substitute the value based on the given launch argument. Otherwise it produces an error due to different time.
3. Added standard AMCL parameter and launch argument option.
4. Minor bug in param file and in launch file regarding lifecycle nodes.
5. Added more RviZ visualization i.e. for global/local plan, amcl particle_cloud.

The nav2 parameters are by default configured for the real robot.
For testing in simulation, we need to either change the parameters or adjust the odom and cmd_vel topic remappings inside the urdf file to reproduce the same result.

Testing

1. Launching simulator
   ros2 launch interbotix_xslocobot_sim xslocobot_gz_classic.launch.py robot_model:=locobot_base robot_name:=locobot use_lidar:=true

2. Mapping mode: Launching SLAM using slam_toolbox
   ros2 launch interbotix_xslocobot_nav xslocobot_slam_toolbox.launch.py launch_driver:=false slam_mode:=online_async use_sim_time:=true cmd_vel_topic:=/locobot/diffdrive_controller/cmd_vel_unstamped

3. Localization mode: launching nav2 + map from yaml and using amcl localization
   *Note: replace map filepath to your interbotix_ros_rovers folder location
   ros2 launch interbotix_xslocobot_nav xslocobot_nav2_bringup.launch.py slam_mode:=localization robot_name:=locobot map:=/workspaces/locobot/src/interbotix_ws/interbotix_ros_rovers/interbotix_ros_xslocobots/interbotix_xslocobot_nav/maps/interbotix_map.yaml use_sim_time:=true cmd_vel_topic:=/locobot/diffdrive_controller/cmd_vel_unstamped

[lukeschmitt-tr]

@stephenadhi what is left to do before this is ready to be reviewed?

[stephenadhi]

Hi, you can review and test whether the localization with AMCL is working.

@stephenadhi what is left to do before this is ready to be reviewed?

[stephenadhi]

@LSinterbotix UPDATE 23/05/2023:
Since xslocobot_nav2_bringup.launch.py is used by both slam_toolbox and RTABMAP, the launch argument slam_toolbox_mode is only exclusive for the slam_toolbox launch file. I reverted the slam_toolbox_mode in nav2_bringup launch back to slam_mode and the options are only supposed to be between 'localization' and 'mapping'.

So if anyone ever wants to use AMCL, they should just launch the xslocobot_nav2_bringup.launch.py directly with the launch argument slam_mode:=localization (PR description updated). This is exactly how it is done in Nav2 bringup , support for using localization with slam toolbox is then out of scope.

For me the simulation seems to be a bit problematic with the robot odometry, both in mapping and localization mode (Its more prone to shifts compared to in real life). So far I have been mapping using the slam toolbox in the real robot hardware without problem, performance is really good. I did not have the time to test this AMCL localization yet.

Also specific to the humble distro branch:
Added use composition launch and fixed param namings