Publishing map->odom for custom particle filter (BFL)

edit

Hi,

I am having some trouble trying to implement the map->odom transform for our robot setup. Our stack uses ORB-SLAM, Robot Localization, Gmapping, and finally our own custom particle filter. So far, our tf tree from ORB-SLAM/Robot Localization EKF/Gmapping is functional (odom -> base_link) and is able to generate a map.

Our problem right now is trying to implement the map->odom transform in order to use our custom BFL. Our code is derived from the BFL tutorial and is able to subscribe to range finder data and publish the map pose, which we believe is what we can use to calculate the map->base_link transform. We know from doing a bit of online digging that we can just use the inverse of the odom->base_link and combine it with map->base_link to generate the map->odom transform, but we are stuck with trying to write the code. Trying to use the AMCL code as a reference (the laserReceived function in particular) is proving to be difficult so we're hoping to get some sort of guidance by way of some simpler sample code.

One hint we got from the AMCL code is that we might be able to "[subtract] base to odom from map to base and send map to odom instead", which we think would simply be something like this: map->base_link - base_link->odom = map->odom

Or somewhere in our code, maybe have something like this:

base_odom_pose = odom_msg.pose.pose.inverse() //where odom_msg is taken from our EKF
map_base_pose  = bfl_pose
map_odom_pose = map_base_pose - base_odom_pose