Skip to content

[ROS][refact] Parse CSV

Tong Fu requested to merge ROS/refact/ParseCSV into master

Goal

The CSV reader needs to be improved.

Before, loop closure indices are provided by a csv file. It's difficult to know loop closure indices before a slam map is built. So we want to save trajectory with index information so that one can get loop closure indices with saved trajectory. So that users can:

  • Play slam and stop the bag when a loop closure is observed.
  • Save trajectory
  • Open trajectory with a visual tool and get the loop closure indices
  • Create a csv file of loop closure indices. More than one loop can be added in the file

Changes

  • Save trajectory with index term

  • Add helper function ParseSentence

    • Input: a string of currentLine, the delimiter
    • Output: a vector of words
    • Function:
      • remove the extra spaces before and after the delimiter
      • extract words in the currentLine
      • if the delimiter is not ,, check for comma decimal separator and change it to point

  • Add helper function ParseCSV

    • Input: CSV file path, startLineIdx which indicates data position, the delimiter
    • Output: a vector of sentences (sentence = a vector of strings)
    • Function:
      • Read each line of CSV file started from line startLineIdx and parse it into sentence by calling ParseSentence function
      • Check each element in a sentence has a numerical value

  • Overload ReadCSV. Globally, it does two things, find the delimiter and parse CSV (by calling ParseCSV function).

    • ReadCSV(path, nbHeaderLines, nbFields) is the previous function. Given a csv path and number of header lines, this function get the first data line, and try to separate this line into several fields with delimiters in a list of delimiters. If one delimiter can separate the line into nbFields, we use this delimiter to parse csv. Then it check whether or not each sentence has the nbFields, if not reject the line.
    • ReadCSV(path, nbHeaderLines, fieldsToCheck) is added to check whether or not the required fields exist in the CSV file (headerline)
      • First, we look for the position of a field of fieldsToCheck in the headerline. Then the delimiter is found by looking to the left and to the right of the position of this field. The extra spaces case is considered.
      • Then it reorders the lines in the same order of required fieldsToCheck.

  • Refact ReadPose function to check csv fields. Since index term is added when saving slam trajectory and this term is not needed when we want to load pose, we check only the fields we need when reading pose from a csv field.

Results

Data to test

  • I used car_loop
  • data : weird csv files for the test of parse csvCSV_test.zip

Build

catkin_make -j --cmake-args -DCMAKE_BUILD_TYPE=Release

Run

roslaunch lidar_slam slam_velodyne.py \
rosbag play --clock path/to/bag

Then use Reset trajectory or Load loop indices command to read csv files

Output

Test of read CSV function

  • test1
[ INFO] [1701864832.482835285, 1698334274.503147239]: time = 0.00000000000000
1.00000000000000, 1.00000000000000, 0.00000000000000, 1.00000000000000
0.00000000000000, 2.00000000000000, 0.00000000000000, 2.00000000000000
0.00000000000000, 0.00000000000000, 1.00000000000000, 3.00000000000000
0.00000000000000, 0.00000000000000, 0.00000000000000, 1.00000000000000
  • test2
[ INFO] [1701865137.454313067, 1698334274.692999599]: time = 2.00000000000000
3.00000000000000, 7.00000000000000, 11.00000000000000, 2.00000000000000
8.00000000000000, 4.00000000000000, 12.00000000000000, 5.00000000000000
9.00000000000000, 10.00000000000000, 1.00000000000000, 6.00000000000000
0.00000000000000, 0.00000000000000, 0.00000000000000, 1.00000000000000
  • test3
[ INFO] [1701865273.291824755, 1698334274.891894407]: time = 1.00000000000000
5.00000000000000, 8.00000000000000, 11.00000000000000, 2.00000000000000
6.00000000000000, 9.00000000000000, 12.00000000000000, 3.00000000000000
7.00000000000000, 10.00000000000000, 13.00000000000000, 4.00000000000000
0.00000000000000, 0.00000000000000, 0.00000000000000, 1.00000000000000
  • test4
[ INFO] [1701867612.904658175, 1698334274.799694286]: time = 1.00000000000000
5.00000000000000, 8.00000000000000, 11.00000000000000, 2.00000000000000
6.00000000000000, 9.00000000000000, 12.00000000000000, 3.00000000000000
7.00000000000000, 10.00000000000000, 13.00000000000000, 4.00000000000000
0.00000000000000, 0.00000000000000, 0.00000000000000, 1.00000000000000
  • test5
[ INFO] [1702044445.861313466, 1698334282.595179461]: time = 6.000000000000
4.000000000000,7.000000000000,10.000000000000,1.100000023842
5.000000000000,8.000000000000,11.000000000000,2.500000000000
1.000000000000,9.000000000000,12.000000000000,3.700000047684
0.000000000000,0.000000000000,0.000000000000,1.000000000000

What remains to solve

  • Bring features in ros2_wrapping

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
  • Adda header to each new file
Edited by Tong Fu

Merge request reports