search

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
89 stars 81 forks source link

How can I merge two NAV350 LiDAR units for slam? #330

Open kkk3449 opened 1 month ago

kkk3449 commented 1 month ago

Hello, I want to merge two NAV350 units for autonomous robot navigation. The IP and port of the NAV350s mounted on the robot are as follows.

IP: 192.168.127.32 Port:2112 IP: 192.168.127.32 Port:2112 ![Untitled (8)](https://github.com/SICKAG/sick_scan_xd/assets/60597598/70d84648-3f2a-4b28-8ada-fd50d938e974) ![그림1_nav350_re](https://github.com/SICKAG/sick_scan_xd/assets/60597598/94cf5577-b4cb-48eb-bc9c-b9d52c96c8a3) I successfully received each set of scan data using the `roslaunch sick_scan_xd sick_nav_350.launch` command, and I was also able to receive them simultaneously with some modifications to the code. However, when trying to merge them using the `sick_multiscan.launch` command, an error occurred and it failed. ```[ INFO] [1715830988.130370534]: CustomPointCloudConfiguration(cloud_unstructured_segments): frameid = world [ INFO] [1715830988.130380150]: CustomPointCloudConfiguration(cloud_unstructured_segments): coordinate_notation = 0 [ INFO] [1715830988.130390728]: CustomPointCloudConfiguration(cloud_unstructured_segments): update_method = 1 [ INFO] [1715830988.130401701]: CustomPointCloudConfiguration(cloud_unstructured_segments): fields_enabled = i,x,y,z [ INFO] [1715830988.130410534]: CustomPointCloudConfiguration(cloud_unstructured_segments): echos_enabled = 0,1,2 [ INFO] [1715830988.130422068]: CustomPointCloudConfiguration(cloud_unstructured_segments): layers_enabled = 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 [ INFO] [1715830988.130433732]: CustomPointCloudConfiguration(cloud_unstructured_segments): reflector_enabled = 0,1 [ INFO] [1715830988.130442253]: CustomPointCloudConfiguration(cloud_unstructured_segments): infringed_enabled = 0,1 [ INFO] [1715830988.130462819]: CustomPointCloudConfiguration(cloud_unstructured_segments): range_filter = (0.050,999.000,1) [ INFO] [1715830988.130886374]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): publish = 1 [ INFO] [1715830988.130899825]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): topic = /cloud_unstructured_fullframe [ INFO] [1715830988.130911883]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): frameid = world [ INFO] [1715830988.130921131]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): coordinate_notation = 0 [ INFO] [1715830988.130928769]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): update_method = 0 [ INFO] [1715830988.130939754]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): fields_enabled = i,x,y,z [ INFO] [1715830988.130948515]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): echos_enabled = 0,1,2 [ INFO] [1715830988.130958907]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): layers_enabled = 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 [ INFO] [1715830988.130967100]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): reflector_enabled = 0,1 [ INFO] [1715830988.130974971]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): infringed_enabled = 0,1 [ INFO] [1715830988.130986573]: CustomPointCloudConfiguration(cloud_unstructured_fullframe): range_filter = (0.050,999.000,1) [ INFO] [1715830988.131389000]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): publish = 1 [ INFO] [1715830988.131403066]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): topic = /cloud_polar_unstructured_segments [ INFO] [1715830988.131409937]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): frameid = world [ INFO] [1715830988.131421478]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): coordinate_notation = 1 [ INFO] [1715830988.131428212]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): update_method = 1 [ INFO] [1715830988.131438438]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): fields_enabled = azimuth,elevation,i,range [ INFO] [1715830988.131446936]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): echos_enabled = 0,1,2 [ INFO] [1715830988.131457318]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): layers_enabled = 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 [ INFO] [1715830988.131467818]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): reflector_enabled = 0,1 [ INFO] [1715830988.131475874]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): infringed_enabled = 0,1 [ INFO] [1715830988.131487752]: CustomPointCloudConfiguration(cloud_polar_unstructured_segments): range_filter = (0.050,999.000,1) [ INFO] [1715830988.131879975]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): publish = 1 [ INFO] [1715830988.131893893]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): topic = /cloud_polar_unstructured_fullframe [ INFO] [1715830988.131900788]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): frameid = world [ INFO] [1715830988.131912280]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): coordinate_notation = 1 [ INFO] [1715830988.131919527]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): update_method = 0 [ INFO] [1715830988.131928562]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): fields_enabled = azimuth,elevation,i,range [ INFO] [1715830988.131937308]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): echos_enabled = 0,1,2 [ INFO] [1715830988.131947817]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): layers_enabled = 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 [ INFO] [1715830988.131956916]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): reflector_enabled = 0,1 [ INFO] [1715830988.131965331]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): infringed_enabled = 0,1 [ INFO] [1715830988.131976802]: CustomPointCloudConfiguration(cloud_polar_unstructured_fullframe): range_filter = (0.050,999.000,1) [ INFO] [1715830988.132363303]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): publish = 1 [ INFO] [1715830988.132379386]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): topic = /cloud_all_fields_fullframe [ INFO] [1715830988.132387792]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): frameid = world [ INFO] [1715830988.132397073]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): coordinate_notation = 3 [ INFO] [1715830988.132404488]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): update_method = 0 [ INFO] [1715830988.132414768]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): fields_enabled = azimuth,echo,elevation,i,layer,range,reflector,x,y,z [ INFO] [1715830988.132424087]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): echos_enabled = 0,1,2 [ INFO] [1715830988.132434923]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): layers_enabled = 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 [ INFO] [1715830988.132443312]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): reflector_enabled = 0,1 [ INFO] [1715830988.132451475]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): infringed_enabled = 0,1 [ INFO] [1715830988.132463134]: CustomPointCloudConfiguration(cloud_all_fields_fullframe): range_filter = (0.000,999.000,0) [ INFO] [1715830988.132760131]: MsgPackThreads: Start msgpack converter, udp receiver and msgpack exporter, receiving from :2115 [ INFO] [1715830988.132790843]: MsgPackThreads: udp receiver for imu data started, receiving from :7503 [ INFO] [1715830988.132819340]: MsgPackThreads: initializing sopas tcp (192.168.127.31:2112, timeout:5, binary:0) [ INFO] [1715830988.146704160]: use_generation_timestamp:=0, using lidar send timestamp instead of generation timestamp for software pll converted message timestamp. [ INFO] [1715830988.147666975]: Publishing on topic "/sick_multiscan/scan", qos=10 [ INFO] [1715830988.148198571]: SickCloudTransform: add_transform_xyz_rpy = (0,0,0,0,0,0) [ INFO] [1715830988.148215247]: SickCloudTransform: azimuth_offset = 0 [deg] [ INFO] [1715830988.148228953]: SickCloudTransform: additional 3x3 rotation matrix = { (1,0,0), (0,1,0), (0,0,1) } [ INFO] [1715830988.148240389]: SickCloudTransform: apply 3x3 rotation = false [ INFO] [1715830988.148251528]: SickCloudTransform: additional translation = (0,0,0) [ INFO] [1715830988.148263750]: SickCloudTransform: check_dynamic_updates = false [ INFO] [1715830988.148279642]: MsgPackThreads: initializing device [ INFO] [1715830988.148308687]: sick_scan_xd: Tcp::open: connecting to 192.168.127.31:2112 ... [ INFO] [1715830988.148898414]: sick_scan_xd Tcp::open: connected to 192.168.127.31:2112 [ INFO] [1715830988.148909636]: MsgPackThreads: initializing services [ INFO] [1715830988.148936505]: SickThread TcpRecvThread started. [ INFO] [1715830988.149302307]: SickScanServices: service "/multiScan/ColaMsg" created ("/multiScan/ColaMsg") [ INFO] [1715830988.149521003]: SickScanServices: service "/multiScan/ECRChangeArr" created ("/multiScan/ECRChangeArr") [ INFO] [1715830988.149737298]: SickScanServices: service "/multiScan/GetContaminationResult" created ("/multiScan/GetContaminationResult") [ INFO] [1715830988.149948872]: SickScanServices: service "/multiScan/LIDoutputstate" created ("/multiScan/LIDoutputstate") [ INFO] [1715830988.150170919]: SickScanServices: service "/multiScan/SCdevicestate" created ("/multiScan/SCdevicestate") [ INFO] [1715830988.150379300]: SickScanServices: service "/multiScan/SCreboot" created ("/multiScan/SCreboot") [ INFO] [1715830988.150586441]: SickScanServices: service "/multiScan/SCsoftreset" created ("/multiScan/SCsoftreset") [ INFO] [1715830988.150852350]: SickScanServices: service "/multiScan/SickScanExit" created ("/multiScan/SickScanExit") [ INFO] [1715830988.150869859]: MsgPackThreads: ros services initialized [ INFO] [1715830988.150879959]: SickScanServices: Sending request "sMN SetAccessMode 3 client" [ INFO] [1715830988.150916109]: Sending : sMN SetAccessMode 3 client [ WARN] [1715830993.151190600]: Timeout during waiting for new datagram [ INFO] [1715830993.151275141]: sendSOPASCommand: no full reply available for read after 5000 ms [ INFO] [1715830993.151320001]: Receiving: [ INFO] [1715830993.151353115]: SOPAS Communication -Error unexpected Sopas answer for request sMN SetAccessMode 3 client [ERROR] [1715830993.151463656]: ## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command "sMN SetAccessMode 3 client" [ERROR] [1715830993.151500024]: ## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command"sMN SetAccessMode 3 client" [ INFO] [1715830993.151527396]: SickScanServices: Sending request "sWN FREchoFilter 2" [ INFO] [1715830993.151572517]: Sending : sWN FREchoFilter 2 [ WARN] [1715830998.151762570]: Timeout during waiting for new datagram [ INFO] [1715830998.151855299]: sendSOPASCommand: no full reply available for read after 5000 ms [ INFO] [1715830998.151944163]: Receiving: [ INFO] [1715830998.151974983]: SOPAS Communication -Error unexpected Sopas answer for request sWN FREchoFilter 2 [ERROR] [1715830998.152004443]: ## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command "sWN FREchoFilter 2" [ERROR] [1715830998.152035176]: ## ERROR SickScanServices::sendSopasCmdCheckResponse() failed on sending command"sWN FREchoFilter 2" [ERROR] [1715830998.152080954]: ## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse("sWN FREchoFilter 2") failed. [ INFO] [1715830998.152119758]: SickScanServices: Sending request "sMN SetAccessMode 3 client" [ INFO] [1715830998.152164521]: Sending : sMN SetAccessMode 3 client [ WARN] [1715831003.152443159]: Timeout during waiting for new datagram [ INFO] [1715831003.152553042]: sendSOPASCommand: no full reply available for read after 5000 ms [ INFO] [1715831003.152596126]: Receiving: [ INFO] [1715831003.152620606]: SOPAS Communication -Error unexpected Sopas answer for request sMN SetAccessMode 3 client [ERROR] [1715831003.152661672]: ## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command "sMN SetAccessMode 3 client" [ERROR] [1715831003.152684428]: ## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command"sMN SetAccessMode 3 client" [ INFO] [1715831003.152708420]: SickScanServices: Sending request "sRN FREchoFilter" [ INFO] [1715831003.152746370]: Sending : sRN FREchoFilter [ WARN] [1715831008.153025612]: Timeout during waiting for new datagram [ INFO] [1715831008.153099980]: sendSOPASCommand: no full reply available for read after 5000 ms [ INFO] [1715831008.153143964]: Receiving: [ INFO] [1715831008.153169625]: SOPAS Communication -Error unexpected Sopas answer for request sRN FREchoFilter [ERROR] [1715831008.153202599]: ## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command "sRN FREchoFilter" [ERROR] [1715831008.153241276]: ## ERROR SickScanServices::queryMultiScanFiltersettings(): sendSopasAndCheckAnswer("sRN FREchoFilter") failed or unexpected response: "", expected: "sRA FREchoFilter" [ INFO] [1715831008.153271816]: SickScanServices: Sending request "sMN SetAccessMode 3 client" [ INFO] [1715831008.153303254]: Sending : sMN SetAccessMode 3 client [ WARN] [1715831013.153493007]: Timeout during waiting for new datagram [ INFO] [1715831013.153582093]: sendSOPASCommand: no full reply available for read after 5000 ms [ INFO] [1715831013.153622428]: Receiving: [ INFO] [1715831013.153646422]: SOPAS Communication -Error unexpected Sopas answer for request sMN SetAccessMode 3 client [ERROR] [1715831013.153695404]: ## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command "sMN SetAccessMode 3 client" [ERROR] [1715831013.153731186]: ## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command"sMN SetAccessMode 3 client" [ INFO] [1715831013.153781994]: SickScanServices: Sending request "sWN ScanDataEthSettings 1 +192 +168 +127 +32 +2115" [ INFO] [1715831013.153837928]: Sending : sWN ScanDataEthSettings 1 +192 +168 +127 +32 +2115 [ WARN] [1715831018.154010654]: Timeout during waiting for new datagram [ INFO] [1715831018.154083398]: sendSOPASCommand: no full reply available for read after 5000 ms [ INFO] [1715831018.154129887]: Receiving: [ INFO] [1715831018.154161829]: SOPAS Communication -Error unexpected Sopas answer for request sWN ScanDataEthSettings 1 +192 +168 +127 +32 +2115 [ERROR] [1715831018.154214427]: ## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command "sWN ScanDataEthSettings 1 +192 +168 +127 +32 +2115" [ERROR] [1715831018.154246352]: ## ERROR SickScanServices::sendSopasCmdCheckResponse() failed on sending command"sWN ScanDataEthSettings 1 +192 +168 +127 +32 +2115" [ERROR] [1715831018.154277623]: ## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse("sWN ScanDataEthSettings 1") failed. ``` The `sick_multiscan.launch` settings for the code are as follows. ``` ``` For your information, the PC receiving the data is running Ubuntu ROS1 Noetic, and its IP address is 192.168.127.214. Do you have any suggestions for solving the merge issue? After merging, I plan to use Hector SLAM as demonstrated in the example code without reflectors. `rosrun rviz rviz -d ./src/sick_scan_xd/test/emulator/config/rviz_slam_nav350.rviz & roslaunch sick_scan_xd sick_nav_350.launch & roslaunch sick_scan_xd test_200_slam_ros1_hector.launch scan_topic:=/sick_nav_350/scan ` I am currently attempting with a single NAV350, but I am encountering a status error in RViz, and the map is not being generated. The tf_tree is as follows. ![Screenshot from 2024-05-16 15-32-05](https://github.com/SICKAG/sick_scan_xd/assets/60597598/bdf7d9a1-e3b4-49f5-860e-cff1a66fd212) ![Screenshot from 2024-05-16 15-31-36](https://github.com/SICKAG/sick_scan_xd/assets/60597598/623673c9-c492-44d4-8412-56dcd6b7b948) ![frames](https://github.com/SICKAG/sick_scan_xd/assets/60597598/c4b7431a-9c4a-4b80-b45f-af2598a58e5d) Is there anything that needs to be modified in this example code? Thank you.
rostest commented 1 month ago

Thanks for your feedback. sick_multiscan.launch is the configuration file for SICK multiScan100 lidars. It cannot be used to merge the scan data from two NAV350 devices.

To run two NAV350 lidars, follow the FAQ example and start sick_scan_xd twice with different ip addresses, topics and frame ids:

roslaunch sick_scan_xd sick_nav_350.launch nodename:=sick_nav_350_front hostname:=192.168.127.31 cloud_topic:=cloud_front laserscan_topic:=scan_front frame_id:=nav_350_front
roslaunch sick_scan_xd sick_nav_350.launch nodename:=sick_nav_350_rear hostname:=192.168.127.32 cloud_topic:=cloud_rear laserscan_topic:=scan_rear frame_id:=nav_350_rear

You can then use e.g. the front lidar for slam by providing your system's transform chain in a hector slam configuration file like test_200_slam_ros1_hector.launch. The static_transform_publishers in the hector launch file define the 6D poses (position and orientation) of your lidars relative to a robot frame and must be adapted to your setup. See https://github.com/SICKAG/sick_scan_xd/blob/develop/doc/slam.md for details.