Implement robot_localization

[EnzoRap117]

No description provided.

[zbrotherton]

This covers much much more than IMU btw, its more of an EKF PR

Linking relevant issues

[EnzoRap117]

Yeah, I realized that after I named the branch though. Will probably replace this branch with one with a more fitting name when I get the chance.

[EnzoRap117]

Currently, this modified version of sim_rtabmap causes rviz to fail to launch automatically when starting the sim, likely due to some failure related to the LiDAR data based on the errors being thrown when running rviz manually.

Still working through how to best implement robot_localization with rtabmap, a better name for this branch would be "implement robot_localization"

[EnzoRap117]

Based on what I read here, I'll try have it so icp_odometry and stereo_odometry can be fed into robot_localization as separate odometry sources and having that act as the odometry source. http://official-rtab-map-forum.206.s1.nabble.com/Combining-visual-and-icp-odometry-td7623.html

[EnzoRap117]

Right now:

Currently trying to change the the rtabmap nodes of icp_odometry and stereo_odometry to rename their publish topics to something other than 'odom' so they don't conflict with robot_localization

Need to feed the data of icp_odometry and stereo_odometry into robot_localization

---

Right now: Figure out how to do the above

Currently, 'odom' is being published to by icp_odometry

• This is bad
  • The only raw sensor data is currently icp_odometry in the topic /rtabmap/odom_sensor_data/raw
• Instead
  • icp_odometry and stereo_odometry should be used as inputs for robot_localization
  • robot_localizaiton should be the only thing publishing to odom

[EnzoRap117]

The code for launching the icp & stereo nodes is copied from the .launch file of rtabmap itself in commit 10. They still need to re-written, ideally in a way that minimizes repetition to avoid topics being set same values multiple times