Cartographer_ros: Initial Pose Wiring

[cschuet]

The initial pose implementation in https://github.com/googlecartographer/cartographer/pull/606 is ready to merge but as of yet untested.

Initial poses need to be wired up in cartographer_ros (probably have an initial_pose topic) and tested that they work correctly.

[cschuet]

@Mofef @damienrg @apr8 @ojura @jihoonl Any interested in taking this?

[damienrg]

I can do it.
Considering implementation, I think that I should add an optional pose in start trajectory rather than subscribing to another one topic as we have to create a new trajectory in any case.
Is it ok for you?

[cschuet]

That sounds like a good approach to me.

[jihoonl]

Can you take a look #513? It was a follow PR of our initialpose implementation. Surely, it can be tuned to use together with googlecartographer/cartographer#606

[cschuet]

Ah, nice! @jihoonl do you want to adapt that PR then since it is already written? Or would you rather have @damienrg take that over? I am currently busy with other work items and I don't want to get in the way of this feature.

[jihoonl]

Sure no problem. I will simply rename initialpose as relative_pose and add int32 to_trajectory in StartTrajectory.srv. I am thinking to use frame_id field as indicator if this relative_pose is given(or initialpose is given). How do you think?

[jihoonl]

Or maybe negative trajectory id to indicate relative pose is not given?

[cschuet]

I would avoid sentinel values to implement an Optional<>. Could you define an InitialPose submessage and have that as an optional field?

[jihoonl]

Sadly, ROS message doesn't support optional field... It always fills with a default value if it isn't set

[cschuet]

I see. How about a repeated field as a poor-man's optional? Is that something people do? @SirVer what do you suggest here?

[SirVer]

My 2c: Add relative_pose and trajectory_id to TrajectoryOptions.msg, since it is not ROS specific (StartTrajectory.srv takes options which is everything concerning the SLAM and topics which is ROS specific). And I think trajectory_id = -1 as sentinel value is the ROS way of life here.

[damienrg]

I set to_trajectory_id to -1. If you want I just pushed the initial-pose-branch on which I am working to see how I started.

[cschuet]

We also need to add the time when the robot saw the other robot under the transformation 'relative_pose'. This is used to look-up the global pose in the world frame.

[damienrg]

Maybe I should open a PR with with actual work to discuss on it?
For now I have encountered the following "flaws":

• initial pose is not handle like pure localization, I think it should be added in proto::TrajectoryBuilderOptions or there should be distinction between "optional arguments" and "mandatory ones".
• the way I handle time is not perfect but for now I do not have a better idea
• it is not possible to record service call in ROS, so I have to start a new trajectory by hand. How did you handle it in your "quality control jobs"?

[cschuet]

Happy to look at any PRs. It's not ideal to write two implementations when only one gets merged, so maybe discuss among yourselves @damienrg and @jihoonl who wants to put in the work to write this thing.

[jihoonl]

Um since I'm in vacation, I will be pretty slow until next week. So, @damienrg feel free to take over. Also feel free to take over implementation in #513

[cschuet]

Enjoy your well-deserved vacation then :-D

[wohe]

Reading this discussion, I wonder if it would not be better to adapt googlecartographer/cartographer#606 to include this information in the trajectory options proto message, similar to how pure localization is handled. This way, there would not even be a need for any change in ROS.

[cschuet]

Yes, that'd make sense, but I think it should be a follow-up PR on top of googlecartographer/cartographer#606.

[damienrg]

What does it mean? Fix #579 to test googlecartographer/cartographer#606, create a new PR in cartographer and drop the fix for #579.
I am more with @wohe to update googlecartographer/cartographer#606.

[SirVer]

I am more with @wohe to update googlecartographer/cartographer#606.

+1.

[cschuet]

Updated googlecartographer/cartographer#606.

[cschuet]

Alright googlecartographer/cartographer#606 is merged. @damienrg I will let you take it from here. Thanks.

[damienrg]

Since https://github.com/googlecartographer/cartographer/pull/606 and as @wohe mentioned there is nothing to do in cartographer_ros as it is handle in TrajectoryBuilderOptions and thus read by start trajectory node.

[kajep09]

Would it be possible to get a short description or an example of how to use the initial_pose option in the configuration files?

[doisyg]

Indeed, an example would be much appreciated

[damienrg]

You can not set it in configuration files, you need to call start_trajectory_main with initial_pose.
For example you can use it this way:

