improved timing for uss data and sending timestamp

modules/catkin_ws/src/car/CMakeLists.txt

@@ -150,8 +150,11 @@ add_library(ultrasonic ${USS_SOURCE_FILES})
 add_dependencies(ultrasonic ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
 target_link_libraries(ultrasonic
     ${catkin_LIBRARIES}
-    libwiringPi.so
-)
+    NetworkingLib
+    libwiringPi.so)
+target_include_directories(ultrasonic PUBLIC
+        ${NetworkingLib_INCLUDE_DIRS}
+        ${MessageLib_INCLUDE_DIRS})
 
 find_package( OpenCV REQUIRED )
 add_library(camera ${CAMERA_SOURCE_FILES})

modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h

@@ -8,6 +8,7 @@
 #include "StreamMedianFilter.h"
 #include <thread>
 #include <logging/MessageOStream.h>
+#include "NetworkingLib/Timer.h"
 
 namespace car
 {
@@ -25,7 +26,8 @@ private:
     ros::Publisher ussData;
     UltrasonicSensor sensor;
     StreamMedianFilter streamMedianFilter;
-    std::thread sensorThread;
+    networking::Networking net;
+    networking::time::Timer::Ptr timer;
 };
 
 }

modules/catkin_ws/src/car/include/ultrasonic/UltrasonicSensor.h

@@ -21,7 +21,7 @@ public:
     static constexpr int RESULT_LOW_BYTE = 0x03;
     static constexpr int RANGING_MODE_CM = 0x51;
 
-    static constexpr int DELAY = 10; //70 ms for ranging to finish
+    static constexpr int DELAY = 70; //70 ms for ranging to finish
 
     static constexpr int MAX_DISTANCE = 255;
 

modules/catkin_ws/src/car/msg/ussDataMsg.msg

 float32 distance
+time timestamp

modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp

@@ -23,18 +23,19 @@ void Ultrasonic::onInit()
 
     ussData = nh.advertise<ussDataMsg>("ussData", 1);
     sensor.init();
-    sensorThread = std::thread{
-        [this]
+    timer = networking::time::Timer::create(net);
+    timer->startPeriodicTimeout(
+        std::chrono::milliseconds(UltrasonicSensor::DELAY),
+        [&]
         {
-            for (;;)
-            {
-                ussDataMsg msg;
-                auto distance = streamMedianFilter.moveWindow(sensor.getDistance());
-                msg.distance = distance;
-                ussData.publish(msg);
-            }
-        }};
+            ussDataMsg msg;
+            auto distance = streamMedianFilter.moveWindow(sensor.getDistance());
+            msg.distance = distance;
+            msg.timestamp = ros::Time::now();
+            ussData.publish(msg);
+        });
 
     messageOStream.write("onInit", "END");
 }
+
 }

modules/catkin_ws/src/car/src/ultrasonic/UltrasonicSensor.cpp

@@ -36,7 +36,6 @@ int UltrasonicSensor::getDistance()
         throw std::runtime_error{"Sensor not initialized!\n"};
 
     wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
-    usleep(DELAY * 1000);
     int distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
     if (distance == 0)  // 0 means distance too far
         distance = 255;