navsat_transform_node to find pose coordinates for a given GPS lat/long
I'm working on a robotic convoy setup entirely in simulation, and want to do the following:
Vehicle 1 - Send its GPS coordinates to follower vehicle (called Vehicle 2)
Vehicle 2- Take the GPS coordinates, convert them to pose coordinates in Vehicle 2's global frame (X,Y coordinates, in case I'm using the wrong words here...), and then use that as a waypoint.
Looking into navsat_transform_node, it seemed like the appropriate tool to do this, but cannot get it to work in a way that makes sense. Here are the things I've tried to do:
1.) Send Vehicle 1's IMU and GPS data to Vehicle 2's navsat_transform_node. When I do this, Veh2's navsat_transform_node ends up publishing pose coordinates in Vehicle 1's global frame. Basically, it just spits out Vehicle 1's odometry/filtered data from Vehicle's 1 perspective.
2.) Send Vehicle 1's GPS data to Vehicle 2's navsat_transform_node, while using Vehicle 2's IMU information. When I do this...the results don't seem to make much sense. The GPS coordinates and how they're transformed into Vehicle 2's global pose coordinate frame don't seem to correspond to cause and effect of what's seen in RVIZ/Gazebo, so I'm assuming this is way off base.
As my assumption of what navsat_transform_node does incorrect, or am I not setting it up correctly?
Some additional notes in case it helps, I'm using Clearpath Husky simulation and robot_localization.
Food for thought, the IMU as far as I understand is used by the navsat transform node only to initialize the global coordinate frame. Each time you start the experiment the coordinate of your global frame will be at a different location centered on where robot 1 was when you started the experiment. If I were you, even though you are in simulation and the IMU is probably giving you good data, I would use a static datum. This will mean that the map origin will be in the same location every time, and if you use the same static datum for both vehicles then they will be essentially in the same global coordinate frame which will make interpreting your experiments much easier.
If you do that I think it might make understanding the rest of your problem easier. Problems with initialization of the global coordinate frame, and the noise ...(more)
Hello!
Thanks for the suggestion! Based on what you said, the way I was filling in the IMU data and updating it constantly seems to have been the problem. When setting a manual datum like you suggested, I get results that look more like I expect. In my actual set up, I actually have 3 vehicles total. Going to do some testing to ensure that this is the right approach, and report back here/mark the question as answered once I can confirm.
Also, just repeating myself to some degree, but if you didn't clearly state what the IMU was far, I would not have been able to parse that out from the documentation, so thanks for helping me understand it's something to set at the beginning, not to constantly update as the vehicle runs.
@cmcheung I am happy to hear you made some useful progress and have a better understanding of the system. Its my pleasure to help, and when you feel like you solved the problem I will convert my comments here to an answer so you can accept it as correct. Good luck and happy experimenting!
I had some funky thinks going on due to vehicle rotation, but after changing the default alignment of the vehicles so they were all facing the same way, and changing the datum pose to:
x - 0 y - 0 z - 1 w -1
that made it so all the follower vehicles were getting the proper coordinates. This indeed was the answer. Thanks!
Also, didn't realize there was a distinction between Comments and Answers. Just tried to mark this as the answer and was confused.