Wrong pose assigned when start new trajectory with initial pose

[iceicehyhy]

Hi,

I'm trying the pure localization mode now, and i'm able to end the previous trajectory and add a new trajectory with initial pose.

However, the initial pose being added does not follow the instruction I sent. It somehow assigns the robot to another point in the map.

[MichaelGrupp]

1. Is the initial pose relative to the origin of the Cartographer map? Or to another frame? You have to be careful with this, the initial pose has to be relative to the frame you specified as map_framein the options.
2. What pose did you send, what did you get?

[iceicehyhy]

Hi,

thanks for your prompt reply.

the initial pose is set exactly on the map_frame that specified in the config file. What i've done is, I wrote a simple script subscribe to /initial_pose topic that RVIZ published, everytime i got the pose sent from RVIZ, i'll call the service to end the previous trajectory and start a new trajectory with the pose that i got from RVIZ.

The interesting thing is , the initial pose is working on some maps, but the pose is not correct for others. I wonder is there anything i need to do when I build the map? Or any other thing i need to do before i call the /write_state service to save the serialized slam state?

[MichaelGrupp]

Your approach sounds good, that's how I'm also doing it more or less.

This shouldn't be affected by mapping. The only thing that could come to my mind is that the frame ID of the initial pose message doesn't match the frame ID of Cartographer's map frame.

---

Side note: since today you can use the /get_trajectory_states service to conveniently determine the active trajectories that need to be finished in your script. See https://github.com/googlecartographer/cartographer_ros/commit/8bbec6bff77a7e63669db34b9023ad1811e741fd (msg / srv)

[iceicehyhy]

Coz the map I used is the grid map that generated by Cartographer occupancy grid node, is possible that the frame origin of the generated grid map differ from the origin of serialized state map ?

[derekhe89]

I got the same problem, if you solve this problem, can you give me some advice?@ iceicehyhy

[dan9thsense]

I would like to do the exact same thing. @iceicehyhy can you share that script?

[derekhe89]

I found the pose used is not relative to the origin but the last trajectory pose .@dan9thsense

[MichaelGrupp]

The to_trajectory_id parameter of the initial pose you pass to the start_trajectoryexecutable is important, this defines relative to which origin the pose is defined. See here for the concept for and here for the syntax (ignore the dummy values). The ID has to be the one that is used by your frozen, global map. Usually it is 0 (better: check with /get_trajectory_states, see above).

My initial pose handler code works very reliable, unfortunately I can't share it at the moment as it is currently embedded in an proprietary project.

[iceicehyhy]

Hi,

@MichaelGrupp . I somehow figure out the problem, but i'm sure about the root cause. The problem is fixed by using a virtual clock instead of ROS walltime, after i set use_sim_time to TRUE, everything works fine. Any idea why is that so?

[iceicehyhy]

Hi,

@dan9thsense . you can find a copy here: https://github.com/googlecartographer/cartographer_ros/pull/637.

[zhenliysu]

@MichaelGrupp @iceicehyhy Hi, guys. I set -initial_pose="{to_trajectory_id=0,relative_pose={translation={0.0,0.0,0.},rotation={0.,0.,0.0}}}". But it appear at the last submap of trajectory_id 0. Then i changed relative_pose some times, it changes too. But not the correct position( or not the desired position). So i guess the position is relative about the last submap of trajectory_id 0. So what is the reason? could you give me some advice about how to solve this problem, thanks. I am sincerely looking forward to your reply.

[MichaelGrupp]

@zhenliysu The pose is relative to the origin of trajectory 0. That's what to_trajectory_id=... specifies.

[MichaelGrupp]

Oh and: try setting

-initial_pose="{
  to_trajectory_id=0,
  relative_pose={translation={0.0,0.0,0.},rotation={0.,0.,0.0}},
  timestamp = 0
}"

...the "timestamp" field is used to determine the trajectory node to which the initial pose is relative to, as far as I know. I always set it to 0, and it works fine for me.

[zhenliysu]

@MichaelGrupp Thank you for your prompt reply. I will try to set timestamp = 0. In my opinons, i think the origin of trajectory 0 is the first submap pose of trajectory 0. So i feel confused.

[MichaelGrupp]

i think the origin of trajectory 0 is the first submap pose of trajectory 0

Yes, but as far as I know the timestamp parameter can be used to select later poses in trajectory 0 as the relative frame (not sure, never used this feature). Maybe it uses the last known pose in the last submap by default if you don't specify it as timestamp=0.

@gaschler

[zhenliysu]

@MichaelGrupp Yes, when i set timestamp=0, it works normally. thank you very much.

[cschuet]

Closing as answered. Thanks @MichaelGrupp !

[carl1218]

@MichaelGrupp ， Hi, I am running 2d slam online ,when i call rosservice call /finish_trajectory,and set the new trajectory to z-axis=5,i runned: rosrun cartographer_ros cartographer_start_trajectory -configuration_basename=backpack_2d.lua -configuration_directory=/home/calyu/catkin_ws/install_isolated/share/cartographer_ros/configuration_files -initial_pose="{to_trajectory_id=0,relative_pose={translation={0.0,0.0,5},rotation={0.,0.,0.0}}} i can see the map look like this:

I want to save this map ,so than i runned rosservice call /finish_trajectory 1,but the Z axis will change to the initial position.(Z-axis = 0m). look like this:

How can i to set the new trajectory in the new plane (Z-axis ≠ 0) and save this map? could you give me some advice about how to solve this problem, thanks. I am sincerely looking forward to your reply.

[MichaelGrupp]

I wonder what you want to achieve here, modelling height differences in 2D SLAM doesn't make much sense to me. The whole 2D SLAM pipeline assumes the poses to be be in a 2D plane, and also projects out of plane poses down to the plane in different places, e.g. here in the optimization: https://github.com/googlecartographer/cartographer/blob/f060815a7fe9a4cb0bdd03c884c5996e85449f9f/cartographer/mapping/internal/2d/pose_graph_2d.cc#L84

If you want to have e.g. multiple floors with 2D SLAM, I'd rather use separate 2D maps per floor.

[carl1218]

@MichaelGrupp Thank you for your reply. yes ,I want to have e.g multiple floors with 2D SLAM. I want to build two maps in different planes, and then compare them. could you give me some advice about how to solve this problem? thank you.

[MichaelGrupp]

I would just process and store the data separately and use separate Cartographer nodes for each floor. On-the-fly floor changes are not possible with 2D SLAM at the moment.

If you want to have multiple 2D maps aligned with a z-offset in 3D space, you would need an additional coordinate frame to model the floors. If you use ROS, you can define an additional parent TF frame (let's call it "world"), in which your floor maps are located. The static transformation from "world" to each map can be adjusted to model the floor height.

       world
      /     \
     /       \
map1           map2

[carl1218]

@MichaelGrupp Thank you very much.

[carl1218]

@MichaelGrupp Hi， Thank you very much for your advice. in order to achieve the effect I need? can you help me modify it, of course, it's not free. Expected for your reply.

[MarcelSoler]

@MichaelGrupp timestamp=0 info was very helpful. Maybe it would be nice to update the documentation with this.It was not easy to reach the solution.

Thank you

[MichaelGrupp]

I know. The timestamp option is weird and we are currently working on a complete refactor of the start trajectory service that will simplify things a lot. See #1245 and the corresponding RFC.

This will break the current API of this service, but in this case it's a good thing 😉