ENSTABretagneRobotics / Hardware-MATLAB

8 stars 2 forks source link

Sample MATLAB Script (test_rplidar.m) works with A3 but crashes with S1 #3

Open ismet55555 opened 4 years ago

ismet55555 commented 4 years ago

This is the step by step what I did:

  1. Cloned repo

  2. Ran hardwarex_setup.m, which generated the proto and thunk files

  3. Changed the provided RPLIDAR0.txt configuration file with the settings below

  4. Connected A3 model RPLIDAR. (connected on COM3)

  5. Using MATLAB, executed test.rplidar.m

    • A MATLAB figure comes up displaying the LIDAR pointcloud.
    • Key press ESC to close figure and stop the program
  6. Disconnect A3 model lidar

  7. Connect S1 model lidar (connected again on COM3)

  8. Using MATLAB executed test.rplidar.m

    • Script executes:
    • [result, distance, angle, bNewScan, quality] = GetScanDataResponseRPLIDAR(pRPLIDAR)
    • MATLAB crashes on calllib() line:
    • result = calllib('hardwarex', 'GetScanDataResponseRPLIDARx', pRPLIDAR, pDistance, pAngle, pbNewScan, Quality)

RPLIDAR0.txt

% Server TCP port (e.g. :4001), client IP address and TCP port (e.g. 127.0.0.1:4001) or local RS232 port
%127.0.0.1:55555
COM3
% BaudRate
256000
% Timeout (in ms)
2500
% threadperiod (in ms, require associated thread)
50
% bSaveRawData (require associated thread)
1
% bStartScanModeAtStartup
1
% ScanMode (0 : SCAN, 1 : EXPRESS_SCAN, 2 : FORCE_SCAN)
0
% motordelay (in ms)
2000
% maxhist (0 : try to automatically remove old data, around 180 : advised for SCAN mode, around 360 : advised for EXPRESS_SCAN mode, require associated thread)
360
% alpha_max_err (in rad)
0.01
% d_max_err (in m)
0.1

MATLAB Crash Report:

MATLAB crash file:C:\Users\ihandzic\AppData\Local\Temp\matlab_crash_dump.12344-1:

--------------------------------------------------------------------------------
          Access violation detected at Fri Jul 17 12:12:00 2020 -0400
--------------------------------------------------------------------------------

Configuration:
  Crash Decoding           : Disabled - No sandbox or build area path
  Crash Mode               : continue (default)
  Default Encoding         : windows-1252
  Deployed                 : false
  Graphics Driver          : NVIDIA Corporation Quadro M2200/PCIe/SSE2 Version 4.6.0 NVIDIA 391.58
  Graphics card 1          : Intel Corporation ( 0x8086 ) Intel(R) HD Graphics P630 Version 26.20.100.8142 (2020-4-11)
  Graphics card 2          : NVIDIA ( 0x10de ) NVIDIA Quadro M2200 Version 23.21.13.9158 (2018-4-30)
  Java Version             : Java 1.8.0_152-b16 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode
  MATLAB Architecture      : win64
  MATLAB Entitlement ID    : 4791263
  MATLAB Root              : C:\Program Files\MATLAB\R2018b
  MATLAB Version           : 9.5.0.944444 (R2018b)
  OpenGL                   : hardware
  Operating System         : Microsoft Windows 10 Pro for Workstations
  Process ID               : 12344
  Processor ID             : x86 Family 6 Model 158 Stepping 9, GenuineIntel
  Session Key              : ec080639-51b4-485b-a033-5d77013b021c
  Window System            : Version 10.0 (Build 18362)

Fault Count: 1

Abnormal termination

Register State (from fault):
  RAX = 00000000efdc6680  RBX = 00000000efdc6910
  RCX = 0000000000000000  RDX = 00000000043f8450
  RSP = 00000000043f8410  RBP = 000000000000127f
  RSI = 00000000efdc6890  RDI = 00000000efdc65c0

   R8 = 00000000043f8430   R9 = 00000000efdc6910
  R10 = 00000000cf1d23c0  R11 = 00000000043f8570
  R12 = 00000000043f8650  R13 = 00007ffad2551600
  R14 = 00000000169eb460  R15 = 00000000efc48d40

  RIP = 000000000471983e  EFL = 00010202

   CS = 0033   FS = 0053   GS = 002b

