ENSTABretagneRobotics / Hardware-MATLAB

8 stars 2 forks source link

S3 RPLIDAR Does Not Respond to Changes in Motor PWM #5

Open ismet55555 opened 4 years ago

ismet55555 commented 4 years ago

Note that all of this is without the RPLIDAR SDK, ie: #define ENABLE_RPLIDAR_SDK_SUPPORT commented out in hardwarex.h I am not sure if this is an issue with the RPLIDAR SDK (will investigate further).

For the A3 RPLIDAR either of these will cause a change in spin rate:

Changing the motorPWM value to the config file, RPLIDAR0.txt
Using the SetMotorPWMRequestRPLIDAR() endpoint
Hardcoding #define DEFAULT_MOTOR_PWM_RPLIDAR <VALUE>

However, with the S3 none of these will cause a change of spin rate.

A slight difference between the A3 and the S1 is the connector to the little USB adapter. The differences are shown in the attached images. There seems to be no wire to the PWM connector. I am not sure if that is the fundamental issue here.

S1_connector A3_connector

ismet55555 commented 4 years ago

Just checked with the RPLIDAR SDK on Linux (Ubuntu 16.04)

A3 pwm/rpm can be changed. S1 pwm/rpm can not be changed.

Another thing is that I am trying to expose GetFIFOComputerRS232Port() in OSComputerRS232Port.h but keep getting error for RPLIDAR SDK.

Basically: hardwarex.h - HARDWAREX_API int GetFIFORPLIDARx(RPLIDAR* pRPLIDAR, int* count); RPLIDAR.h - inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count){ ....} OSComputerRS232Port.h - GetFIFOComputerRS232Port(){ .....}

It doesn't seem to touch anything SDK related, but errors out here: if (ioctl((intptr_t)hDev, FIONREAD, &bytes_avail) != EXIT_SUCCESS) And I can't figure out what and why really...

/*
Get the input queue length on the computer RS232 port.

HANDLE hDev : (IN) Identifier of the computer RS232 port.
int count : (OUT) bytes in input queue.

Return : EXIT_SUCCESS if no error or EXIT_FAILURE if there is an error.
*/
inline int GetFIFOComputerRS232Port(HANDLE hDev, int* count)
{
#ifdef _WIN32
    COMSTAT stats;

    memset(&stats, 0, sizeof(COMSTAT));
    if (ClearCommError(hDev, NULL, &stats))
    {
        *count = stats.cbInQue;
        return EXIT_SUCCESS;
    }
    else
    {
        return EXIT_FAILURE;
    }
#else
    int bytes_avail = 0;

    if (ioctl((intptr_t)hDev, FIONREAD, &bytes_avail) != EXIT_SUCCESS)
    {
        PRINT_DEBUG_ERROR_OSCOMPUTERRS232PORT(("CheckAvailableBytesComputerRS232Port error (%s) : %s(hDev=%#x)\n",
            strtime_m(),
            GetLastErrorMsg(),
            hDev));
        return EXIT_FAILURE;
    }
    else
    {
        *count = bytes_avail;
        return EXIT_SUCCESS;
    }
#endif // _WIN32
}

lebarsfa commented 4 years ago

