Skip to content

[fix][feat][PV][ROS] Rework the pose setting

Tong Fu requested to merge fix/FixInitializeMapsAndPose into master


Issue description

Previously, TransformOdom function was used in 2 cases:

  1. To change the reference frame of the SLAM trajectory. In this case, we'd like to transform also the maps into the reference frame.
  2. To initialize the slam pose, the maps should be fixed in this case

So when users wanted to initialize the slam pose and maps, the pose needed to be initialized before setting the initial maps.


In the case of slam initialization with maps and a pose:

  • In slam lib: a new function JumpPose is created. The SLAM initialization should only be used to jump the initial pose. When a user want to change the initial pose during slam via this category, a warning is displayed.
  • In PV wrapping: a button Set Initial Slam is added to initialize slam with maps and the InitPose
  • In ROS wrapping: the old function 'SetTworld' is replaced by JumpPose

In the case of changing a pose into a new one during slam: it is considered to be equivalent to move the odom so that the pose corresponds to the new pose.

TransformOdom function has an input argument offset. It is used to change the reference frame to odom_new, where: odom_new = odom * **offset**. There are 3 functions which use TransformOdom:

  • (new) SetInitialPose moves odom so that the initial pose corresponds to the input pose
  • (new) SetCurrentPose moves odom so that the last pose corresponds to the input pose
  • ResetTrajWithTworldInit moves odom after PGO so that the initial pose returns to TworldInit

The offset to move odom is computed differently in these functions.


New interface in LidarView:



  • Camel case everywhere except for ROS variables/parameters
  • Lower case for local variables and lambda functions
  • Upper case for members, methods and tool functions (in Utils)
  • Precise namespace when calling a function (or this->X or classe.X)
  • Align code (for multiline if and while, "&&" or "||" go in upper line to ensure alignement)
  • Check your spaces
    • between if, while, for and parenthesis
    • between operators and variables: e.g. a + b
    • after ","
  • Mind your commit titles/desc (plurals, he/she + "s", correct tags, title should begin by a verb...)
  • Function names should start with a verb, variable names should start with a name
  • Macros should be between {}
  • Do not use negative boolean (i.e. noJoe)
  • Check minimal size of the types (double -> float -> int -> uint)
  • Check const and ref in functions arguments
  • References should be written "type& name", not "type &name"
  • Update documentation
  • Add MR labels [ROS]/[ROS2]/[PV]
  • If ros/ros2, update task table here
  • Add a comment over each non trivial function in header files
  • Add a header to each new file

What remains to solve

  • MR for ros2_wrapping ros2 tests
  • After this MR merged, in MR!405: for the slam initialization tool bar, click OK should call the SetInitialSlam function in VTK. @timothee.couble
Edited by Tong Fu

Merge request reports