Stack Trace (from fault):
[  0] 0x000000000471983e C:\Users\ihandzic\Downloads\Hardware-MATLAB\x64\hardwarex.dll+00104510
[  1] 0x00007ffad25516bc C:\Users\ihandzic\Downloads\Hardware-MATLAB\hardwarex_thunk_pcwin64.dll+00005820 int32voidPtrvoidPtrvoidPtrvoidPtrvoidPtrThunk+00000188
[  2] 0x00000000cf18ea00                             bin\win64\libmwcli.dll+00059904
[  3] 0x00000000cf19313d                             bin\win64\libmwcli.dll+00078141
[  4] 0x00000000cf1b32a9                             bin\win64\libmwcli.dll+00209577 PointerMapSize+00104489
[  5] 0x00000000cf1b474d                             bin\win64\libmwcli.dll+00214861 PointerMapSize+00109773
[  6] 0x00000000179ab724                     bin\win64\pgo\m_dispatcher.dll+00046884 Mdispatcher::getDispatcher+00002228
[  7] 0x00000000179aca07                     bin\win64\pgo\m_dispatcher.dll+00051719 Mfh_MATLAB_fn_impl::dispatch_fh_with_reuse+00000343
[  8] 0x000000001855263c                            bin\win64\pgo\m_lxe.dll+01189436 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00307456
[  9] 0x000000001848b8e7                            bin\win64\pgo\m_lxe.dll+00375015
[ 10] 0x000000001864b5b9                            bin\win64\pgo\m_lxe.dll+02209209 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,std::vector<MathWorks::utl::attach_ptr<ir::IrTree>,std::allocator<MathWorks::utl::attach_ptr<ir::IrTree> > > >::load_object_data+00032749
[ 11] 0x0000000018484ceb                            bin\win64\pgo\m_lxe.dll+00347371
[ 12] 0x0000000018615a0f                            bin\win64\pgo\m_lxe.dll+01989135 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+01107155
[ 13] 0x000000001861597d                            bin\win64\pgo\m_lxe.dll+01988989 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+01107009
[ 14] 0x000000001848ab16                            bin\win64\pgo\m_lxe.dll+00371478
[ 15] 0x0000000018488e1c                            bin\win64\pgo\m_lxe.dll+00364060
[ 16] 0x000000001849b265                            bin\win64\pgo\m_lxe.dll+00438885
[ 17] 0x0000000018498e26                            bin\win64\pgo\m_lxe.dll+00429606
[ 18] 0x0000000018498a24                            bin\win64\pgo\m_lxe.dll+00428580
[ 19] 0x00000000179ae007                     bin\win64\pgo\m_dispatcher.dll+00057351 Mfh_file::dispatch_fh_impl+00001111
[ 20] 0x00000000179adaf2                     bin\win64\pgo\m_dispatcher.dll+00056050 Mfh_file::dispatch_fh_with_reuse+00000066
[ 21] 0x000000001855263c                            bin\win64\pgo\m_lxe.dll+01189436 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00307456
[ 22] 0x000000001848b8e7                            bin\win64\pgo\m_lxe.dll+00375015
[ 23] 0x000000001864b5b9                            bin\win64\pgo\m_lxe.dll+02209209 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,std::vector<MathWorks::utl::attach_ptr<ir::IrTree>,std::allocator<MathWorks::utl::attach_ptr<ir::IrTree> > > >::load_object_data+00032749
[ 24] 0x0000000018484ceb                            bin\win64\pgo\m_lxe.dll+00347371
[ 25] 0x0000000018615a0f                            bin\win64\pgo\m_lxe.dll+01989135 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+01107155
[ 26] 0x000000001861597d                            bin\win64\pgo\m_lxe.dll+01988989 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+01107009
[ 27] 0x000000001848ab16                            bin\win64\pgo\m_lxe.dll+00371478
[ 28] 0x0000000018488e1c                            bin\win64\pgo\m_lxe.dll+00364060
[ 29] 0x000000001849b265                            bin\win64\pgo\m_lxe.dll+00438885
[ 30] 0x000000001849a88c                            bin\win64\pgo\m_lxe.dll+00436364
[ 31] 0x0000000018498779                            bin\win64\pgo\m_lxe.dll+00427897
[ 32] 0x00000000184990eb                            bin\win64\pgo\m_lxe.dll+00430315
[ 33] 0x0000000018498a49                            bin\win64\pgo\m_lxe.dll+00428617
[ 34] 0x00000000179ae007                     bin\win64\pgo\m_dispatcher.dll+00057351 Mfh_file::dispatch_fh_impl+00001111
[ 35] 0x00000000179ada9e                     bin\win64\pgo\m_dispatcher.dll+00055966 Mfh_file::dispatch_fh+00000062
[ 36] 0x0000000018484ead                            bin\win64\pgo\m_lxe.dll+00347821
[ 37] 0x000000001861e9b6                            bin\win64\pgo\m_lxe.dll+02025910 MathWorks::lxe::ShutdownLxeEngine+00004034
[ 38] 0x000000001857fd3c                            bin\win64\pgo\m_lxe.dll+01375548 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00493568
[ 39] 0x000000001858091c                            bin\win64\pgo\m_lxe.dll+01378588 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00496608
[ 40] 0x0000000018581c92                            bin\win64\pgo\m_lxe.dll+01383570 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00501590
[ 41] 0x00000000185828f8                            bin\win64\pgo\m_lxe.dll+01386744 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00504764
[ 42] 0x0000000018581ddf                            bin\win64\pgo\m_lxe.dll+01383903 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00501923
[ 43] 0x0000000018581ede                            bin\win64\pgo\m_lxe.dll+01384158 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00502178
[ 44] 0x0000000018489a7d                            bin\win64\pgo\m_lxe.dll+00367229
[ 45] 0x000000001849b265                            bin\win64\pgo\m_lxe.dll+00438885
[ 46] 0x000000001849a88c                            bin\win64\pgo\m_lxe.dll+00436364
[ 47] 0x00000000184942a1                            bin\win64\pgo\m_lxe.dll+00410273
[ 48] 0x00000000184939c6                            bin\win64\pgo\m_lxe.dll+00408006
[ 49] 0x0000000018493ace                            bin\win64\pgo\m_lxe.dll+00408270
[ 50] 0x0000000018673780                            bin\win64\pgo\m_lxe.dll+02373504 mwboost::archive::detail::oserializer<mwboost::archive::xml_oarchive,MathWorks::lxe::CodeClearEvent>::oserializer<mwboost::archive::xml_oarchive,MathWorks::lxe::CodeClearEvent>+00012824
[ 51] 0x000000001867372e                            bin\win64\pgo\m_lxe.dll+02373422 mwboost::archive::detail::oserializer<mwboost::archive::xml_oarchive,MathWorks::lxe::CodeClearEvent>::oserializer<mwboost::archive::xml_oarchive,MathWorks::lxe::CodeClearEvent>+00012742
[ 52] 0x00000000185534fd                            bin\win64\pgo\m_lxe.dll+01193213 mwboost::archive::detail::iserializer<mwboost::archive::binaryTerm_iarchive,MathWorks::lxe::function_descriptor>::load_object_data+00311233
[ 53] 0x00000000178269e7                    bin\win64\pgo\m_interpreter.dll+00354791 inEvalCmdWithLocalReturn+00000063
[ 54] 0x00000000fb610388                          bin\win64\libmwbridge.dll+00131976 mnParser+00001304
[ 55] 0x00000000180e3618                                  bin\win64\mcr.dll+00341528 mcr::runtime::setInterpreterThreadSingletonToCurrent+00020808
[ 56] 0x00000000180e414d                                  bin\win64\mcr.dll+00344397 mcr::runtime::setInterpreterThreadSingletonToCurrent+00023677
[ 57] 0x00000000180ab1ba                                  bin\win64\mcr.dll+00111034 mcrOptions::set_use_license_manager+00075498
[ 58] 0x00000000180c6e74                                  bin\win64\mcr.dll+00224884 mcrOptions::set_use_license_manager+00189348
[ 59] 0x00000000fd02cfee                                  bin\win64\iqm.dll+00643054 iqm::PackagedTaskPlugin::execute+00000894
[ 60] 0x00000000fd02ce63                                  bin\win64\iqm.dll+00642659 iqm::PackagedTaskPlugin::execute+00000499
[ 61] 0x00000000fcffcc07                                  bin\win64\iqm.dll+00445447 iqm::Iqm::setupIqmFcnPtrs+00076215
[ 62] 0x00000000fcffcc59                                  bin\win64\iqm.dll+00445529 iqm::Iqm::setupIqmFcnPtrs+00076297
[ 63] 0x00000000fcffca16                                  bin\win64\iqm.dll+00444950 iqm::Iqm::setupIqmFcnPtrs+00075718
[ 64] 0x00000000fcfd9748                                  bin\win64\iqm.dll+00300872 iqm::Iqm::deliver+00005880
[ 65] 0x00000000fcfd8eef                                  bin\win64\iqm.dll+00298735 iqm::Iqm::deliver+00003743
[ 66] 0x00000000fcfdac0d                                  bin\win64\iqm.dll+00306189 iqm::Iqm::deliver+00011197
[ 67] 0x0000000100203775                        bin\win64\libmwservices.dll+02176885 services::system_events::PpeDispatchHook::dispatchOne+00036213
[ 68] 0x0000000100212a63                        bin\win64\libmwservices.dll+02239075 sysq::addProcessPendingEventsUnitTestHook+00006035
[ 69] 0x0000000100214310                        bin\win64\libmwservices.dll+02245392 sysq::getCondition+00004208
[ 70] 0x000000010021596d                        bin\win64\libmwservices.dll+02251117 svWS_ProcessPendingEvents+00000221
[ 71] 0x00000000180e7724                                  bin\win64\mcr.dll+00358180 mcr::runtime::setInterpreterThreadSingletonToCurrent+00037460
[ 72] 0x00000000180e7f16                                  bin\win64\mcr.dll+00360214 mcr::runtime::setInterpreterThreadSingletonToCurrent+00039494
[ 73] 0x00000000180dcabf                                  bin\win64\mcr.dll+00314047 mcr_process_events+00001007
[ 74] 0x0000000017fed040                             bin\win64\MVMLocal.dll+00380992 mvm_server::inproc::LocalFactory::terminate+00177312
[ 75] 0x00000000fa9d9480                                  bin\win64\mvm.dll+01741952 mvm::detail::SessionImpl::initWithOptions+00000592
[ 76] 0x00000000fa9da170                                  bin\win64\mvm.dll+01745264 mvm::detail::SessionImpl::runMain+00000128
[ 77] 0x00000000fa9da395                                  bin\win64\mvm.dll+01745813 mvm::detail::SessionImpl::runMatlabDesktop+00000261
[ 78] 0x0000000140007036                               bin\win64\MATLAB.exe+00028726 mwboost::serialization::singleton_module::unlock+00000966
[ 79] 0x0000000140007f13                               bin\win64\MATLAB.exe+00032531 mwboost::serialization::singleton_module::unlock+00004771
[ 80] 0x00007ffadec77bd4                   C:\WINDOWS\System32\KERNEL32.DLL+00097236 BaseThreadInitThunk+00000020
[ 81] 0x00007ffae086ce51                      C:\WINDOWS\SYSTEM32\ntdll.dll+00446033 RtlUserThreadStart+00000033
lebarsfa commented 4 years ago

