From f43633e8307bfa269c80308aa1c4548a913c6bbe Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Mon, 14 May 2018 16:11:59 +0200
Subject: [PATCH] improved timing for uss data and sending timestamp
---
modules/catkin_ws/src/car/CMakeLists.txt | 7 +++++--
.../src/car/include/ultrasonic/Ultrasonic.h | 4 +++-
.../car/include/ultrasonic/UltrasonicSensor.h | 2 +-
modules/catkin_ws/src/car/msg/ussDataMsg.msg | 1 +
.../src/car/src/ultrasonic/Ultrasonic.cpp | 21 ++++++++++---------
.../car/src/ultrasonic/UltrasonicSensor.cpp | 1 -
6 files changed, 21 insertions(+), 15 deletions(-)
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index a3da590d..9d7ff7fc 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/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})
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h b/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h
index 4c5c900a..d8d20227 100644
--- a/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h
+++ b/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;
};
}
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/UltrasonicSensor.h b/modules/catkin_ws/src/car/include/ultrasonic/UltrasonicSensor.h
index 7d1711a6..8150e355 100644
--- a/modules/catkin_ws/src/car/include/ultrasonic/UltrasonicSensor.h
+++ b/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;
diff --git a/modules/catkin_ws/src/car/msg/ussDataMsg.msg b/modules/catkin_ws/src/car/msg/ussDataMsg.msg
index 5cd4939c..5d17d4de 100644
--- a/modules/catkin_ws/src/car/msg/ussDataMsg.msg
+++ b/modules/catkin_ws/src/car/msg/ussDataMsg.msg
@@ -1 +1,2 @@
float32 distance
+time timestamp
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
index 4de34f01..847e7b17 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
+++ b/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");
}
+
}
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/UltrasonicSensor.cpp b/modules/catkin_ws/src/car/src/ultrasonic/UltrasonicSensor.cpp
index 266fa489..f61a6013 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/UltrasonicSensor.cpp
+++ b/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;
--
GitLab