I did not have the time to check in details but about the pwm/rpm, I agree with what you say (I was more or less implying that in https://github.com/ENSTABretagneRobotics/Hardware-MATLAB/issues/3#issuecomment-668009204, i.e. SetLidarSpinSpeedRequestRPLIDAR() is the function to use for the S1 instead of SetMotorPWMRequestRPLIDAR() which is for the A2-A3, as implied by the source code of the SDK).

About GetFIFOComputerRS232Port(), in GetFIFORPLIDAR() you would probably need to call GetFIFOComputerRS232Port() with its HANDLE hDev parameter corresponding to the serial port opened internally by the RPLIDAR SDK (since pRPLIDAR->RS232Port cannot be initialized in that case because the SDK does its own things), which should be something like pRPLIDAR->drv->_chanDev->_rxtxSerial->_serial_handle (to check more in details), do you do something like that? Also, can you confirm that it works without RPLIDAR SDK?

ismet55555 commented 4 years ago

As always, thank you for responding to this issue. And again, if happen to have a BuyMeCoffee account I can add to or something, let me know. Your responses have been more than helpful.

I must have missed the S1 only working with RPM and now PWM commands. This makes sense since using the frame grabber, and debugging it with VS code (attaching) I was able to see that setting pWM value will hit the speed endpoint. I’ll try that and monitor.

So I attached the frame_grabber.exe to debug and I know the Rplidar SDK works and is able to manipulate the S1.

The using the SDK handle is a really good lead giving that the added GetFIFO Endpoint doesn’t reference the SDK and is based on some pre-SDK version Of your repo. The GetFIFO endpoint works without the SDK, which is more of an indicator of that I think.

Let me give it a shot and report back!

ismet55555 commented 4 years ago

Well, SetLidarSpinSpeedRequestRPLIDAR() works, however I had it at a bad place in the code. It looks like for it to work I needed it after StartOtherScanRequestRPLIDAR().

As far as our custom added GetFIFORPLIDAR(), I am getting stuck on:

inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count)
{
    RS232PORT* pRS232Port = &pRPLIDAR->drv->_chanDev->_rxtxSerial->_serial_handle;
    return GetFIFOComputerRS232Port(pRS232Port, count);
}

The issue is that the _rxtxSerial property is showing an error (class rp::standalone::rplidar::ChannelDevice has no member "_rxtxSeral"), which is odd since when tracing the reaction of RPlidarDriver it creates SerialChannelDevice which is the following. And it seems to have _rxtxSerial available. Not sure if I have to expose it on the SDK level so it can be referenced?

I think the reference to _rxtxSerial shows as rp::standalone::rplidar::SerialChannelDevice::_rxtxSerial in the SDK in rplidar_driver_serial.h, which I think it should be referencable in RPLIDAR.h?

[rplidar_driver_serial.h]

class SerialChannelDevice :public ChannelDevice
{
public:
    rp::hal::serial_rxtx  * _rxtxSerial;
    bool _closePending;

    SerialChannelDevice():_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx()){}

    bool bind(const char * portname, uint32_t baudrate)
    {
        _closePending = false;
        return _rxtxSerial->bind(portname, baudrate);
    }
    bool open()
    {
        return _rxtxSerial->open();
    }
    void close()
    {
        _closePending = true;
        _rxtxSerial->cancelOperation();
        _rxtxSerial->close();
    }
    void flush()
    {
        _rxtxSerial->flush(0);
    }
    bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL)
    {
        if (_closePending) return false;
        return (_rxtxSerial->waitfordata(data_count, timeout, returned_size) == rp::hal::serial_rxtx::ANS_OK);
    }
    int senddata(const _u8 * data, size_t size)
    {
        return _rxtxSerial->senddata(data, size) ;
    }
    int recvdata(unsigned char * data, size_t size)
    {
        size_t lenRec = 0;
        lenRec = _rxtxSerial->recvdata(data, size);
        return lenRec;
    }
    void setDTR()
    {
        _rxtxSerial->setDTR();
    }
    void clearDTR()
    {
        _rxtxSerial->clearDTR();
    }
    void ReleaseRxTx()
    {
        rp::hal::serial_rxtx::ReleaseRxTx(_rxtxSerial);
    }
};

So when debugging hardwarex in RPLIDAR.h I can see that _rxtxSerial is being refereced, however for SerialChannelDevice, (image)

(*((rp::arch::net::raw_serial*)(*((rp::standalone::rplidar::SerialChannelDevice*)pRPLIDAR->drv->_chanDev))._rxtxSerial))._serial_handle

This all is probably my lack of knowledge of C++ programming ...

lebarsfa commented 4 years ago