In the current state, Step 2 might cause problems if you did not rebuild hardwarex.dll (potential inconsistency between the .dll and the .h, related to the state of ENABLE_MAVLINK_SUPPORT, ENABLE_SBG_SDK_SUPPORT, ENABLE_RPLIDAR_SDK_SUPPORT options, see 3rd_support/ReadMe.txt).

I just added a GitHub release with some updates and which provides the source code with binaries without ENABLE_MAVLINK_SUPPORT, ENABLE_SBG_SDK_SUPPORT, ENABLE_RPLIDAR_SDK_SUPPORT options as well as the source code with binaries with ENABLE_MAVLINK_SUPPORT, ENABLE_SBG_SDK_SUPPORT, ENABLE_RPLIDAR_SDK_SUPPORT options. Both should work with RPLIDAR A3 and potentially other LIDARs that would support legacy SCAN and EXPRESS SCAN modes (and you should not do Step 2 for any of the binaries versions, normally all the correct proto and thunk files are provided).

For the S1, can you confirm that with frame_grabber.exe, you have the option to run it with Standard and Express Scan mode (for A3, I have also Boost, Sensitivity, Stability)?

ismet55555 commented 4 years ago

First of all, thank you again for getting back at this so quick and so thorough. If you have the account, I can buy you a coffee with https://www.buymeacoffee.com/.

I just tried the code with the updated binaries ( "the source code with binaries without ENABLE_MAVLINK_SUPPORT, ENABLE_SBG_SDK_SUPPORT, ENABLE_RPLIDAR_SDK_SUPPORT options"). This is what I got from it:

S1

  1. Only seems to display a seemingly correct LIDAR pointcloud with ScanMode of value 0 (SCAN)
  2. ScanMode value 1 (EXPRESS_SCAN) and 2 (FORCE_SCAN) show the plot, however all returned values of [result, distances, angles, bNewScan, quality] return zero.
  3. Tried the commented lines with other scan modes (GetScanDataResponseFromThreadRPLIDAR, GetExpressScanDataResponseRPLIDAR, GetExpressScanDataResponseFromThreadRPLIDAR), however these options either crash MATLAB or return zeros, depending on the ScanMode selected.

~However, when the S1 does work the pointcloud seems to be lagging. Not sure if this is a bug or a feature though.~ Never, mind, I think the pause(0.05) is what does it. Removing it, it seems to do as well as the frame_grabber.exe

Also, is there any way you can explain what the ENABLE_RPLIDAR_SDK_SUPPORT in the code actually does?


So I tested the various ScanModes in RPLIDAR0.txt with the various data read library functions in test_rplidar.m, this is what I got:

A3

  0 - SCAN
    GetScanDataResponseRPLIDAR - **GOOD**
    GetScanDataResponseFromThreadRPLIDAR - MATLAB Crash
    GetExpressScanDataResponseRPLIDAR - Blank Plot, Nothing happening
    GetExpressScanDataResponseFromThreadRPLIDAR - MATLAB Crash

  1 - EXPRESS_SCAN
    GetScanDataResponseRPLIDAR  - Pointcloud Messed up
    GetScanDataResponseFromThreadRPLIDAR - MATLAB Crash
    GetExpressScanDataResponseRPLIDAR - **GOOD**
    GetExpressScanDataResponseFromThreadRPLIDAR - MATLAB Crash

  2 - FORCE_SCAN
    GetScanDataResponseRPLIDAR - **GOOD**
    GetScanDataResponseFromThreadRPLIDAR - MATLAB Crash
    GetExpressScanDataResponseRPLIDAR - Blank Plot, Nothing happening
    GetExpressScanDataResponseFromThreadRPLIDAR - MATLAB Crash

S1

  0 - SCAN
    GetScanDataResponseRPLIDAR - **GOOD**
    GetScanDataResponseFromThreadRPLIDAR - MATLAB Crash
    GetExpressScanDataResponseRPLIDAR - Zeros returned
    GetExpressScanDataResponseFromThreadRPLIDAR - MATLAB Crash

  1 - EXPRESS_SCAN
    GetScanDataResponseRPLIDAR - Zeros returned
    GetScanDataResponseFromThreadRPLIDAR - MATLAB Crash
    GetExpressScanDataResponseRPLIDAR - Zeros returned
    GetExpressScanDataResponseFromThreadRPLIDAR - MATLAB Crash

  2 - FORCE_SCAN
    GetScanDataResponseRPLIDAR - Zeros returned
    GetScanDataResponseFromThreadRPLIDAR - MATLAB Crash
    GetExpressScanDataResponseRPLIDAR - Zeros returned
    GetExpressScanDataResponseFromThreadRPLIDAR - MATLAB Crash

For the S1 these are the scan modes I have available on windows using frame_grabber.exe

S1_Scan_Options

lebarsfa commented 4 years ago

Thank you for this detailed report.

From your screen capture, it seems now logical that only what I call SCAN mode would work with S1, and that it probably does not support EXPRESS SCAN.

Without ENABLE_RPLIDAR_SDK_SUPPORT the code uses its own implementation of the RPLIDAR protocol from the documentation, while with ENABLE_RPLIDAR_SDK_SUPPORT, the code calls Slamtec RPLIDAR SDK functions. They are currently supposed to work the same for the SCAN, EXPRESS and FORCE SCAN modes (note that FORCE SCAN and SCAN are probably almost the same if not the same).

