Cartographer_ros: StartTrajectory service should support initial pose

[gaschler]

The initial pose feature is the recommended way to use localization, in particular on larger maps.

Currently, initial_pose is only available through the start_trajectory executable.
More details are the in PR where initial_pose was introduced.

ROS service StartTrajectory.srv should take initial pose as an argument (lua string should not be necessary).
This is a missing feature.
At the same time, the format of the initial pose argument could be reworked, it is unnecessarily complex to set the pose in 2D.

Ideally, the format should be so easy that start_trajectory_main.cc can be deleted.

[neomanic]

When you say the format of the pose argument is overly complex, do you mean only for calling the executable rather than the .srv (which is loaded with parameters already)?

Would the best approach to adding initial_pose to StartTrajectory.srv be with a geometry_msgs/Pose initial_pose field? i.e. Okay to depend on the standard ros messages?

[MichaelGrupp]

My 2 cents since we use this service in production since more than a year:

Initial pose would be easy to add but is not really the bottleneck IMO.

The ugliest part of this service is the binary string of the trajectory options proto, which is impossible to build by client code that should only know the message contract from cartographer_ros_msgs. The start_trajectory_main.cc executable makes this convenient because it hides exactly these implementation details away from the user/client with a proper interface that has only 3 simple options.

My ideal message definition would follow that interface:

geometry_msgs/Pose initial_pose
int32 to_trajectory_id
string configuration_directory
string configuration_basename
---
cartographer_ros_msgs/StatusResponse status
int32 trajectory_id

and all the configuration parsing, proto etc details are handled by the node. Clients only need to depend on cartographer_ros_msgs and geometry_msgs.

Then I'd be happy to see start_trajectory_main.cc die, but before: nah

[gaschler]

My issue description was misleading.
The real issue is what @MichaelGrupp said, building the trajectory options proto is not user-friendly.
The need for start_trajectory_main.cc proves that.

[MichaelGrupp]

I can do a PR for the refactored service as I wrote above, should be not too hard and would simplify things a lot.

[MichaelGrupp]

Or, because this would be breaking existing API, a short RFC summarizing what is written here.

[neomanic]

I like your proposed solution @MichaelGrupp, that would suit our use case (lost localisation, need new trajectory with the same options as we started up with, just a new initial pose) well. In the meantime I'll just call out to the start_trajectory executable.

[MichaelGrupp]

I suggest we continue the discussion here: https://github.com/googlecartographer/rfcs/pull/40

[MichaelGrupp]

Implemented on master.