Make it possible to snapshot/restart a simulation

[daewok]

It'd be super useful to put the glider into a specific state without needing to tear everything down and restart it by specifying the desired state as the initial condition.

For example, the glider's built in simulation software will basically teleport the glider to a given location whenever the sensors s_ini_lat and s_ini_lon are set and a mission is not in progress. I think supporting that would be a good first step. I assume the extctl_sim driver could use the set model state service to do this, but there are two issues with that: 1) That service call may happen while the kinematics code is in the middle of a loop, in which case the kinematics code will end up unteleporting the glider and 2) we need some way to reset the dead reckoning estimator to the new position.

Maybe 1. could be addressed by pausing the simulation first? I don't know enough about the internals of Gazebo to know if that'll guarantee the kinematics code and teleporting code won't clash. 2. Could be addressed by either a separate service call or merging the dead reckoning code into the extctl_sim driver (which is, honestly, closer to the actual glider architecture).

If we get that working, an amazing next step would to be able to "snapshot" the simulation and restore directly to that snapshot. This would require dumping the internal state of everything involved in the simulation. A big pro to this is that you could restore directly to a given point in time, even with uncertainties and errors preserved. For instance, the easy approach to the first step would result in the DR code being set to the glider's true position after teleporting; with this, you could restore to a state where the accumulated errors in the DR are still present.

It wouldn't be too difficult to support this in the extctl_sim driver. Most of the state there is just in a giant dictionary that could easily be serialized to a file and then deserialized at some point. I'm not sure how difficult it would be with the kinematics plugin, though.

[woensug-choi]

How accurate would this "snapshot" have to be? If repositioning of the vehicle is enough, it would be a much closer milestone. (1) Making a new msg variable at UwGliderCommand such as re_lat, re_lon to reposition the vehicle.

Currently, if the destination is near the surface, the DR will automatically sync with the GPS signal (after few seconds of confirmation). The DR node can subscribe to re_lat, re_lon too to reset if needed.

Would this be enough? Is there any specific need to use service call? There's a rosservice call /gazebo/reset_simulation command. It will reset the time too.