If you use the binaries with ENABLE_RPLIDAR_SDK_SUPPORT, I added also GetOtherScanDataResponseRPLIDAR() function to use in place of GetExpressScanDataResponseRPLIDAR() in combination with a specific value for ScanMode which may give access to the other modes of the LIDARs, e.g. for RPLIDAR A3 in that case a ScanMode of 0 probably corresponds to Standard, 2 to Express, 3 to Boost, 4 to Sensitivity, 5 to Stability in frame_grabber.exe, while I would guess for RPLIDAR S1 a ScanMode of 0 corresponds to Standard, 1 to DenseBoost... I will probably have to add this in comments in the code samples if this works well.

About the GetXXXFromThread() versions, you need also to uncomment StartXXXThreadRPLIDAR() before the while loop as well as StopXXXThreadRPLIDAR() after the loop for them to work. You will need also to tweak at least the condition about count to improve the display, e.g. if count > 8*360 for EXPRESS SCAN, but the way things are displayed should be changed significantly. Those functions may be helpful in some use cases since they just return latest data without communication delays, contrary to the others.

ismet55555 commented 4 years ago

I see. Thanks for the explanation. I will try the SDK functionality as well.

In the mean time. I was trying to build the repo code with some of the changes we had in the old code (basically a slow and systematic git merge).

