Skip to content

[ROS2][feat] Allow to load poses from file

Julia Sanchez requested to merge ROS2/feat/LoadPoses into feat/ROS2

Goal

Update with !370 (merged)

Allow to load an external trajectory from a CSV file.

These poses can be used to de-skew the input pointcloud, to deduce a prior pose for the registration algorithm and/or to add a constraint in the SLAM optimization.

Changes

  • Update the rviz plugin package
  • Add a command in command message in lidar_slam package
  • Add ext_sensor in TF tree

Results

Reproduce

Data

  • Example of CSV file :
ext_sensor 
time,x,y,z,x0,y0,z0,x1,y1,z1,x2,y2,z2 
1695972780,-0.348,0.17,-0.131,-0.974,-0.221,0.012,0.2099,-0.89587,0.392,-0.0755,0.3849,0.9198

Build

catkin build --cmake-args -DCMAKE_BUILD_TYPE=Release

Parameters

  • No change

Run

ros2 launch lidar_slam slam_velodyne.launch

ros2 topic pub -1 /slam_command lidar_slam/SlamCommand "{command: 40, string_arg: path/to/ext_poses_ROS.csv}"

Output

You should see :

[ INFO] [1702315740.991950582]: Calibration for ext poses set to : 
1 0 0 0 
0 1 0 0 
0 0 1 0 
0 0 0 1
[ INFO] [1702315740.993038762]: External poses loaded!

Checklist

  • Camel case everywhere except for ROS variables/parameters
  • Lower case for local variables and lambda functions
  • Upper case for pour 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)
  • Space between if, while, for and parenthesis
  • Space between operators and variables: e.g. a + b
  • Space 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 if needed
  • 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

Merge request reports