InertialSense::releaseDevice() closes DeviceLogger associated with de…

…vice.

Light rework to InertialSense::OpenSerialPorts() to use ISDevice.
Made InertialSense::registerDevice() and InertialSense::registerNewDevice() from private to public scope.
Added mutex guards to ISDevice classes which access ports.

src/ISDevice.h

@@ -44,15 +44,18 @@ class ISDevice {
     static std::string getFirmwareInfo(const dev_info_t& devInfo, int detail = 1, eHdwRunStates hdwRunState = eHdwRunStates::HDW_STATE_APP);
 
     ISDevice() {
+        //std::cout << "Creating empty ISDevice: " << this << std::endl;
         flashCfg.checksum = (uint32_t)-1;
     }
 
     ISDevice(port_handle_t _port) {
+        // std::cout << "Creating ISDevice for port " << portName(_port) << " " << this << std::endl;
         flashCfg.checksum = (uint32_t)-1;
         port = _port;
     }
 
     ISDevice(const ISDevice& src) : devLogger(src.devLogger) {
+        // std::cout << "Creating ISDevice copy from " << ISDevice::getIdAsString(src.devInfo)  << " " << this << std::endl;
         port = src.port;
         hdwId = src.hdwId;
         hdwRunState = src.hdwRunState;
@@ -67,11 +70,15 @@ class ISDevice {
     }
 
     ~ISDevice() {
+//        if (((hdwId != IS_HARDWARE_TYPE_UNKNOWN) && (hdwId != IS_HARDWARE_ANY)) || (devInfo.serialNumber != 0))
+//            std::cout << "Destroying ISDevice " << getDescription() << ". " << this << std::endl;
+
         //if (port && serialPortIsOpen(port))
         //    serialPortClose(port);
-        port = 0;
+        devInfo = {};
         hdwId = IS_HARDWARE_TYPE_UNKNOWN;
         hdwRunState = HDW_STATE_UNKNOWN;
+        port = 0;
     }
 
     ISDevice& operator=(const ISDevice& src) {
@@ -92,7 +99,9 @@ class ISDevice {
     /**
      * @return true is this ISDevice has a valid, and open port
      */
-    bool isConnected() { return (port && serialPortIsOpen(port)); }
+    bool isConnected() { std::lock_guard<std::recursive_mutex> lock(portMutex); return (port && serialPortIsOpen(port)); }
+
+    std::string getPortName() {  std::lock_guard<std::recursive_mutex> lock(portMutex); return (port ? portName(port) : ""); }
 
     bool Update();
     bool step();
@@ -107,8 +116,8 @@ class ISDevice {
     // Convenience Functions
     bool BroadcastBinaryData(uint32_t dataId, int periodMultiple);
     void BroadcastBinaryDataRmcPreset(uint64_t rmcPreset, uint32_t rmcOptions) { std::lock_guard<std::recursive_mutex> lock(portMutex); comManagerGetDataRmc(port, rmcPreset, rmcOptions); }
-    void GetData(eDataIDs dataId, uint16_t length=0, uint16_t offset=0, uint16_t period=0) {  std::lock_guard<std::recursive_mutex> lock(portMutex); comManagerGetData(port, dataId, length, offset, period); }
-    int SendData(eDataIDs dataId, const uint8_t* data, uint32_t length, uint32_t offset = 0) {  std::lock_guard<std::recursive_mutex> lock(portMutex); return comManagerSendData(port, data, dataId, length, offset); }
+    void GetData(eDataIDs dataId, uint16_t length=0, uint16_t offset=0, uint16_t period=0) { std::lock_guard<std::recursive_mutex> lock(portMutex); comManagerGetData(port, dataId, length, offset, period); }
+    int SendData(eDataIDs dataId, const uint8_t* data, uint32_t length, uint32_t offset = 0) { std::lock_guard<std::recursive_mutex> lock(portMutex); return comManagerSendData(port, data, dataId, length, offset); }
     int SendRaw(const uint8_t* data, uint32_t length) {  std::lock_guard<std::recursive_mutex> lock(portMutex); return comManagerSendRaw(port, data, length); }
 
     int SendNmea(const std::string& nmeaMsg);

src/InertialSense.cpp

@@ -233,14 +233,17 @@ bool InertialSense::releaseDevice(ISDevice* device, bool closePort)
     if (deviceIter == m_comManagerState.devices.end())
         return false;
 
+    auto dl = Logger()->getDeviceLogByPort(device->port);
+    if (dl) dl->CloseAllFiles();
+
     if (closePort && device->port) {
         serialPortClose(device->port);
         freeSerialPort(device->port, false);
     }
 
     m_comManagerState.devices.erase(deviceIter); // erase only remove the ISDevice* from the list, but doesn't release/free the instance itself
     device->port = NULL;
-    delete device;
+    delete device; // causes a double free??
 
     return true;
 }
@@ -575,8 +578,24 @@ bool InertialSense::Update()
     return anyOpen;
 }
 
+/**
+ * TCP Server primary handler - parses data received via the first connected device, looking for RTCM3/UBLOX protocol
+ * and sends that same data out to the underlying ISTCPServer's connected clients
+ * @return always returns true, though should probably return false if the m_tcpServer has no active clients (or something)
+ */
 bool InertialSense::UpdateServer()
 {
+    // As I understand it, this function is responsible for reading RTCM3, and other useful data sets from connected IMX,
+    // and publishing it to connected clients (because it is the server).
+
+    // This is a little different, kind-of, because we don't actually let the ISDevice parse any data (but maybe we should).
+    // Rather, we parse data directly from the COMM buffer, so we can determine what type of data it is (though we should
+    // already know this). then, based on the packet type (RTCM3/UBLOX, etc) we'll send that data out to the socket.
+    //
+    // Ideally, the TCP socket would also be a port_handle_t, and we'd essentially plumb up a passthrough:  Let the ISDevice
+    // parse data FROM the device, call a custom callback for the data types we're interested in, and then when those are
+    // received, we'd send them right back out the TCP port_handle_t.  Perhaps one day; not today.
+
     // as a tcp server, only the first serial port is read from
     port_handle_t port = m_comManagerState.devices.front()->port;
     is_comm_instance_t *comm = &(COMM_PORT(port)->comm);
@@ -630,6 +649,10 @@ bool InertialSense::UpdateServer()
     return true;
 }
 
+/**
+ *
+ * @return
+ */
 bool InertialSense::UpdateClient()
 {
     if (m_clientStream == NULLPTR)
@@ -1469,17 +1492,18 @@ bool InertialSense::OpenSerialPorts(const char* portPattern, int baudRate)
                 break;
             }
 
+            ISDevice device;    // we'll reuse this device for querying each port
             for (auto port : m_serialPorts) {
-                ISDevice device(port);
+                device.port = port;
                 switch (checkType) {
                     case 0 :
-                        comManagerSendRaw(port, (uint8_t *) NMEA_CMD_QUERY_DEVICE_INFO, NMEA_CMD_SIZE);
+                        device.SendRaw((uint8_t *) NMEA_CMD_QUERY_DEVICE_INFO, NMEA_CMD_SIZE);
                         break;
                     case 1 :
-                        comManagerGetData(port, DID_DEV_INFO, 0, 0, 0);
+                        device.GetData(DID_DEV_INFO, 0, 0, 0);
                         break;
                     case 2 :
-                        // comManagerGetData(port, DID_DEV_INFO, 0, 0, 0);
+                        // device.GetData(DID_DEV_INFO, 0, 0, 0);
                         break;
                     case 3 :
                         device.queryDeviceInfoISbl();
@@ -1489,12 +1513,12 @@ bool InertialSense::OpenSerialPorts(const char* portPattern, int baudRate)
 
                 for (int i = 0; i < 3; i++) {
                     SLEEP_MS(5);
-                    comManagerStep();
+                    device.step();
                 }
 
                 if ((device.hdwId != 0) && (device.hdwRunState != 0)) {
                     debug_message("[DBG] Received response from serial port '%s'. Registering device.\n", SERIAL_PORT(port)->portName);
-                    registerDevice(&device);
+                    registerNewDevice(port, device.devInfo);
                 } else if (SERIAL_PORT(port)->errorCode) {
                     // there was some other janky issue with the requested port; even though the device technically exists, its in a bad state. Let's just drop it now.
                     debug_message("[DBG] There was an error accessing serial port '%s': %s\n", SERIAL_PORT(port)->portName, SERIAL_PORT(port)->error);
@@ -1883,8 +1907,9 @@ std::vector<std::string> InertialSense::checkForNewPorts(std::vector<std::string
         if (ignored)
             continue;
 
-        if (DeviceByPortName(portName) == nullptr)
+        if (DeviceByPortName(portName) == nullptr) {
             new_ports.push_back(portName);
+        }
     }
 
     return new_ports;

src/InertialSense.h

@@ -689,6 +689,9 @@ class InertialSense : public iISTcpServerDelegate
     InertialSense::com_manager_cpp_state_t* ComManagerState() { return &m_comManagerState; }
     // ISDevice* ComManagerDevice(port_handle_t port=0) { if (portId(port) >= (int)m_comManagerState.devices.size()) return NULLPTR; return &(m_comManagerState.devices[portId(port)]); }
 
+    bool registerDevice(ISDevice* device);
+    ISDevice* registerNewDevice(port_handle_t port, dev_info_t devInfo = {});
+
     bool freeSerialPort(port_handle_t port, bool releaseDevice = false);
     bool releaseDevice(ISDevice* device, bool closePort = true);
 
@@ -751,8 +754,6 @@ class InertialSense : public iISTcpServerDelegate
     bool UpdateClient();
     bool EnableLogging(const std::string& path, const cISLogger::sSaveOptions& options = cISLogger::sSaveOptions());
     void DisableLogging();
-    bool registerDevice(ISDevice* device);
-    ISDevice* registerNewDevice(port_handle_t port, dev_info_t devInfo);
     bool HasReceivedDeviceInfo(ISDevice* device);
     bool HasReceivedDeviceInfoFromAllDevices();
     void RemoveDevice(size_t index);

tests/gtest_helpers.h

@@ -1,6 +1,8 @@
 #include <gtest/gtest.h>
+#ifndef GTEST_HELPERS_H
+#define GTEST_HELPERS_H
 
-#if defined(GTestColor) // ((ROS_VERSION_MAJOR == 1) && (ROS_VERSION_MINOR <= 13))
+#if defined(GTestColor)
 namespace testing
 {
     namespace internal
@@ -15,19 +17,62 @@ namespace testing
         extern void ColoredPrintf(GTestColor color, const char* fmt, ...);
     }
 }
-#define PRINTF(...)  do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN, "[          ] "); testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } while (0)
+
+#define ANSI_BLACK       "\u001b[30m"
+#define ANSI_DARKGRAY    "\u001b[1,30m"
+#define ANSI_DARKRED     "\u001b[31m"
+#define ANSI_RED         "\u001b[1,31m"
+#define ANSI_DARKGREEN   "\u001b[32m"
+#define ANSI_GREEN       "\u001b[1,32m"
+#define ANSI_BROWN       "\u001b[33m"
+#define ANSI_YELLOW      "\u001b[1,33m"
+#define ANSI_DARKBLUE    "\u001b[34m"
+#define ANSI_BLUE        "\u001b[1,34m"
+#define ANSI_DARKMAGENTA "\u001b[35m"
+#define ANSI_MAGENTA     "\u001b[1,35m"
+#define ANSI_DARKCYAN    "\u001b[36m"
+#define ANSI_CYAN        "\u001b[1,36m"
+#define ANSI_GRAY        "\u001b[37m"
+#define ANSI_WHITE       "\u001b[1,37m"
+#define ANSI_DEFAULT     "\u001b[39m"
+#define ANSI_RESET       "\u001b[0m"
+
+#define TEST_PRINTF(...)  do { testing::internal::ColoredPrintf(testing::internal::COLOR_GREEN, "[          ] "); testing::internal::ColoredPrintf(testing::internal::COLOR_YELLOW, __VA_ARGS__); } while (0)
 #else
-#define PRINTF(...)
+#define ANSI_BLACK       ""
+#define ANSI_DARKGRAY    ""
+#define ANSI_DARKRED     ""
+#define ANSI_RED         ""
+#define ANSI_DARKGREEN   ""
+#define ANSI_GREEN       ""
+#define ANSI_BROWN       ""
+#define ANSI_YELLOW      ""
+#define ANSI_DARKBLUE    ""
+#define ANSI_BLUE        ""
+#define ANSI_DARKMAGENTA ""
+#define ANSI_MAGENTA     ""
+#define ANSI_DARKCYAN    ""
+#define ANSI_CYAN        ""
+#define ANSI_GRAY        ""
+#define ANSI_WHITE       ""
+#define ANSI_DEFAULT     ""
+#define ANSI_RESET       ""
+
+#define TEST_PRINTF(...)  do { printf("[          ] "); printf(__VA_ARGS__); fflush(stdout); } while (0);
 #endif
 
+
+
 // C++ stream interface
 class TestCout : public std::stringstream
 {
 public:
     ~TestCout()
     {
-        PRINTF("%s",str().c_str());
+        TEST_PRINTF("%s",str().c_str());
     }
 };
 
 #define TEST_COUT  TestCout()
+
+#endif // GTEST_HELPERS_H