Yes, it seems due to C++ inheritance things that are blocking that (see e.g. https://stackoverflow.com/questions/19501838/get-derived-type-via-base-class-virtual-function). I quickly tried something like

rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(drv->_chanDev);
rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

Then there is a problem with _serial_handle defined as protected in raw_serial class (see e.g. https://stackoverflow.com/questions/19862952/accessing-protected-function-from-another-class-c), it seems we can do that to access to it

namespace rp {
    namespace arch {
        namespace net {
            class raw_raw_serial : public raw_serial {
            public:
                raw_raw_serial(raw_serial rs) : raw_serial(rs) {}
                HANDLE get_serial_handle()
                {
                    //return this->raw_serial::_serial_handle;
                    return this->_serial_handle;
                }
            };
        }
    }
}

so that then we can do

rp::arch::net::raw_raw_serial rrs = *prs;
HANDLE hDev = rrs.get_serial_handle();

and then it should be possible to call Windows API functions using hDev... You might need to include before headers

#include "arch/win32/arch_win32.h"
#include "arch/win32/net_serial.h"
#include "hal/abs_rxtx.h"
#include "hal/thread.h"
#include "hal/types.h"
#include "hal/assert.h"
#include "hal/locker.h"
#include "hal/socket.h"
#include "hal/event.h"
#include "rplidar_driver_impl.h"
#include "rplidar_driver_serial.h"

ismet55555 commented 4 years ago

To be honest, I don't think I could have figured that out ...

So here is how I implemented it and it ALMOST works

I added the extra raw_raw_serial class with the exposed _serial_handle to the bottom of net_serial.h in the SDK.

Added the few lines you posted to RPLIDAR.h in to my custom GetFIFORPLIDAR() method

// Getting the length of the FIFO on the RS232 serial port
inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count)
{
rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

rp::arch::net::raw_raw_serial rrs = *prs;
HANDLE hDev = rrs.get_serial_handle();

return GetFIFOComputerRS232Port(rrs.get_serial_handle(), count);
}

Added the include statements you posted. into RPLIDAR.h BUT I was not able to build the solution as it was I needed to add rplidar_cmd.h and rplidar_driver.h so:
```
#include "arch/win32/arch_win32.h"
#include "arch/win32/net_serial.h"
```

include "hal/abs_rxtx.h"

include "hal/thread.h"

include "hal/types.h"

include "hal/assert.h"

include "hal/locker.h"

include "hal/socket.h"

include "hal/event.h"

include "rplidar_cmd.h"

include "rplidar_driver.h"

include "rplidar_driver_impl.h"

include "rplidar_driver_serial.h"

- Build both, SDK and hardwarex
- Open MATLAB and run `hardwarex_setup.m`

Here is where I  am getting an error that was odd:

Error using loadlibrary Failed to preprocess the input file. Output from preprocessor is:hardwarex.h rplidar_sdk\sdk\sdk\include\rplidar_driver.h(39): fatal error C1189: #error: "The RPlidar SDK requires a C++ compiler to be built"

Error in hardwarex_setup (line 16) loadlibrary('hardwarex', 'hardwarex.h', 'notempdir', ...


Which is coming from `rplidar_driver.h` 
```cpp
#ifndef __cplusplus
#error "The RPlidar SDK requires a C++ compiler to be built"
#endif

I initially thought that somehow the C++ compiler wasn't set up so I ran mex -setup and things seem to be in order. I def am able to go through all the steps without the include statements and added code.

>> mex -setup
MEX configured to use 'Microsoft Visual C++ 2017 (C)' for C language compilation.
Warning: The MATLAB C and Fortran API has changed to support MATLAB
     variables with more than 2^32-1 elements. You will be required
     to update your code to utilize the new API.
     You can find more information about this at:
     https://www.mathworks.com/help/matlab/matlab_external/upgrading-mex-files-to-use-64-bit-api.html.

To choose a different C compiler, select one from the following:
MinGW64 Compiler (C)  mex -setup:'C:\Program Files\MATLAB\R2018b\bin\win64\mexopts\mingw64.xml' C
Microsoft Visual C++ 2015 (C)  mex -setup:'C:\Program Files\MATLAB\R2018b\bin\win64\mexopts\msvc2015.xml' C
Microsoft Visual C++ 2017 (C)  mex -setup:C:\Users\ihandzic\AppData\Roaming\MathWorks\MATLAB\R2018b\mex_C_win64.xml C

To choose a different language, select one from the following:
 mex -setup C++ 
 mex -setup FORTRAN

Checking the Visual Studio code project, the linkers seem to be pointing to where the SDK outputted the static library.

If i get rid of the block of code in the SDK with the __cplusplus macro check, it all builds, however when running the MATLAB hardware_setup.m script, it yields a ton of errors ...

lebarsfa commented 4 years ago

I am not sure what is happening, maybe it is related to the order or place of the #include... If you consider it's OK to modify the SDK, the problem of _serial_handle defined as protected in raw_serial class can be more easily solved by replacing protected with public, and maybe also remove the volatile (I am not sure but maybe it was causing problems at runtime) before _serial_handle in net_serial.h. For the rest, in RPLIDAR.h try to replace

#include "../include/rplidar.h"
using namespace rp::standalone::rplidar;

with

#include "../include/rplidar.h"

//#include "arch/win32/arch_win32.h"
#include "arch/win32/net_serial.h"
#include "hal/abs_rxtx.h"
#include "hal/thread.h"
//#include "hal/types.h"
//#include "hal/assert.h"
#include "hal/locker.h"
//#include "hal/socket.h"
#include "hal/event.h"
#include "rplidar_driver_impl.h"
#include "rplidar_driver_serial.h"

 // Because _serial_handle is protected...
namespace rp {
    namespace arch {
        namespace net {
            class raw_raw_serial : public raw_serial {
            public:
                raw_raw_serial(raw_serial rs) : raw_serial(rs) {}
                HANDLE get_serial_handle()
                {
                    //return this->raw_serial::_serial_handle;
                    return this->_serial_handle;
                }
            };
        }
    }
}

using namespace rp::standalone::rplidar;

and then as a test I tried to replace

    if (IS_FAIL(pRPLIDAR->drv->stop()))
    {
        printf("A RPLIDAR is not responding correctly : stop() failed. \n");
        return EXIT_FAILURE;
    }

with

    rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
    rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);
    //rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);
    //HANDLE hDev = rrs.get_serial_handle();
    HANDLE hDev = prs->_serial_handle;

    unsigned char reqbuf[] = {0xA5,0x25};

    // Send request.
    if (WriteAllComputerRS232Port(hDev, reqbuf, sizeof(reqbuf)) != EXIT_SUCCESS)
    {
        printf("Error writing data to a RPLIDAR. \n");
        return EXIT_FAILURE;
    }

in StopRequestRPLIDAR (note that it is probably not fully equivalent to drv->stop())...

lebarsfa commented 4 years ago

Also, did you run also mex -setup C++ (and not only mex -setup) in MATLAB?

ismet55555 commented 4 years ago

I did not run mex -setup C++ before, and only ran mex -setup. I just ran mex -setup C++, trying it with all different compiler options, however I'm getting the same result. I kept it at Microsoft Visual C++ 2017

I'm about to tinker with your updated suggestions and see if I can get it to work ...

ismet55555 commented 4 years ago

Well I think it was the position of the include statments. I had the include statments in RPLIDAR.h right after #include "RS232Port.h" ... however needed it after using namespace rp::standalone::rplidar;

I also tried changing those SDK class properties that include _rxtxSerial to public.

Tried adding the extra class to net_serial.h and/or to RPLIDAR.h in the position you said.

I was able to build both, hardwarex and the SDK, (changing things every which way), and it looks like the reference to the handle seems off. I get this from MATLAB when running hardwarex_setup.m

c:\...\rplidar\RPLIDAR.h(735): error C2143: syntax error: missing ';' before ':'
c:\...\rplidar\RPLIDAR.h(736): error C2143: syntax error: missing ';' before ':'
c:\...\rplidar\RPLIDAR.h(741): error C2065: 'prs': undeclared identifier
c:\...\rplidar\RPLIDAR.h(741): error C2223: left of '->_serial_handle' must point to struct/union
c:\...\rplidar\RPLIDAR.h(736): error C2045: 'rp': label redefined

which seems to be that reference (tried the commented out code as well)

    rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
    rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

    //rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);
    //HANDLE hDev = rrs.get_serial_handle();

    HANDLE hDev = prs->_serial_handle;

    return GetFIFOComputerRS232Port(hDev, count);

So tried it on linux, and linux def doesn't like it since the reference seems to be different in its rplidar_sdk\sdk\sdk\src\arch\linux\net_serial.h:

    bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0);
    void _init();

    char _portName[200];
    uint32_t _baudrate;
    uint32_t _flags;

    int serial_fd;

    size_t required_tx_cnt;
    size_t required_rx_cnt;

    int    _selfpipe[2];
    bool   _operation_aborted;

Ultimately we are using the LIDAR in Linux. I wasn't initially aware until now that the reference would only work on Windows, given that the Windows serial port handle is: (\rplidar_sdk\sdk\sdk\src\arch\win32\net_serial.h)

    bool open(const char * portname, _u32 baudrate, _u32 flags);
    void _init();

    char _portName[20];
    uint32_t _baudrate;
    uint32_t _flags;

    OVERLAPPED _ro, _wo;
    OVERLAPPED _wait_o;
    HANDLE _serial_handle;
    DCB _dcb;
    COMMTIMEOUTS _co;

Would the serial port reference for linux be serial_fd?

lebarsfa commented 4 years ago

Yes, for Linux it is very likely to be serial_fd. Note that if you changed protected to public in net_serial.h you can remove

namespace rp {
    namespace arch {
        namespace net {
            class raw_raw_serial : public raw_serial {
            public:
                raw_raw_serial(raw_serial rs) : raw_serial(rs) {}
                HANDLE get_serial_handle()
                {
                    //return this->raw_serial::_serial_handle;
                    return this->_serial_handle;
                }
            };
        }
    }
}

in case that part is causing problems, otherwise I would say you have to play with the order/place of the includes and maybe add or remove some includes from some SDK files...

ismet55555 commented 4 years ago

So maybe serial_fd is not what I am looking for. So the the GetFIFOComputerRS232Port(HANDLE hDev, int* count) I am trying to use, it needs some handle, however, the serial_fd is an integer. I know that it should be cast into HANDLE or void somehow, but I can't seem to figure it out.

So with everything in place, either with the public or protected way, and with and wihtout the exposing the protected properties (when it is set to protected), everything builds and runs fine if I exclude:

    rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
    rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

    rp::arch::net::raw_raw_serial rrs = *prs;
    HANDLE hDev = rrs.get_serial_handle();

    return GetFIFOComputerRS232Port(hDev, count);

That is, any combination of that block of code commented or not commented. So if any of that is used, it throws errors. First, at hardwarex compile, it complains invalid conversion from 'int' to 'HANDLE {aka void*} [-fpremissive]

Than when running hardwarex_setup.m I get:

Error using loadlibrary
Building hardwarex_thunk_glnxa64 failed.  Compiler output is:
/usr/bin/gcc -I"MAVLinkSDK" -I"/usr/local/include" -I"sbgECom/src" -I"sbgECom/common" -I"/usr/local/include/sbgECom/src"
-I"/usr/local/include/sbgECom/common" -I"rplidar_sdk/sdk/sdk/include" -I"rplidar_sdk/sdk/sdk/src"
-I"/usr/local/include/rplidar_sdk/sdk/sdk/include" -I"/usr/local/include/rplidar_sdk/sdk/sdk/src" -I"/usr/local/MATLAB/R2018b/extern/include"
-fexceptions -fPIC -fno-omit-frame-pointer -pthread -I"/rplidar"
-I"/rplidar" "hardwarex_thunk_glnxa64.c" -o "hardwarex_thunk_glnxa64.so"
-Wl,-E -shared
In file included from /rplidar/hardwarex.h:87:0,
                 from hardwarex_thunk_glnxa64.c:27:
/rplidar/RPLIDAR.h: In function ‘GetFIFORPLIDAR’:
/rplidar/RPLIDAR.h:735:5: error: expected expression before ‘:’ token
  rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);

     ^
/rplidar/RPLIDAR.h:736:2: error: duplicate label ‘rp’
  rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

  ^
/rplidar/RPLIDAR.h:735:2: note: previous definition of ‘rp’ was here
  rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);

  ^
/rplidar/RPLIDAR.h:736:5: error: expected expression before ‘:’ token
  rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

     ^
/rplidar/RPLIDAR.h:738:2: error: duplicate label ‘rp’
  rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);

  ^
/rplidar/RPLIDAR.h:735:2: note: previous definition of ‘rp’ was here
  rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);

  ^
/rplidar/RPLIDAR.h:738:5: error: expected expression before ‘:’ token
  rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);

     ^
/rplidar/RPLIDAR.h:739:16: error: ‘rrs’ undeclared (first use in this
function)
  HANDLE hDev = rrs.get_serial_handle();

                ^
/rplidar/RPLIDAR.h:739:16: note: each undeclared identifier is reported
only once for each function it appears in

I definitely need to study up on this more, because those references and dynamic_casting is pretty foreign to me

lebarsfa commented 4 years ago

About serial_fd, if think it is really what you need but you need to adapt GetFIFOComputerRS232Port() so that it calls Linux serial port functions, and you should probably not use HANDLE type any more since Linux is not supposed to use it. For the build errors, I will try to check on Linux when I have the time, it looks as if RPLIDAR.h is built as C code and not C++ (but maybe it is something else...)... Also, on Windows I was using MATLAB 2017 and not 2018, I don't know if there could be changes in the behavior depending on the version...

ismet55555 commented 4 years ago

Thanks for the swift response again. Let me see if I can modify GetFIFOComputerRS232Port() to fit serial_fd and see if I can get around MATLABs compile errors

ismet55555 commented 4 years ago

Question: So you mentioned you are running MATLAB R2017b, what compiler are you using with matlab's loadlibrary function to compile the interface files on Linux?

I'm getting a MATLAB compile error on the the :: part of rp::standalone::rplidar::SerialChannelDevice*, that first :: ... I really think it's the compiler I'm using on Ubuntu.

The MATLAB docs say that for MATLAB R2018b, the recommend GCC 6.3.x versions.

When running g++ --version on my system, I get, 5.4.0. That's running on Ubuntu 16.04 (i know i know, pretty retro, but that's what I'm working with now)

I will upgrade and try it out ...

lebarsfa commented 4 years ago

The prebuilt hardwarex.so in the latest release were built from https://travis-ci.org/github/ENSTABretagneRobotics/Hardware-MATLAB/jobs/714875860. However I do not know how to make a test in MATLAB in the Travis automated build system so I just tested the interface with Python for that build, but maybe I made other tests manually in virtual machines that I probably deleted later. Anyway, I just tested a virtual machine with Ubuntu 16.04 64 bit with MATLAB R2010b, same g++ version and RPLIDAR A3, it seemed to work without complaining much. To be able to check the usability of the serial_fd variable, I changed protected to public in arch/linux/net_serial.h, in RPLIDAR.h I replaced

#include "../include/rplidar.h"
using namespace rp::standalone::rplidar;

with

#include "../include/rplidar.h"

#ifdef _WIN32
//#include "arch/win32/arch_win32.h"
#include "arch/win32/net_serial.h"
#else
//#include "arch/linux/arch_linux.h"
#include "arch/linux/net_serial.h"
#endif // _WIN32
#include "hal/abs_rxtx.h"
#include "hal/thread.h"
//#include "hal/types.h"
//#include "hal/assert.h"
#include "hal/locker.h"
//#include "hal/socket.h"
#include "hal/event.h"
#include "rplidar_driver_impl.h"
#include "rplidar_driver_serial.h"

// Because _serial_handle is protected...
namespace rp {
    namespace arch {
        namespace net {
            class raw_raw_serial : public raw_serial {
            public:
                raw_raw_serial(raw_serial rs) : raw_serial(rs) {}
#ifdef _WIN32
                HANDLE get_serial_handle()
                {
                    //return this->raw_serial::_serial_handle;
                    return this->_serial_handle;
                }
#else
                int get_serial_fd()
                {
                    //return this->raw_serial::serial_fd;
                    return this->serial_fd;
                }
#endif // _WIN32
            };
        }
    }
}

using namespace rp::standalone::rplidar;

(note that the part namespace rp {...} can be removed), and replaced

    if (IS_FAIL(pRPLIDAR->drv->stop()))
    {
        printf("A RPLIDAR is not responding correctly : stop() failed. \n");
        return EXIT_FAILURE;
    }

with

    rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
    rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);
    //rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);
#ifdef _WIN32
    //HANDLE hDev = rrs.get_serial_handle();
    HANDLE hDev = prs->_serial_handle;
#else
    //int hDev = rrs.get_serial_fd();
    int hDev = prs->serial_fd;
#endif // _WIN32

    unsigned char reqbuf[] = {0xA5,0x25};

    // Send request.
    if (WriteAllComputerRS232Port(hDev, reqbuf, sizeof(reqbuf)) != EXIT_SUCCESS)
    {
        printf("Error writing data to a RPLIDAR. \n");
        return EXIT_FAILURE;
    }

in StopRequestRPLIDAR (note that inside WriteAllComputerRS232Port(), the write() Linux API function is called with the hDev interpreted as an int). Can you confirm that if you start from the latest release code and just make those modifications you have a problem?

ismet55555 commented 4 years ago

Well... without my additional GetFIFOComputerRS232Port() and with the changes you send, it compiles and works just fine. That is, everything constant with the above changes, that stop() works ...

So here is what I have before any changes:


// Getting the length of the FIFO on the RS232 serial port, this is fine without the SDK obviously.
inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count)
{
    RS232PORT* pRS232Port = &pRPLIDAR->RS232Port;
    return GetFIFOComputerRS232Port(pRS232Port->hDev, count); 
}

And here is what does not work:

inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count)
{
    rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
    rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);
    //rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);
#ifdef _WIN32
    //HANDLE hDev = rrs.get_serial_handle();
    HANDLE hDev = prs->_serial_handle;
#else
    //int hDev = rrs.get_serial_fd();
    int hDev = prs->serial_fd;
#endif // _WIN32

     return GetFIFOComputerRS232Port(hDev, count); 
}

Getting this error in the second instance:

Error using loadlibrary
Building hardwarex_thunk_glnxa64 failed.  Compiler output is:
/usr/bin/gcc -I"MAVLinkSDK" -I"/usr/local/include" -I"sbgECom/src" -I"sbgECom/common"
-I"/usr/local/include/sbgECom/src" -I"/usr/local/include/sbgECom/common"
-I"rplidar_sdk/sdk/sdk/include" -I"rplidar_sdk/sdk/sdk/src"
-I"/usr/local/include/rplidar_sdk/sdk/sdk/include"
-I"/usr/local/include/rplidar_sdk/sdk/sdk/src" -I"/usr/local/MATLAB/R2018b/extern/include"
-fexceptions -fPIC -fno-omit-frame-pointer -pthread
-I"/rplidar"
-I"/rplidar"
"hardwarex_thunk_glnxa64.c" -o "hardwarex_thunk_glnxa64.so" -Wl,-E -shared
In file included from
/rplidar/hardwarex.h:87:0,
                 from hardwarex_thunk_glnxa64.c:27:
/rplidar/RPLIDAR.h: In
function ‘GetFIFORPLIDAR’:
/rplidar/RPLIDAR.h:759:5:
error: expected expression before ‘:’ token
  rp::standalone::rplidar::SerialChannelDevice* pscd =
  dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);

     ^
/rplidar/RPLIDAR.h:760:2:
error: duplicate label ‘rp’
  rp::arch::net::raw_serial* prs =
  dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

  ^~
s/rplidar/RPLIDAR.h:759:2:
note: previous definition of ‘rp’ was here
  rp::standalone::rplidar::SerialChannelDevice* pscd =
  dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);

  ^~
/rplidar/RPLIDAR.h:760:5:
error: expected expression before ‘:’ token
  rp::arch::net::raw_serial* prs =
  dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);

     ^
/rplidar/RPLIDAR.h:767:13:
error: ‘prs’ undeclared (first use in this function)
  int hDev = prs->serial_fd;

             ^~~
/rplidar/RPLIDAR.h:767:13:
note: each undeclared identifier is reported only once for each function it appears in
/rplidar/RPLIDAR.h:774:34:
warning: passing argument 1 of ‘GetFIFOComputerRS232Port’ makes pointer from integer
without a cast [-Wint-conversion]
  return GetFIFOComputerRS232Port(hDev, count);

                                  ^~~~
In file included from
/rplidar/RS232Port.h:25:0,
                 from
/rplidar/SBG.h:14,
                 from
                 /home/xonar/projects/xonar_matlab_scripts_NEW_4_SDK/common_functions/rplidar/hardwarex.h:76,
                                  from hardwarex_thunk_glnxa64.c:27:
/rplidar/OSComputerRS232Port.h:1442:12:
note: expected ‘HANDLE {aka void *}’ but argument is of type ‘int’
 inline int GetFIFOComputerRS232Port(HANDLE hDev, int* count)

            ^~~~~~~~~~~~~~~~~~~~~~~~

Error in hardwarex_init (line 19)
            [notfound,warnings]=loadlibrary('hardwarex', 'hardwarex.h', 'includepath',
            'MAVLinkSDK', 'includepath', '/usr/local/include', 'includepath',
            'sbgECom/src', 'includepath', 'sbgECom/common', 'includepath',
            '/usr/local/include/sbgECom/src', 'includepath',
            '/usr/local/include/sbgECom/common', 'includepath',
            'rplidar_sdk/sdk/sdk/include', 'includepath', 'rplidar_sdk/sdk/sdk/src',
            'includepath', '/usr/local/include/rplidar_sdk/sdk/sdk/include',
            'includepath', '/usr/local/include/rplidar_sdk/sdk/sdk/src');

Error in test_rplidar_SIMPLE (line 20)
hardwarex_init;

I think there is something small missing here ... Let me hack at it some more ...

ismet55555 commented 4 years ago

Well.... so I did the following (emulating what the other methods are doing) and it compiles fine and runs fine... returns the count. I think i needed the ENBALE_RPLIDAR_SDK_SUPPORT check ...

// Getting the length of the FIFO on the RS232 serial port
inline int GetFIFORPLIDAR(RPLIDAR* pRPLIDAR, int* count)
{
#ifdef ENABLE_RPLIDAR_SDK_SUPPORT
    rp::standalone::rplidar::SerialChannelDevice* pscd = dynamic_cast<rp::standalone::rplidar::SerialChannelDevice*>(pRPLIDAR->drv->_chanDev);
    rp::arch::net::raw_serial* prs = dynamic_cast<rp::arch::net::raw_serial*>(pscd->_rxtxSerial);
    //rp::arch::net::raw_raw_serial rrs = rp::arch::net::raw_raw_serial(*prs);
#ifdef _WIN32
    //HANDLE hDev = rrs.get_serial_handle();
    HANDLE hDev = prs->_serial_handle;
#else
    //int hDev = rrs.get_serial_fd();
    int hDev = prs->serial_fd;
#endif // _WIN32
    // Get the FIFO form the Port
    return GetFIFOComputerRS232Port(hDev, count
#else
    RS232PORT* pRS232Port = &pRPLIDAR->RS232Port;
    return GetFIFOComputerRS232Port(pRS232Port->hDev, count)
#endif // ENABLE_RPLIDAR_SDK_SUPPORT 
}

ismet55555 commented 4 years ago

Yeah, I tried it different ways, and I think it works with keeping those SDK variables as public. I think that is good enough for us here.

But thank you very much for the continuous and thorough help. Again, I do not think I could have figured this out without your help. Much appreciated!

lebarsfa commented 4 years ago

OK, indeed that code was assuming ENABLE_RPLIDAR_SDK_SUPPORT case...