logging to stdout

modules/catkin_ws/src/VeloxProtocolLib/CMakeLists.txt

@@ -64,9 +64,9 @@ set(PUBLIC_HEADER_FILES
         include/VeloxProtocolLib/Connection.h
         ${CMAKE_CURRENT_BINARY_DIR}/VeloxProtocolLibConfig.h)
 
-foreach(HEADER ${PUBLIC_HEADER_FILES})
+foreach (HEADER ${PUBLIC_HEADER_FILES})
     set(PUBLIC_HEADER_FILES_COMBINED "${PUBLIC_HEADER_FILES_COMBINED}\\;${HEADER}")
-endforeach()
+endforeach ()
 
 add_library(VeloxProtocolLib SHARED ${SOURCE_FILES})
 
@@ -86,7 +86,7 @@ find_package(Boost REQUIRED COMPONENTS system regex)
 link_libraries(${Boost_LIBRARIES})
 
 set(INSTALL_PACKAGE ON)
-if(INSTALL_PACKAGE)
+if (INSTALL_PACKAGE)
     #############################
     # Specify install directories
     #############################
@@ -96,12 +96,12 @@ if(INSTALL_PACKAGE)
     set(INSTALL_CMAKE_DIR lib/VeloxProtocolLib/CMake)
 
     # Make relative paths absolute (needed later on)
-    foreach(p LIB INCLUDE CMAKE)
+    foreach (p LIB INCLUDE CMAKE)
         set(var INSTALL_${p}_DIR)
-        if(NOT IS_ABSOLUTE "${${var}}")
+        if (NOT IS_ABSOLUTE "${${var}}")
             set(${var} "${CMAKE_INSTALL_PREFIX}/${${var}}")
-        endif()
-    endforeach()
+        endif ()
+    endforeach ()
 
     ####################################################
     # Create the *Config.cmake and *ConfigVersion files
@@ -143,17 +143,21 @@ if(INSTALL_PACKAGE)
     # Install the export set for use with the install-tree
     install(EXPORT VeloxProtocolLibTargets
             DESTINATION "${INSTALL_CMAKE_DIR}")
-endif()
+endif ()
 
 #######
 # Test
 #######
-add_executable(TerminalControl ${SOURCE_FILES} test/TerminalControl.cpp)
-add_executable(SpeedMeasure ${SOURCE_FILES} test/SpeedMeasure.cpp)
+set(TEST_SOURCE_FILES
+        test/USS_SRF02.h
+        test/USS_SRF02.cpp)
+
+add_executable(TerminalControl ${SOURCE_FILES} ${TEST_SOURCE_FILES} test/TerminalControl.cpp)
+add_executable(SpeedMeasure ${SOURCE_FILES} ${TEST_SOURCE_FILES} test/SpeedMeasure.cpp)
 
 # NetworkingLib
 target_include_directories(TerminalControl PUBLIC ${NetworkingLib_INCLUDE_DIRS})
-target_link_libraries(TerminalControl NetworkingLib)
+target_link_libraries(TerminalControl NetworkingLib libwiringPi.so)
 
 target_include_directories(SpeedMeasure PUBLIC ${NetworkingLib_INCLUDE_DIRS})
-target_link_libraries(SpeedMeasure NetworkingLib)
\ No newline at end of file
+target_link_libraries(SpeedMeasure NetworkingLib libwiringPi.so)
\ No newline at end of file

modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.cpp

+#include "ultrasonic/USS_SRF02.h"
+#include "wiringPiI2C.h"
+
+#include <iostream>
+
+#include <unistd.h>
+
+constexpr int USS_SRF02::SRF02_SDA;
+constexpr int USS_SRF02::SRF02_SCL;
+constexpr int USS_SRF02::DEVICE_ADDRESS1;
+constexpr int USS_SRF02::DEVICE_ADDRESS2;
+constexpr int USS_SRF02::DEVICE_ADDRESS3;
+constexpr char USS_SRF02::DEVICE[];
+constexpr int USS_SRF02::COMMAND_REGISTER;
+constexpr int USS_SRF02::RESULT_HIGH_BYTE;
+constexpr int USS_SRF02::RESULT_LOW_BYTE;
+constexpr int USS_SRF02::RANGING_MODE_CM;
+constexpr int USS_SRF02::DELAY;
+
+USS_SRF02::USS_SRF02(int devId)
+{
+    fd = wiringPiI2CSetupInterface(DEVICE, devId);
+    if (fd == -1)
+        throw std::runtime_error{"Device not found!\n"};
+}
+
+int USS_SRF02::getDistance()
+{
+    wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
+    usleep(DELAY * 1000);
+    return wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+}

modules/catkin_ws/src/VeloxProtocolLib/test/USS_SRF02.h

+#ifndef USS_SRF02_H_
+#define USS_SRF02_H_
+
+class USS_SRF02
+{
+public:
+    //gpio pins where the sonar is connected
+    static constexpr int SRF02_SDA = 8; //i2c data pin ;
+    static constexpr int SRF02_SCL = 9; //i2c clock pin ;
+
+    // addresses of the sonars, the number is also as a sticker on the devices themselves
+    static constexpr int DEVICE_ADDRESS1 = 0x74;
+    static constexpr int DEVICE_ADDRESS2 = 0x76;
+    static constexpr int DEVICE_ADDRESS3 = 0x77;
+
+    //path to i2c file
+    static constexpr char DEVICE[] = "/dev/i2c-1";
+
+    static constexpr int COMMAND_REGISTER = 0x00;
+    static constexpr int RESULT_HIGH_BYTE = 0x02;
+    static constexpr int RESULT_LOW_BYTE = 0x03;
+    static constexpr int RANGING_MODE_CM = 0x51;
+
+    static constexpr int DELAY = 70; //70 ms for ranging to finish
+
+    explicit USS_SRF02(int devId);
+
+    int getDistance();
+
+private:
+    int fd;
+};
+
+
+#endif /* USS_SRF02_H_ */

modules/catkin_ws/src/car/include/logging/MessageOStream.h

@@ -52,6 +52,7 @@ public:
         result << ">> " << stream.module << " [" << stream.ossKey.str() << "] " << stream.ossValue.str() << "\n";
         msg.logMsg = result.str();
         stream.logStringPublisher.publish(msg);
+        std::cout << msg.logMsg;
         stream.ossValue = std::ostringstream{};
         stream.ossKey = std::ostringstream{};
         stream.currStream = &stream.ossValue;

modules/catkin_ws/src/car/src/logging/Logging.cpp

@@ -22,7 +22,7 @@ Logging::Logging()
 
 void Logging::onInit()
 {
-    NODELET_INFO("Logging::onInit -- START");
+    std::cout << ">> Logging [onInit] START\n";
 
     camData = nh_.subscribe("camData", 1, &Logging::camDataCallback, this);
     ccData = nh_.subscribe("ccData", 1, &Logging::ccDataCallback, this);
@@ -43,7 +43,7 @@ void Logging::onInit()
                 loggingBuffer, std::min(loggingBuffer.size(), MAX_MESSAGE_SIZE));
         });
 
-    NODELET_INFO("Logging::onInit -- END");
+    std::cout << ">> Logging [onInit] END\n";
 }
 
 void Logging::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
@@ -94,4 +94,4 @@ void Logging::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
     loggingOStream << ">> ussData [distance] " << inMsg->distance << "\n";
 }
 
-}
+}
\ No newline at end of file