Zero shot localization, multi-agent support, convoy

[bretwitt]

Added a lot of stuff, not expecting to merge within the next week or so until I do a whole lot of cleaning up (lots of random changes that need to be reverted) and testing on the live ARTs. This may also be better for a branch that's not main. Would also like some opinions on the design decision made. Just a rundown of new features I'm proposing

1. Multi-agent structuring

   • robot_ns parameter (will probably make a util function for this, the PythonExpression's are an eyesore)
   • More comprehensive tf support
     - Description package
     -Multi-agent localization to a global map frame: map -> robot_ns/odom -> robot_ns/base_link

2. Localization updates

   • Use exclusively robot_localization & navsat_transform_node with a custom set_datum client to have a shared map frame between ART instances
   • This is a much less complicated localization setup than previously, and still maintains pose & twist estimation while making it (in theory) completely identical between sim & reality
   • Only difference between sim & reality is the new chrono_imu_filter package (changing this to chrono_sensor_filter soon) which acts as an interface between Chrono & R_L. It republishes the Chrono topics as ~/gps/fix, ~/imu/data. Magnetometer calculations are handled and directly thrown into the ~/imu/data message (copied from ground truth heading calculations). This should behave exactly the same as the sensor suite on Oak.
   • For this to work I had to make some hotfixes to Chrono Sensor (IMU & GPS were broken, I'll make a PR to Chrono soon)

3. Convoy demo

   • Reason we need multi-agent support
   • Added a pure pursuit controller, doesn't work the greatest.
   • Added a version of Harry's trajectory waypoint following controller that's compatible with the vehicle localization setup

4. Vehicle configs

   • Different vehicles use different serial ports & PWM values, instead of different vehicles having different settings in the same files & constantly overwriting them with careless Git pushes and walking on eggshells in general, we can isolate these changes between separate vehicles using vehicle-unique configs/ROS2 parameters. For example, Arrowwood computer config
   • Configs are loaded by env variable like this: art_sensing_launch/config/${VEH_CONFIG}.yml, ${VEH_CONFIG} is stored in atk.env
   • Utility functions would be useful here so we don't have to copy and paste the same two lines of code
   • Vehicle/sensing being different packages doesn't make much sense to me, I believe it should all be under the vehicle package

5. Drivers support robot_ns namespacing

6. FastDDS Discovery Server & Super Client

   • Communications are extremely strained v2v without changes to FastDDS behavior
   • Switching from multi-cast discovery to discovery servers speeds things up a lot and reduces congestion
   • To turn off, comment out ROS_DISCOVERY_SERVER from atk.env
   • To turn on, launch as is, and launch the new discovery service (no interactive shell is fine)
   • Be aware that ros2cli is extremely janky with discovery server UNLESS you turn on Super Client mode in a FastDDS XML configuration, you can turn this on for a shell instance using introspection.sh
   • The preferred configuration is the hosting ART (the one with the discovery service running) has the ROS_DISCOVERY_SERVER=127.0.0.1:11811, and all other vehicles use ROS_DISCOVERY_SERVER=<lead vehicle wlan0>:11811
   • If you'd like to see the effect yourself, use iftop with it on and off to profile congestion

7. Hardware in the Loop

   • Run flags on vehicles use_sim:=True and use_filters:=True with $VEH_CONFIG=sim

Right now to launch from art.launch.py you just have to use the robot_ns and leader_ns parameters (i.e artcar_1, artcar_2) The lead car should use leader_ns:=none. The leader is the car directly in front of the current car, not the first car in the convoy.

Other planned/WIP features:

• Base station: curses TUI that handles cmd_vel and VehicleInputs messages seemlessly, switch up control of different vehicles, call relevant services
• Expand launch utils for PythonExpression substitution support when appending namespaces to topics through LaunchConfiguration, also add utility functions for loading vehicle configs