• start slam with cartographer_node
• call finish trajectory when the robot does not move
• call start_trajectory_main --initial_pose '{to_trajectory_id = 0, relative_pose = { translation = { 0., 0., 0. }, rotation = { 0., 0., 0., } } }'

Just keep in mind that the position is relative to the trajectory to_trajectory_id at the specified timestamp (if not specified it means now).

See https://github.com/googlecartographer/cartographer/pull/642#issuecomment-345177084 to have another one example.

[doisyg]

Thanks for your answer, but it is still very unclear to me.

• start slam with cartographer_node
  => OK

• call finish trajectory when the robot does not move
  => Done by calling the ros service /finish_trajectory

• call start_trajectory_main --initial_pose '{to_trajectory_id = 0, relative_pose = { translation = { 0., 0., 0. }, rotation = { 0., 0., 0., } } }'
  => How to do that? there is a ros service /start_trajectory but there is no field for --initial_pose , is start_trajectory_main suppose to be a service or something else?

[damienrg]

I meant rosrun cartographer_ros cartographer_start_trajectory ... -initial_pose '{to_trajectory_id = 0, relative_pose = { translation = { 0., 0., 0. }, rotation = { 0., 0., 0., } } }'.

[doisyg]

Thanks! This is working.
For the ones willing to try with pure localization:

1. Start cartographer node in pure localization mode. By default the map trajectory ID is 0 and the node automatically starts a new one (ID 1)
2. call /finish_trajectory for ID 1
3. rosrun cartographer_ros cartographer_start_trajectory ... -initial_pose '{to_trajectory_id = 0, relative_pose = { translation = { 0., 0., 0. }, rotation = { 0., 0., 0., } } }'.

[WilliamWoo45]

@doisyg @damienrg
I'm wondering the correct steps for setting initial_pose for pure localization mode. Is it like this? :

1. Start cartographer node in pure localization mode by:
   roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/xxx/xxx.bag

2. Call /finish_trajectory by rosservice call /finish_trajectory "trajectory_id: 0"

3. rosrun cartographer_ros cartographer_start_trajectory ... -initial_pose '{to_trajectory_id = 0, relative_pose = { translation = { 0., 0., 0. }, rotation = { 0., 0., 0., } } }

If so, which step should I add the load_state_filename:=/xxx/xxx.bag.pbstream and bag_filename:=/xxx/xxx.bag in?

Thanks!!

[ojura]

Do not confuse Google flags when you are executing Cartographer executables directly (-flag=name, for some types of flags you can also use a space instead of equals) with ROS node or launch file arguments (these use:=).

Also, offline_backpack_2d.launch is not pure localization. It is regular mapping which produces a .pbstream file which you can then load as a map for pure localization with -load_state_filename. Please carefully examine the provided example launch files and the documentation on readthedocs.

[WilliamWoo45]

@ojura
Thanks for all your guidance. I've made it work!!

@doisyg @damienrg
I think the correct steps for setting initial_pose for pure localization mode should be like this:

1. Add the code start_trajectory_with_default_topics=false into the demo_backpack_2d_localization.launch file like this

2. Start cartographer node in pure localization mode by:

roslaunch cartographer_ros demo_backpack_2d_localization.launch \
   load_state_filename:=${HOME}/xxx.bag.pbstream \
   bag_filename:=${HOME}/xxx.bag

1. Set the initial pose in another Terminal right after step 1:
   rosrun cartographer_ros cartographer_start_trajectory ... -configuration_directory '${HOME}/xxx/xxx/cartographer_ros/cartographer_ros/configuration_files' -configuration_basename 'backpack_2d_localization.lua' -initial_pose '{to_trajectory_id = 0, relative_pose = { translation = { 0, 0, 0}, rotation = {0., 0., 0., } } }'

Thanks for all your works!!

[soorajanilkumar]

There is an important field (timestamp) missing in the above syntax. If it is not set, by default I think it takes the pose at the last time stamp of trajectory 0 as the reference frame, which can mess things up.
I found this in issue 913

rosrun cartographer_ros cartographer_start_trajectory -configuration_directory <path-to-config> -configuration_basename <config-filename.lua> -initial_pose '{to_trajectory_id = 0, relative_pose = { translation = {0.0, 0.0, 0. }, rotation = { 0.0, 0.0, 0.0 } } ,timestamp = 0}'
This should set the initial pose of the new trajectory with respect to the map frame hopefully.

Hope this helps anyone looking at the issue in future.