Open ismet55555 opened 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
)?
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
ScanMode
of value 0
(SCAN)ScanMode
value 1
(EXPRESS_SCAN) and 2
(FORCE_SCAN) show the plot, however all returned values of [result, distances, angles, bNewScan, quality]
return zero.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
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.
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:
rplidar_sdk\sdk
is in the root directoryrplidar_sdk\sdk\output\win32\Debug
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...
Never mind. It looks like you left a ReadMe.txt
with notes on rebuilding in the mavlink_sbg_support
directory. Let me try that.
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.
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.
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.
Clone Hardware-MATLAB
onto computer
git clone https://github.com/ENSTABretagneRobotics/Hardware-MATLAB
Clone rplidar_sdk
into Hardware-MATLAB
directory
git clone https://github.com/lebarsfa/rplidar_sdk
(forked from https://github.com/Slamtec/rplidar_sdk)In Hardware-MATLAB
create the these directories:
SBG Systems
MAVLinkSDK/mavlink
Extract the contents of https://github.com/mavlink/c_library_v1/archive/0fc203298923f1d7c225cc6f43918d6663809e38.zip
into the MAVLinkSDK/mavlink
directory
Extract the contents of https://www.ensta-bretagne.fr/lebars/Share/SBG%20Systems.zip
into the SBG Systems
directory.
rplidar_driver.lib
Open Visual Studio 2019
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)Change the Configuration Manager
options
Change Project Properties of frame_grabber
, simple_grabber
, ultra_simple
Visual Studio 2019 (v142)
Change Project Properties of "rplidar_driver"
$(SolutionDir)\..\..\output\win64\$(Configuration)\
$(SolutionDir)\..\..\obj\win64\$(ProjectName)\$(Configuration)\
Static library (.lib)
Visual Studio 2019 (v142)
..\..\..\sdk\include;..\..\..\sdk\src;%(AdditionalIncludeDirectories)
WIN64;_DEBUG;_LIB;%(PreprocessorDefinitions)
Multi-threaded Debug (/MTd)
Build the sdk_and_demo
solution
rplidar_sdk\sdk\output\win64\Debug\rplidar_driver.lib
hardware.dll
Open Visual Studio 2019
Open the hardwarex
visual studio project:
Change the Configuration Manager
options
Build the hardwarex
solution
can not open hardwarex.dll
error.x64\Debug\hardwarex.dll
Open MATLAB
Make sure the MATLAB mex tool configuration is set to Visual Studio (2013 and later)
mex -setup
in command line to checkmex -setup
Open file hardwarex_setup.m
in MATLAB
Ensure that the addpath()
includes the directory where the hardwarex.dll
is located.
Run hardwarex_setup.m
to generate and update a set of files:
hardwarex_proto.m
and hardwarex_thunk_pcwin64.dll
are important for MATLAB library loadinghardwarex_thunk_pcwin64.dll
<-------hardwarex_thunk_pcwin64.exp
hardwarex_thunk_pcwin64.lib
hardwarex_thunk_pcwin64.obj
hardwarex_proto.m
<-------hardwarex_thunk_pcwin64.c
hardwarex.i
test_rplidar.m
)Load the library
hardwarex_init;
List all available/exposed library methods:
libfunctions('hardwarex')
Connect, use, and destroy the rplidar library object pointer.
pRPLIDAR = CreateRPLIDAR();
ConnectRPLIDAR(pRPLIDAR, 'RPLIDAR0.txt')
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);
Clear the library pointer reference
clear pRPLIDAR
(pRPLIDAR
is the variable you stored your poniter in)Unload the library
unloadlibrary('hardwarex')
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.
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()
?
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...
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.
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()
.
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!
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.
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.
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
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.
Hey thanks again for updating the code. This feature is very useful I think. I'll try to run and test it soon.
This is the step by step what I did:
Cloned repo
Ran
hardwarex_setup.m
, which generated the proto and thunk filesChanged the provided
RPLIDAR0.txt
configuration file with the settings belowConnected A3 model RPLIDAR. (connected on
COM3
)Using MATLAB, executed
test.rplidar.m
ESC
to close figure and stop the programDisconnect A3 model lidar
Connect S1 model lidar (connected again on
COM3
)Using MATLAB executed
test.rplidar.m
[result, distance, angle, bNewScan, quality] = GetScanDataResponseRPLIDAR(pRPLIDAR)
calllib()
line:result = calllib('hardwarex', 'GetScanDataResponseRPLIDARx', pRPLIDAR, pDistance, pAngle, pbNewScan, Quality)
RPLIDAR0.txt
MATLAB Crash Report: