SICKAG / sick_scan_xd

Based on the sick_scan drivers for ROS1, sick_scan_xd merges sick_scan, sick_scan2 and sick_scan_base repositories. The driver supports both Linux (native, ROS1, ROS2) and Windows (native and ROS2).
Apache License 2.0
99 stars 84 forks source link

Usage of the Driver for Linux generic without ROS #169

Closed ManuelFreudenreich closed 1 year ago

ManuelFreudenreich commented 1 year ago

Hi, I work with a sick multiscan 100 ( or 136 did not get the difference yet ) and try to write a wrapper for the driver to provide the sensor data to my software. Therefore I installed the driver ( which seems to work fine ) and used the cpp example ( https://github.com/SICKAG/sick_scan_xd/blob/master/examples/cpp/minimum_sick_scan_api_client.cpp ) to start my implementation. The problem is now I came across different problems ( I didn't know if i should open one issue per problem so I went with a overall usage in generic Linux ticket ):

  1. When I use the functions SickScanApiLoadLibrary and SickScanApiUnloadLibrary as used in the example I get a linking error with "undefined reference to ..." but I am able to find the functions in the included header so I am not sure whats the problem there ? But this is also not that big of a problem because without those functions it still works and I get the data in the callback to a function defined like in the example.
  2. Now I want to use the driver by using a member function of my wrapper class but because there is no parameter in the function call for a instance of my wrapper I am not sure how to use the functions to get the data of the sensor inside my member function callback. Could you please explain to me how to get this working or what the thoughts are on how to use the driver?
  3. I miss some functions to get sensor information is there something like that what I did not find or is there something planned like that ?

If I did not find something in the documentation or there is something obvious I am sorry I was not able to find answers to my questions by going through the repository. Also I did not find anything in the open or closed issues after some search.

Thank you and best regards Manuel Freudenreich

rostest commented 1 year ago

Thanks for your feedback! The example minimum_sick_scan_api_client.cpp is a good starting point for implementation and integration of the driver into your own application.

  1. About the undefined reference to SickScanApiLoadLibrary and SickScanApiUnloadLibrary: These functions only load and unload the shared library (libsick_scan_shared_lib.so on Linux resp. sick_scan_shared_lib.dll on Windows). They are declared in sick_scan_api.h and implemented in sick_scan_xd_api_wrapper.c. We recommend to compile and link sick_scan_xd_api_wrapper.c in your own project to load and unload the shared library during runtime. If your application can call the SickScanApi-functions without SickScanApiLoadLibrary resp. SickScanApiUnloadLibrary, you probably link directly (statically) against the library. This is possible and correct, as long as the library and the application are bundled and compiled with identical compilers and compiler settings. To reduce dependencies, we recommend to build your application with sick_scan_xd_api_wrapper.c and to load resp. unload the shared libsick_scan_shared_lib.so during runtime.

  2. About member function callbacks: Since each process should serve only one lidar with a single TCP-connection, the idea is to use the API and the library callbacks as a singleton. This means, that you can have only one (singleton) set of callbacks in your application for communication and data exchange with your lidar. E.g. you can declare a member function static to implement the callback. Alternatively, you could define a static function, which simply delegates the data to a member function of your instance. Or you could implement a wrapper class using a singleton pattern with static callbacks. Within the static callback, we recommend to just copy the scan data into a threadsafe fifo and to process the data in a different thread. This way the data processing thread will avoid blocking any callbacks.

  3. About sensor information: Currently the sensor status is only given by the API return values. If SickScanApiInit returns succesful (SICK_SCAN_API_SUCCESS resp. 0) and a registered callback receives pointcloud messages continously, both lidar and library are working as expected.

ManuelFreudenreich commented 1 year ago

Thank you for the detailed and quick response. I will look into it an give you feedback if it worked for me when I find time.

ManuelFreudenreich commented 1 year ago

I found time to look further into the topic and I have the following comments:

  1. Fine for me and understandable.

  2. First of all I understand the concept how you intend to use the software and I can imagine this is influenced by the way ros is used. The problem is for my use case and for example a small application just fusing the data of two sensors together the approach "each process should serve only one lidar" seems very limiting to me. In the small application example it should from my perspective not be necessary to implement inter process communication and such things. Therefore my experiences using libraries is to be able to use a context for such callbacks works great and enables for example multi threaded application to use the library in a very flexible way. Just as an example and to be clear what I mean here is link for more information "https://rufflewind.com/2016-09-21/function-pointer-context".

  3. Ok thank you good to know but what is with information about the sensor as for example model type, number of layers or number of high resolution layers. Something to be able to distinguish different types of sensors and provide information to the user.

  4. I also have a new question: Why is the point cloud message I get from the sensor unordered ( in my understanding it should be clear how its ordered from sensor side ) or could this be depending on the sensor I have ?

Thanks in advance.

michael1309 commented 1 year ago

Thank you for your valuable feedback. Regarding your new question:

For most lidars, a well-ordered point cloud is returned. With multiScan, however, a different principle is used. Here, so-called segments are transmitted. The starting angles of the different segments are different. This is due to the measuring principle of this type of lidar. In order to enable a timely processing of the point clouds, we parse the segments and convert them into point clouds. If the application does not necessarily need the data close to real time, you can subscribe to the topic "/cloud_fullframe". There, the segment data is collected internally in the driver and exported when a 360 degree round trip has been completed. Again, the starting values are different for each layer. But they always contain one 360 degree round trip per layer.

ManuelFreudenreich commented 1 year ago

Thank you for the explanation about the new question.

For me at the moment point 2 is very important so are you able to give a statement about that ? Will there be effort to change the code in the direction it is usable in a I would say more general way ?

aiplemaSICKAG commented 1 year ago

Hello, we are currently looking into how we can solve your issue in the best way and integrate it into the next release. In the mean time, for the application fusing data from two sensors you could try a workaround based on the apiHandle parameter which is passed to the callback function. This is unique per driver instance and therefore should allow you to recover the context from within the callback function.

Best regards Manuel

ManuelFreudenreich commented 1 year ago

Thank you for the response and I am looking forward to here from you. I also thought about using the apiHandle I will think about it.

At the moment I have another question related to having two sensors simultaneously running in the same network.
I use launch files for the sensors but I have some trouble with it. I created one launch file for every sensor. I changed the "hostname" to the ip address of the sensor, the "udp_receiver_ip" to the ip of my laptop and tried to get data from both sensors. As expected it does not work like that because they use the same udp settings where data is getting some kind of "fused" but also the interfere each other.

Because of that I thought i would just need to change the "udp_port" in the launch file and the "Port" in the "Streaming settings" of the web interface of the second sensor. Unfortunately I was not able to get both running like that at the same time. My question is now where is my error and how do I need to configure both sensors and launch files ?

Also do I assume it correct the settings value "port" in the launch file needs to stay on 2115 because it defines the port on the sensor not on the laptop ?

Thank you for the help.

rostest commented 1 year ago

Thank you for your feedback. Currently you can run two sensors simultaneously by starting two processes with two different launchfiles. For each sensor, its IP address and UDP port must be configured differently in the corresponding launchfile. Example with client IP 192.168.0.100 (your laptop running sick_scan_xd):

Launchfile configuration for 1.st sensor (sensor IP 192.168.0.1, udp port 2115):

        <param name="hostname" type="string" value="192.168.0.1" />           <!-- IP address of multiScan136 to post start and stop commands, f.e. "192.168.0.1" (default) -->
        <param name="udp_receiver_ip" type="string" value="192.168.0.100" />  <!-- UDP destination IP address (ip address of udp receiver), f.e. "192.168.0.140" -->
        <param name="udp_port" type="int" value="2115" />                     <!-- default udp port for multiScan136 resp. multiScan136 emulator is 2115 -->

Launchfile configuration for 2.nd sensor (sensor ip 192.168.0.2, udp port 2116):

        <param name="hostname" type="string" value="192.168.0.2" />           <!-- IP address of multiScan136 to post start and stop commands, f.e. "192.168.0.1" (default) -->
        <param name="udp_receiver_ip" type="string" value="192.168.0.100" />  <!-- UDP destination IP address (ip address of udp receiver), f.e. "192.168.0.140" -->
        <param name="udp_port" type="int" value="2116" />                     <!-- default udp port for multiScan136 resp. multiScan136 emulator is 2115 -->

The sensor's IP address and UDP port must be configured and written to the sensor's EEPROM using SOPAS Air (web GUI, open sensor ip in your browser, http://192.168.0.1 or http://192.168.0.2) or SOPAS ET under Windows.

Please note that the sick_scan_xd library is currently not guaranteed to be fully thread-safe. We recommend the use of ROS-1 or ROS-2 for multi-sensor systems.

rostest commented 1 year ago

Many thanks for the hint about UDP port configuration by parameter udp_port versus port. Currently both parameter must be set identical to the UDP port of the Multiscan lidar. The launchfile configuration for a 2. nd sensor with e.g. sensor ip 192.168.0.2 and udp port 2116 is therefore:

        <param name="hostname" type="string" value="192.168.0.2" />           <!-- IP address of multiScan136 to post start and stop commands, f.e. "192.168.0.1" (default) -->
        <param name="udp_receiver_ip" type="string" value="192.168.0.100" />  <!-- UDP destination IP address (ip address of udp receiver), f.e. "192.168.0.140" -->
        <param name="udp_port" type="int" value="2116" />                     <!-- default udp port for multiScan136 resp. multiScan136 emulator is 2115 -->
        <param name="port" type="int" value="2116" />                         <!-- IP port of multiScan136 to post start and stop commands -->

We will remove the port parameter in the future (i.e. the UDP port is set exclusively by parameter udp_port). Until then, please set both udp_port and port to identical values.

bazzou-mohammed commented 1 year ago

Hello, If I understand correctly what you have montioned in this last answer: we have to set up two processes (1 and 2) for the two scanners, then we change the configuration as you have done in each launch file(sick_multiscan.launch) of the two scanners. My question is: which launch file should I put in the command argument on visual studio? thanks for your help

rostest commented 1 year ago

@bazzou-mohammed You're right. Start the application twice with different launchfiles for each scanner. You can debug one of them in Visual Studio, or both concurrently in 2 Visual Studio instances, or attach the debugger at runtime alternating to one or the other process (Debug -> Attach to Process).

bazzou-mohammed commented 1 year ago

Hello, Thank you for your reply. Trying to start the application for one of the two scanners using the corresponding launcher file, here is where I ended up: image thanks for your help

rostest commented 1 year ago

Sopas commands "LFPangleRangeFilter" and "ScanDataEthSettings" are specific for the Multiscan136 lidar. Make sure that the launchfile corresponds to your lidar. Use e.g. sick_lms_5xx.launch for LMS-511 scanners, sick_multiscan.launch for Multiscan136 scanner, and so on. See https://github.com/SICKAG/sick_scan_xd/blob/master/USAGE.md for a list of supported scanners and their corresponding launchfile. Please post all log messages or screenshots of the complete initialization from start, if the problem remains.

bazzou-mohammed commented 1 year ago

Thank you for your reply. The scanner I'm working with is not the Multiscan136, but rather an LMS-511. Can you give me some specific advice about this scanner model?

rostest commented 1 year ago

To use two LMS-511 scanners simultanously in two processes, both scanners must have different IP addresses, e.g. 192.168.0.1 (default ip address) and 192.168.0.2 (2.nd ip address). Use SOPAS ET to configure the IP addresses and store them in the lidars EEPROM.

Then start sick_scan_xd_api_test (resp. an application using these lidars) with sick_lms_5xx.launch and overwrite the IP address by commandline, e.g.

start "sick_scan_xd_api_test 192.168.0.1" .\build_win64\Debug\sick_scan_xd_api_test.exe ./launch/sick_lms_5xx.launch hostname:=192.168.0.1
start "sick_scan_xd_api_test 192.168.0.2" .\build_win64\Debug\sick_scan_xd_api_test.exe ./launch/sick_lms_5xx.launch hostname:=192.168.0.2

This way you have started two processes using the same launchfile, just with a different hostname (IP-address) for each scanner. Attach the Visual Studio Debugger to one or both processes if required.

Alternatively, you can specify the scanners IP addresses in two different launchfiles (i.e. change parameter hostname to 192.168.0.2 in the 2.nd launchfile). For ROS, parameter cloud_topic and nodename must also be different for both lidars. See https://github.com/SICKAG/sick_scan_xd/blob/master/FAQ.md#how-to-run-multiple-sensors-concurrently for a TiM-7xx example.

rostest commented 1 year ago

I am closing this issue for now. Please do not hesitate to reopen the ticket if required.