diff --git a/CHANGELOG.md b/CHANGELOG.md index 37f74051..78b9299c 100644 --- 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 index be001313..d7134b93 100644 --- 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 .yaml`. Load these parameter with `rosparam load .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 new file mode 100644 index 00000000..5a1d3902 Binary files /dev/null and b/doc/cone_section.png differ diff --git a/driver/src/sick_generic_laser.cpp b/driver/src/sick_generic_laser.cpp index 0453adcc..b8693fbd 100644 --- 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 index 30098d01..ffe2d82a 100644 --- 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 index 02dee22d..f562be75 100644 --- 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 new file mode 100644 index 00000000..bd23ecd8 --- /dev/null +++ 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: + 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: + 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: + 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 new file mode 100644 index 00000000..47f248fa --- /dev/null +++ b/test/emulator/launch/emulator_tim240.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/test/emulator/src/test_server_thread.cpp b/test/emulator/src/test_server_thread.cpp index b98dfb22..a3155370 100644 --- 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 new file mode 100644 index 00000000..86cc5790 --- /dev/null +++ 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 index abf7d07b..ae0d6952 100644 --- 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 diff --git a/test/scripts/run_linux_ros1_simu_tim240.bash b/test/scripts/run_linux_ros1_simu_tim240.bash new file mode 100644 index 00000000..96314880 --- /dev/null +++ b/test/scripts/run_linux_ros1_simu_tim240.bash @@ -0,0 +1,46 @@ +#!/bin/bash +printf "\033c" +pushd ../../../.. +if [ -f /opt/ros/melodic/setup.bash ] ; then source /opt/ros/melodic/setup.bash ; fi +if [ -f /opt/ros/noetic/setup.bash ] ; then source /opt/ros/noetic/setup.bash ; fi +if [ -f ./devel_isolated/setup.bash ] ; then source ./devel_isolated/setup.bash ; fi + +echo -e "run_simu_tim240.bash: starting tim240 emulation\n" + +# Start roscore if not yet running +roscore_running=`(ps -elf | grep roscore | grep -v grep | wc -l)` +if [ $roscore_running -lt 1 ] ; then + roscore & + sleep 3 +fi + +# Start sick_scan emulator +roslaunch sick_scan emulator_tim240.launch & +sleep 1 + +# Start rviz +# Note: Due to a bug in opengl 3 in combination with rviz and VMware, opengl 2 should be used by rviz option --opengl 210 +# See https://github.com/ros-visualization/rviz/issues/1444 and https://github.com/ros-visualization/rviz/issues/1508 for further details + +rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_tim240.rviz --opengl 210 & +sleep 1 + +# Start sick_scan driver for tim240 +echo -e "Launching sick_scan sick_tim_240.launch\n" +roslaunch sick_scan sick_tim_240.launch hostname:=127.0.0.1 sw_pll_only_publish:=False & +sleep 1 + +# Wait for 'q' or 'Q' to exit or until rviz is closed +while true ; do + echo -e "tim240 emulation running. Close rviz or press 'q' to exit..." ; read -t 1.0 -n1 -s key + if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi + rviz_running=`(ps -elf | grep rviz | grep -v grep | wc -l)` + if [ $rviz_running -lt 1 ] ; then break ; fi +done + +# Shutdown +rosnode kill -a ; sleep 1 +killall sick_generic_caller ; sleep 1 +killall sick_scan_emulator ; sleep 1 +popd +