Release v2.9.1 - Angle correction

* Angle correction (min/max angle settings), fix TIM551 points have bad coordinates. #166 * TiM240 initialization (start measurement)
SICKAG · May 15, 2023 · 6825960 · 6825960
2 parents f8a028a + 7e4e8b3
commit 6825960
Show file tree

Hide file tree

Showing 12 changed files with 317 additions and 7 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -9,6 +9,11 @@ features that will be removed in future versions **Removed** for deprecated feat
 
 ## Released ##
 
+### v2.9.1 - Angle correction
+  - **Fix** Angle correction (min/max angle settings), fix #166
+  - **Fix** TiM240 initialization (start measurement)
+  - **Added** Documentation for Interlacing mode
+
 ### v2.9.0 - RMSxxxx support and NAV350 support
   - **Added** RMSxxxx support, unification of RMS-1xxx and RMS-2xxx Note: RMSxxxx supports ASCII-communication mode only (Cola-A).
   - **Update** #159 (nav310 angle setting compability), merge with NAV310 angle settings branch https://github.com/SICKAG/sick_scan_xd/tree/159-nav310-angle-setting-compability

diff --git a/FAQ.md b/FAQ.md
@@ -265,7 +265,7 @@ In the example the ip address 192.168.0.4 is the laserscanner MRS1104 and the ip
 ```bash
 ifconfig|grep 192.168.0.22
 ```
-## IP Address of Laser Scanner
+## IP Address of Lidar
 
 :question: Question:
 My scanner does not use the default ip address. What shall I do?
@@ -345,3 +345,41 @@ How can I debug sick_generic_caller on ROS-1?
 
 :white_check_mark: Answer:
 Build with compiler option `-g` and run sick_generic_caller as described using a launchfile. Stop sick_generic_caller (Ctrl-C or kill) and save the current ros parameter using `rosparam dump <dumpfile>.yaml`. Load these parameter with `rosparam load <dumpfile>.yaml` and restart sick_generic_caller in gdb or in your IDE. 
+
+## Curved lines on a straight wall
+
+:question: Question:
+The X,Y points of the lidar show a curved line even though the lidar is scanning a straight wall. How can this be?
+
+:white_check_mark: Answer:
+This effect occurs when the lidar has multiple planes that are tilted up or down. In this case, the laser beams of this plane do not lie on a flat plane. Rather, the beams lie on a cone. If the laser beams then hit a wall, the result is a curved course of the lidar points. If the lidar is horizontal and the wall is vertical, this is a hyperbola (see following figure):
+
+![cone_section](doc/cone_section.png)
+
+
+
+This image is generated using the website https://www.intmath.com/plane-analytic-geometry/conic-sections-summary-interactive.php 
+
+Thus, the mathematical laws for a conic section apply, as they are explained e.g. at Wikipedia at https://en.wikipedia.org/wiki/Conic_section.
+
+## Interlacing
+
+:question: Question:
+How should I interpret the scan rate and lidar resolution from the manual? What is the relationship between ROS point cloud publishing rate and scan frequency here?
+
+:white_check_mark: Answer:
+
+The angular resolution and scan frequency is configurable in many lidars such as the LRS-4xxx or MRS-1xxx. Depending on the lidar type, angular resolution and scan frequency can be set in the launch file either via the parameter "scan_cfg_list_entry" or the parameters "ang_res" and "scan_freq". Angular resolution and scan frequency are not independent of each other. If no default settings are used, the values must be selected according to the manual for the respective lidar and set in launch file.
+
+An increase in resolution is achieved by interlacing by a factor of N. This means that N consecutive scans are rotated by a constant angular offset. Each scan in itself still has the physically given angular resolution and frequency. By concatenating N interlaced scans, the angular resolution is increased by the factor N.
+
+Example: The default setting of an MRS-1xxx is 0.25 degrees horizontal angular resolution at 50 Hz scan frequency without interlacing and an angular range of 275 degrees in total. I.e. each scan measures the distance at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ].
+
+If 0.125 degrees horizontal angular resolution is configured at 25 Hz scan frequency, the scans are performed with 2 times interlacing (N=2). Every 2nd scan is horizontally shifted by 0.125 degrees. I.e. each scan measures alternately at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ] and [ ... , 0.125, 0.375, 0.625, 0.875 ... ]. 50 single scans per second resp. 25 interlaced scans per second are sent.
+
+If 0.0625 degrees horizontal angular resolution at 12.5 Hz scan frequency is configured, the scans are performed with 4 times interlacing (N=4). Successive scans are shifted horizontally by 0.0625 degrees each. That is, each scan measures alternately at the hor. angles [ ..., 0.000, 0.250, 0.500, 0.750, ... ], [... , 0.0625, 0.3125, 0.5625, 0.8125 ... ], [... , 0.125, 0.375, 0.625, 0.875 ... ] and [... , 0.1875, 0.4375, 0.6875, 0.9375 ... ]. 50 single scans per second resp. 12.5 interlaced scans per second are sent.
+
+In interlacing mode, laser scan and point cloud messages are published interlaced, too. In rviz, the higher angular resolution is clearly visible when the decay time is increased. 
+If you check the publishing rate of the point cloud messages, you will measure 50 Hz, since 50 interlace messages are sent per second. 
+But the resolution of each single message is 0.125 [deg]. Only by interleaving 4 consecutive messages you get the high resolution of 0.0625 [deg].
+
diff --git a/doc/cone_section.png b/doc/cone_section.png
diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp
@@ -93,7 +93,7 @@
 
 #define SICK_GENERIC_MAJOR_VER "2"
 #define SICK_GENERIC_MINOR_VER "9"