So after the changes were in place, I did the following:

  1. Cloned the rplidar SDK repo (https://github.com/Slamtec/rplidar_sdk) into the your repo such that rplidar_sdk\sdk is in the root directory
  2. Build the rplidar_sdk (frame grabber and all of the other included projects). This produced an output as in rplidar_sdk\sdk\output\win32\Debug
  3. Build your repo project and got this error: cannot open file 'sdgEComd.lib'

Looked up the project properties in for linkers and saw there is a SBG Systems\Inertial SDK\Software Development\sbgECom\x86\vc15\lib reference there. After some googling I thought I could copy this repo, https://github.com/soulsheng/sbgECom, into the root directory of the project, but that yielded the same error.

I also tried removing the references to the SBG Systems in the Project settings but that didn't do it, it was still looking for that library file.

Do you know what I'm doing wrong here to build that project. Does Compat_vs2017.bat have anything to do with it? I'm assuming it's a tool to be able to set different options in the code...

ismet55555 commented 4 years ago

Never mind. It looks like you left a ReadMe.txt with notes on rebuilding in the mavlink_sbg_support directory. Let me try that.

lebarsfa commented 4 years ago

The instructions to rebuild should be now in 3rd_support/ReadMe.txt (note that I renamed the folder, previously it was mavlink_sbg_support and now it is 3rd_support), but yes, in your case you can also try to remove SBG references in the Visual Studio project .vcxproj using Compat_vs2017.bat, or manually in Visual Studio (in that case do not forget to do it for Debug and Release configurations of Win32 and x64 platforms). Additionally, I recently added CMake support with a CMakeLists.txt with options that are supposed to download the necessary prerequisites.

ismet55555 commented 4 years ago

Ok I think I got it to work with our code additions/changes merged into your most recent commit of this repo, and in turn running everything using your test_rplidar.m sample code.

So the trickiest part for me was to build everything with x64. That is, building the rplidar_sdk and then referencing the resulting rplidar_driver.lib, when building this repo code. There were some project settings that needed to be win64 for me.

rplidar_sdk was asking for a Visual Studio 2019 (v142) toolset, and since I was running Visual Studio 2017 that didn't seem to support that, I had to upgrade to Visual Studio 2019 in order to build rplidar_sdk.

In addition, getting MAVLINK and SBG in its right places in the repo gave me some issues, but in the end it seemed like a stupid path issue on my part.

ismet55555 commented 4 years ago

Hey so here is something I documented for us here to get going with your repo code in Windows x64. As of now this works, but I'm sure there are some things in here that may be unnecessary or even wrong.

I'm just adding this here in case someone needs to get going with building this code or making minor changes, and they may not have the greatest knowledge of C/C++ development.


How to Build RPLIDAR MATLAB Interface Library on Windows 64-bit System

Setting up the project

  1. Clone Hardware-MATLAB onto computer

    • git clone https://github.com/ENSTABretagneRobotics/Hardware-MATLAB
  2. Clone rplidar_sdk into Hardware-MATLAB directory

  3. In Hardware-MATLAB create the these directories:

    • SBG Systems
    • MAVLinkSDK/mavlink
  4. Extract the contents of https://github.com/mavlink/c_library_v1/archive/0fc203298923f1d7c225cc6f43918d6663809e38.zip into the MAVLinkSDK/mavlink directory

    • You should get a directory structure like this:
      • Hardware-MATLAB
        • MAVLink
          • mavlink
            • ardupilotmega
            • ASLUAV
            • ....
  5. Extract the contents of https://www.ensta-bretagne.fr/lebars/Share/SBG%20Systems.zip into the SBG Systems directory.

    • You should get a directory structure like this:
      • Hardware-MATLAB
        • SBG Systems
          • Inertial SDK
            • Software Development
              • sbgECom
                • ....

Building rplidar_driver.lib

  1. Open Visual Studio 2019

  2. Open the rplidar_sdk visual studio solution:

    • rplidar_sdk\sdk\workspaces\vc14\sdk_and_demo.sln (Can use vc10, vc14, vc15, or vc16 depending on your Visual Studio)
    • This solution will include 4 different projects:
      • frame_grabber
      • rplidar_driver
      • simple_grabber
      • ultra_simple
  3. Change the Configuration Manager options

    • Build > Configuration Manager
    • Active solution platform: x64
  4. Change Project Properties of frame_grabber, simple_grabber, ultra_simple

    • In solution explorer, right click on project - Properties
    • Up top, Platform > x64
    • Configuration Properties
      • General
        • Platform Toolset > Visual Studio 2019 (v142)
  5. Change Project Properties of "rplidar_driver"

    • Up top, Platform > x64
    • Configuration Properties
      • General
        • Output Directory > $(SolutionDir)\..\..\output\win64\$(Configuration)\
        • Intermediate Directory > $(SolutionDir)\..\..\obj\win64\$(ProjectName)\$(Configuration)\
        • Configuration Type > Static library (.lib)
        • Platform Toolset > Visual Studio 2019 (v142)
      • C/C++
        • All Options
          • Additional Include Directories > ..\..\..\sdk\include;..\..\..\sdk\src;%(AdditionalIncludeDirectories)
          • Preprocessor Definition > WIN64;_DEBUG;_LIB;%(PreprocessorDefinitions)
          • Runtime Library > Multi-threaded Debug (/MTd)
  6. Build the sdk_and_demo solution

    • Build > Clean Solution (why not)
    • Build > Build Solution
    • This will produce a the library file needed in the subsequent steps
      • rplidar_sdk\sdk\output\win64\Debug\rplidar_driver.lib

Building hardware.dll

  1. Open Visual Studio 2019

  2. Open the hardwarex visual studio project:

    • hardwarex.vcxproj
  3. Change the Configuration Manager options

    • Build > Configuration Manager
    • Active solution platform: x64
  4. Build the hardwarex solution

    • Note: When you build, make sure the library is not loaded in MATLAB. It will throw a can not open hardwarex.dll error.
    • Build > Clean Solution (why not)
    • Build > Build Solution
    • This will produce a the library file needed in the subsequent steps
      • x64\Debug\hardwarex.dll

Generating MATLAB Proto and Thunk Interface Files

  1. Open MATLAB

  2. Make sure the MATLAB mex tool configuration is set to Visual Studio (2013 and later)

    • Enter mex -setup in command line to check
    • If it is not Visual Studio 2013 or later, change using mex -setup
  3. Open file hardwarex_setup.m in MATLAB

  4. Ensure that the addpath() includes the directory where the hardwarex.dll is located.

  5. Run hardwarex_setup.m to generate and update a set of files:

    • Note that only hardwarex_proto.m and hardwarex_thunk_pcwin64.dll are important for MATLAB library loading
    • hardwarex_thunk_pcwin64.dll <-------
    • hardwarex_thunk_pcwin64.exp
    • hardwarex_thunk_pcwin64.lib
    • hardwarex_thunk_pcwin64.obj
    • hardwarex_proto.m <-------
    • hardwarex_thunk_pcwin64.c
    • hardwarex.i

Using the Proto and Thunk Interface Files in a Script (Check out test_rplidar.m)

  1. Load the library

    • Run hardwarex_init;
  2. List all available/exposed library methods:

    • libfunctions('hardwarex')
  3. Connect, use, and destroy the rplidar library object pointer.

    • pRPLIDAR = CreateRPLIDAR();
    • ConnectRPLIDAR(pRPLIDAR, 'RPLIDAR0.txt')
    • If bStartScanModeAtStartup is set to 0 in RPLIDAR0.txt, can use StartScanRequestRPLIDAR(pRPLIDAR) after connecting.
    • [result, distances, angles, bNewScan, quality] = GetScanDataResponseRPLIDAR(pRPLIDAR); [Or some other scanning mode]
    • DestroyRPLIDAR(pRPLIDAR);
  4. Clear the library pointer reference

    • clear pRPLIDAR (pRPLIDAR is the variable you stored your poniter in)
  5. Unload the library

    • unloadlibrary('hardwarex')
lebarsfa commented 4 years ago

Thank you very much for this detailed guide. For the rplidar_sdk part, I updated/added also Visual Studio 2010 (vc10), 2015 (vc14), 2017 (vc15), 2019 (vc16) project files (the original project files are a mix between Visual Studio 2015 and 2019) on https://github.com/lebarsfa/rplidar_sdk/tree/master/sdk/workspaces and corresponding prebuilt binaries for Visual Studio 2017 on https://www.ensta-bretagne.fr/lebars/Share/rplidar_sdk.zip.

ismet55555 commented 4 years ago

Hey, so I'm having two questions here.

I compiled this repo with our slight changes in it, with the ENABLE_RPLIDAR_SDK_SUPPORT defined. Seems to work fine with the S1 with the regular scan returning one data point per scan (StartScanRequestRPLIDAR(), GetScanDataResponseRPLIDAR()). But trying to get the another scanmode on the S1 to work, I get this:

pRPLIDAR = CreateRPLIDAR();
[connect_result] = ConnectRPLIDAR(pRPLIDAR, 'RPLIDAR0.txt')
[start_request_result] = StartOtherScanRequestRPLIDAR(pRPLIDAR, 0)
Error using calllib
Array must be numeric or logical.

Error in StartOtherScanRequestRPLIDAR (line 3)
    result = calllib('hardwarex', 'StartOtherScanRequestRPLIDARx', pRPLIDAR, scanmode_out);

Error in test_rplidar_GITHUB (line 48)
[start_request_result] = StartOtherScanRequestRPLIDAR(pRPLIDAR, 1)  % Not implemented, only with SDK

I tried changing StartOtherScanRequestRPLIDAR.m to:

function [result] = StartOtherScanRequestRPLIDAR(pRPLIDAR, scanmode)
    scanmode_out = libpointer('int32', scanmode)
    result = calllib('hardwarex', 'StartOtherScanRequestRPLIDARx', pRPLIDAR, scanmode_out);
end

But that didn't seem to make a difference.


Another thing. Do you think these are exposable from rplidar_driver.h? I wonder if the S1 is able to get a response from these, but i'm not really sure.

/// Get all scan modes that supported by lidar
virtual u_result getAllSupportedScanModes(std::vector < RplidarScanMode > &outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;

/// Get typical scan mode of lidar
virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;

/// Start scan
///
/// \param force            Force the core system to output scan data regardless whether the scanning motor is rotating or not.
/// \param useTypicalScan   Use lidar's typical scan mode or use the compatibility mode (2k sps)
/// \param options          Scan options (please use 0)
/// \param outUsedScanMode  The scan mode selected by lidar
virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0;

My C++ is not really that good, so I am not sure how to exactly add the getAllSupportedScanModes() to RPLIDAR.h and further expose it in hardwarex.h. Is there any way you know how to reference getAllSupportedScanModes() in RPLIDAR.h? I think I can take it form there ...

Do you think I can use rplidar_driver.h directly in MATLAB? Loading it in with loadlibrary()?

lebarsfa commented 4 years ago

About StartOtherScanRequestRPLIDAR(pRPLIDAR)/GetOtherScanDataResponseRPLIDAR(), the ScanMode parameter in RPLIDAR0.txt needs to be with probably 0 (Standard) or 1 (DenseBoost) to choose the mode (for the S1). If bStartScanModeAtStartup is 1 in RPLIDAR0.txt, it should be possible to do something similar to

pRPLIDAR = CreateRPLIDAR();
[result] = ConnectRPLIDAR(pRPLIDAR, 'RPLIDAR0.txt')
[result, distances, angles, bNewScan] = GetOtherScanDataResponseRPLIDAR(pRPLIDAR);
str = sprintf('Distance at %f deg = %f m\n', angles(1)*180.0/pi, distances(1));
disp(str);
[result] = DisconnectRPLIDAR(pRPLIDAR)
DestroyRPLIDAR(pRPLIDAR);

-> Correction : the previous code won't work without at least a StopRequestRPLIDAR(pRPLIDAR) and StartOtherScanRequestRPLIDAR(pRPLIDAR, scanmodeid) before GetOtherScanDataResponseRPLIDAR(pRPLIDAR)...

If bStartScanModeAtStartup is 0 in RPLIDAR0.txt

pRPLIDAR = CreateRPLIDAR();
[result] = ConnectRPLIDAR(pRPLIDAR, 'RPLIDAR0.txt')
% It should be possible to override ScanMode...
pRPLIDAR.value.ScanMode = 0; % or 1 for the S1, up to 4 for the A3...
[result] = GetStartupMessageRPLIDAR(pRPLIDAR)
[result] = StopRequestRPLIDAR(pRPLIDAR)
[result] = SetMotorPWMRequestRPLIDAR(pRPLIDAR, 660)
[result] = StartOtherScanRequestRPLIDAR(pRPLIDAR, pRPLIDAR.value.ScanMode)
pause(2)
[result, distances, angles, bNewScan] = GetOtherScanDataResponseRPLIDAR(pRPLIDAR);
str = sprintf('Distance at %f deg = %f m\n', angles(1)*180.0/pi, distances(1));
disp(str);
[result] = DisconnectRPLIDAR(pRPLIDAR)
DestroyRPLIDAR(pRPLIDAR);

About the functions to expose, one of the main difficulty is that they are C++ with classes, which probably cannot be directly exported, however in what I did the C++ is encapsulated in C structures and functions and in the end, the functionalities provided by startScan() should be more or less in my StartOtherScanRequestRPLIDAR() (it calls startScanExpress(), which calls startScan() depending on the ScanMode value). About the 2 other functions, I will probably add them soon somehow...

ismet55555 commented 4 years ago

Hey thanks again for the swift response. Very helpful.

So I gave the two things you outlined a try and I got the second block of code to work (with the override of scan mode). So it seems to work with the ENABLE_RPLIDAR_SDK_SUPPORT defined and rebuild with scan mode on 1 where it returns 32 data points per scan as per DenseScan for the S1.

The interesting part is if I set the Scan Mode to 0 in the RPLIDAR0.txt file AND on top of that override it with pRPLIDAR.value.ScanMode = 0 I still get 32 points back per scan. Not sure what that's all about, but as of right now, I'm glad it works with DenseScan.

The only thing that that gives me a return value of 1 is SetMotorPWMRequestRPLIDAR(pRPLIDAR, 660) although this, I think I can troubleshoot. Also, DisconnectRPLIDAR(pRPLIDAR) tends to return a 1 as well.

lebarsfa commented 4 years ago

Indeed, I expect StartOtherScanRequestRPLIDAR(pRPLIDAR, 0) to always give 32 points, but normally it should be really with a Standard quality if you draw and compare with StartOtherScanRequestRPLIDAR(pRPLIDAR, 1) that should be DenseBoost (for the S1), to check also with frame_grabber...

I just realize that the first block of code (with bStartScanModeAtStartup to 1) should not work as it is (at least a StopRequestRPLIDAR(pRPLIDAR) and StartOtherScanRequestRPLIDAR(pRPLIDAR, scanmodeid) should be necessary) with GetOtherScanDataResponseRPLIDAR() so forget it...

If the SetMotorPWMRequestRPLIDAR() fails, maybe I should make it optional...

I just made an updated release with an attempt to implement GetTypicalScanModeRPLIDAR() and GetAllSupportedScanModesRPLIDAR().

ismet55555 commented 4 years ago

So as of now with the DenseScan working and the bStartScanModeAtStartup at 0 working, that seems to be good for us right now.

One of the things we are adding though is the setting of the motorPWM variable via RPLIDAR0.txt, so I was hoping that the SetMotorPWMRequestRPLIDAR() was working, however, like I said I am able to add that after the fact.

Thank you very much for updating the code, exposing those two new functions. I think that will come in very handy here. I will build with ENABLE_RPLIDAR_SDK_SUPPORT and let you know what works and what doesn't work or if anything odd is happening.

Again, all this work you are doing is more than appreciated!

ismet55555 commented 4 years ago

Just to give you an update here.

I rebuild the repo and build hardwarex with the ENABLE_RPLIDAR_SDK_SUPPORT flag defined.

Everything seems to be working with the S1, the data is coming in with DenseScan, however Standard mode seems to still give DenseScan output (which, again, is ok with me). I am not getting any errors with SetMotorPWMRequestRPLIDAR() and DisconnectRPLIDAR().

Both GetTypicalScanModeRPLIDAR() and GetAllSupportedScanModesRPLIDAR() return the right values for the S1 as well.

I'll go ahead and try it with two S1 lidars and will let you know.

lebarsfa commented 4 years ago

About the SetMotorPWMRequestRPLIDAR(), I made a little change so that it can fail silently in ConnectRPLIDAR() and DisconnectRPLIDAR() so I would expect that you do not see wrong return values in those functions. If we check https://github.com/Slamtec/rplidar_sdk/blob/504a2e5dfb544124c3edbb94419f866ed274b99c/sdk/sdk/include/rplidar_driver.h#L180-L188 and https://github.com/Slamtec/rplidar_sdk/blob/504a2e5dfb544124c3edbb94419f866ed274b99c/sdk/sdk/src/rplidar_driver.cpp#L2175-L2204 we should expect that the SetMotorPWMRequestRPLIDAR() does not do anything for a S1, so I plan to add SetLidarSpinSpeedRequestRPLIDAR().

Also, in case you did not know, if you launch MATLAB from Windows command prompt you should see additional debug messages from hardwarex.dll.

ismet55555 commented 4 years ago

Oh I see. Makes sense now. Thank you for clarifying and pointing it out in the rplidar sdk. I was not paying attention to the spin rate, however it did seem slower than what we usually set it as at 1023. It's a little trickier to tell with the S1 since it's enclosed. A SetLidarSpinSpeedRequestRPLIDAR() would be pretty nice though. So that would not set the typical 0 - 1023 motorPWM value, but a rotational speed?

Hey thank you for the debugging tip! So we were using that information on linux, where we were starting matlab through the terminal, but honestly I didn't think of doing the same one windows with powershell of command prompt. So on windows we never got those messages from the lidar library.

But interestingly enough I did have a .bat script on windows to automate some stuff, for one, the startup without the UI (Below). However no output from the library in the little Matlab command prompt that pops up.

@echo off
cls

:: Ensure that any changed variables aren't persistent after restart
setlocal EnableDelayedExpansion

:: Change window title and color
title LIDAR
COLOR 0a

echo [LIDAR START] : Start script

:: Variables
set SCRIPT_DIR=<path to lidar start script directory>
set SCRIPT_M_FILE=<name of the startup script file without the '.m'>

:: Change working directory
echo [LIDAR START] : Changing the working directory to %SCRIPT_DIR% ...
chdir /d %SCRIPT_DIR%

:: Open MATLAB and move on without waiting
echo [LIDAR START] : Starting MATLAB with stript '%SCRIPT_M_FILE%' ...
start matlab -nosplash -nodesktop -r %SCRIPT_M_FILE%

echo [LIDAR START] : This window will close in 15 seconds ...
echo.
timeout 15
exit
lebarsfa commented 4 years ago

Indeed, I realized that the messages do not appear, so maybe I got them only when testing the Python interface or on another OS. I just added a release with SetLidarSpinSpeedRequestRPLIDAR(), with the same rpm parameter as in the Slamtec SDK.

ismet55555 commented 4 years ago

Hey thanks again for updating the code. This feature is very useful I think. I'll try to run and test it soon.