-#define SICK_GENERIC_PATCH_LEVEL "0"
+#define SICK_GENERIC_PATCH_LEVEL "1"
 
 #define DELETE_PTR(p) if(p){delete(p);p=0;}
 

diff --git a/driver/src/sick_lmd_scandata_parser.cpp b/driver/src/sick_lmd_scandata_parser.cpp
@@ -110,9 +110,10 @@ namespace sick_scan
     {
        bool angle_slightly_modified = false;
        float pi_multiplier = *angle_val/M_PI;
-       float check_deviation_to_abs_one = fabs(pi_multiplier) - 1.0;
+       // float check_deviation_to_abs_one = fabs(pi_multiplier) - 1.0;
        // check for a small deviation
-       if (check_deviation_to_abs_one < 10.0 * FLT_EPSILON )
+       // if (check_deviation_to_abs_one < 10.0 * FLT_EPSILON )
+       if (pi_multiplier > 1.0 - 10.0 * FLT_EPSILON || pi_multiplier < -1.0 + 10.0 * FLT_EPSILON)
        {
         float factor =  (*angle_val < 0.0) ? (-1.0) : (1.0);
         *angle_val = factor * (1.0 - FLT_EPSILON) * M_PI;
@@ -526,6 +527,7 @@ namespace sick_scan
                                 msg.angle_max *= -1.0;
 
                               }
+                              ROS_DEBUG_STREAM("msg.angle_min=" << (msg.angle_min*180/M_PI) << ", msg.angle_max=" << (msg.angle_max*180/M_PI) << ", msg.angle_increment=" << (msg.angle_increment*180/M_PI));
 
                               // Avoid 2*PI wrap around, if (msg.angle_max - msg.angle_min - 2*PI) is slightly above 0.0 due to floating point arithmetics
                               bool wrap_avoid = false;
@@ -546,6 +548,7 @@ namespace sick_scan
                               {
                                 msg.angle_increment = (msg.angle_max - msg.angle_min) / (numberOfItems - 1);
                               }
+                              ROS_DEBUG_STREAM("msg.angle_min=" << (msg.angle_min*180/M_PI) << ", msg.angle_max=" << (msg.angle_max*180/M_PI) << ", msg.angle_increment=" << (msg.angle_increment*180/M_PI));
 
                               float *rangePtr = NULL;
 

diff --git a/driver/src/sick_scan_common.cpp b/driver/src/sick_scan_common.cpp
@@ -3613,6 +3613,8 @@ namespace sick_scan
       {
         // the TiM240 operates directly in the ros coordinate system
         // do nothing for a TiM240
+        startProtocolSequence.push_back(CMD_RUN);  // leave user level
+        startProtocolSequence.push_back(CMD_START_SCANDATA);
       }
       else if (this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0)
       {
@@ -4858,7 +4860,8 @@ namespace sick_scan
               {
 
                 float angle = (float)config_.min_ang;
-                if(this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0  // Check and todo: Can we use msg.angle_min for all lidars?
+                if(this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_TIM_240_NAME) == 0
+                || this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0  // Check and todo: Can we use msg.angle_min for all lidars?
                 || this->parser_->getCurrentParamPtr()->getScannerName().compare(SICK_SCANNER_MRS_1XXX_NAME) == 0) // Can we use this for all lidars where msg.angle_min is not 0?
                 {
                   angle = msg.angle_min - angleShift; // LMS-1xxx and MRS-1xxx have 4 interlaced layer with different start angle in each layer, start angle parsed from LMDscandata and set in msg.angle_min

diff --git a/test/emulator/config/rviz_tim240.rviz b/test/emulator/config/rviz_tim240.rviz
@@ -0,0 +1,184 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /PointCloud21
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /cloud
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /sick_tim_240/scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.05000000074505806
+      Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: cloud
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Yaw: 3.133575201034546
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 921
+  Y: 27
diff --git a/test/emulator/launch/emulator_tim240.launch b/test/emulator/launch/emulator_tim240.launch
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Launch sick_scan_emulator -->
+  <arg name="scandatafiles" default="$(find sick_scan)/scandata/20230510_tim240.pcapng.json"/>
+  <arg name="scandatatypes" default="sSN LMDscandata "/>
+  <rosparam command="load" file="$(find sick_scan)/yaml/emulator_port2111.yaml" />
+  <node name="sick_scan_emulator" pkg="sick_scan" type="sick_scan_emulator" output="screen">
+    <param name="scandatafiles" type="string" value="$(arg scandatafiles)"/>
+    <param name="scandatatypes" type="string" value="$(arg scandatatypes)"/>
+    <param name="scanner_type" type="string" value="sick_tim_240"/>
+  </node>
+
+</launch>
+
diff --git a/test/emulator/src/test_server_thread.cpp b/test/emulator/src/test_server_thread.cpp
@@ -510,13 +510,14 @@ void sick_scan::TestServerThread::runWorkerThreadColaCb(socket_ptr p_socket)
         iTransmitErrorCnt = 0;
       }
       // Start or stop scandata thread
-      if(sick_scan::TestcaseGenerator::SendScandataEnabled() == true && m_tcp_send_scandata_thread == 0)
+      bool send_scandata_enabled = sick_scan::TestcaseGenerator::SendScandataEnabled() || m_scanner_type == "sick_tim_240"; // TIM240 does not support "LMCstartmeas"
+      if(send_scandata_enabled == true && m_tcp_send_scandata_thread == 0)
       {
         // Start scandata thread
         m_tcp_send_scandata_thread_running = true;
         m_tcp_send_scandata_thread = new std::thread(&sick_scan::TestServerThread::runWorkerThreadScandataCb, this, p_socket);
       }
-      else if(sick_scan::TestcaseGenerator::SendScandataEnabled() == false && m_tcp_send_scandata_thread != 0)
+      else if(send_scandata_enabled == false && m_tcp_send_scandata_thread != 0)
       {
         // Stop scandata thread
         closeScandataThread();

diff --git a/test/emulator/yaml/emulator_port2111.yaml b/test/emulator/yaml/emulator_port2111.yaml
@@ -0,0 +1,14 @@
+# Configuration of sick_scan_emulator
+# sick_scan_emulator implements a simple tcp server, simulating a Tim875S for unittests.
+# It implements a simple tcp server, accepting tcp connections from clients and generating telegrams to test the ros driver for sim localization.
+sick_scan:
+
+  # Server configuration. See Operation-Instruction-v1.1.0.241R.pdf, page 51, "IP port number and protocol" for default tcp ports.
+  test_server:
+    result_telegrams_tcp_port:       2201  # Default tcp port for sim_loc_test_server is 2201 (ip port number of the localization controller sending localization results)
+    cola_telegrams_tcp_port:         2111  # For requests and to transmit settings to the localization controller: IP port number 2111 and 2112 to send telegrams and to request data, SOPAS CoLa-A or CoLa-B protocols
+    start_scandata_delay:               1  # Delay between scandata activation ("LMCstartmeas" request) and first scandata message, default: 1 second
+    result_telegrams_rate:             10  # Rate to generate and send result port telegrams
+    result_testcases_topic:          "/sick_scan/emulator/result_testcases"  # ROS topic to publish testcases with result port telegrams (type SickLocResultPortTestcaseMsg)
+    result_testcases_frame_id:       "result_testcases" # ROS frame id of testcase messages (type SickLocResultPortTestcaseMsg)
+
diff --git a/test/pcap_json_converter/pcap_json_converter.cmd b/test/pcap_json_converter/pcap_json_converter.cmd
@@ -13,6 +13,7 @@ echo PYTHON_DIR=%PYTHON_DIR%
 python --version
 @echo.
 
+python pcap_json_converter.py --pcap_filename=../emulator/scandata/20230510_tim240.pcapng
 python pcap_json_converter.py --pcap_filename=../emulator/scandata/20230112_01_mrs1000_layer_1111_50hz_0.25deg.pcapng
 python pcap_json_converter.py --pcap_filename=../emulator/scandata/20230112_02_mrs1000_layer_1000_50hz_0.25deg.pcapng
 python pcap_json_converter.py --pcap_filename=../emulator/scandata/20230112_03_mrs1000_layer_0100_50hz_0.25deg.pcapng