From e670862c006e3b5a32dba3a99c80568a7523eeb8 Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Wed, 9 May 2018 13:26:24 +0200
Subject: [PATCH] Implemented key-value logging
---
modules/catkin_ws/src/PC/src/Logger.cpp | 2 +-
.../catkin_ws/src/car/include/camera/Camera.h | 29 +++++------
.../src/car/include/lanekeeping/Lanekeeping.h | 12 ++---
.../src/car/include/logging/Logging.h | 24 +---------
.../src/car/include/logging/MessageOStream.h | 48 +++++++++++++++----
.../catkin_ws/src/car/launch/fullstart.launch | 2 +-
.../catkin_ws/src/car/src/camera/Camera.cpp | 16 +++----
.../src/car/src/environment/Environment.cpp | 10 ++--
.../src/car/src/lanekeeping/Lanekeeping.cpp | 18 +++----
.../catkin_ws/src/car/src/logging/Logging.cpp | 13 +++--
.../src/car/src/mainNode/CruiseController.cpp | 15 ++++--
.../src/car/src/mainNode/EgoMotion.cpp | 14 ++----
.../src/car/src/mainNode/MainNode.cpp | 6 +--
.../car/src/mainNode/PlatoonController.cpp | 15 +++---
.../catkin_ws/src/car/src/mavLink/MavLink.cpp | 8 ++--
.../src/car/src/ultrasonic/USS_SRF02.cpp | 10 ++--
.../src/car/src/ultrasonic/Ultrasonic.cpp | 8 ++--
17 files changed, 132 insertions(+), 118 deletions(-)
diff --git a/modules/catkin_ws/src/PC/src/Logger.cpp b/modules/catkin_ws/src/PC/src/Logger.cpp
index fd7930c6..7d5588cb 100644
--- a/modules/catkin_ws/src/PC/src/Logger.cpp
+++ b/modules/catkin_ws/src/PC/src/Logger.cpp
@@ -39,7 +39,7 @@ void Logger::log(AsyncState::Ptr state)
if (!responseMessage.empty())
std::cout << responseMessage;
- state->self->timer->startTimeout(100ms, [state]
+ state->self->timer->startTimeout(1ms, [state]
{ state->self->log(state); });
});
}
diff --git a/modules/catkin_ws/src/car/include/camera/Camera.h b/modules/catkin_ws/src/car/include/camera/Camera.h
index 6d86a308..e04b4038 100644
--- a/modules/catkin_ws/src/car/include/camera/Camera.h
+++ b/modules/catkin_ws/src/car/include/camera/Camera.h
@@ -3,22 +3,23 @@
#include <nodelet/nodelet.h>
#include <ros/ros.h>
-#include "boost/thread.hpp"
+#include <thread>
+#include <logging/MessageOStream.h>
namespace car
{
- class Camera : public nodelet::Nodelet
- {
- public:
- virtual void onInit();
- Camera(ros::NodeHandle &nh, std::string &name);
- Camera();
- ~Camera();
- private:
- ros::NodeHandle nh_;
- std::string name_;
- ros::Publisher camData;
- boost::thread main;
- };
+class Camera : public nodelet::Nodelet
+{
+public:
+ Camera();
+
+ void onInit() override;
+
+private:
+ ros::NodeHandle nh;
+ MessageOStream messageOStream;
+ ros::Publisher camData;
+ std::thread cameraThread;
+};
}
#endif
diff --git a/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h b/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
index c94ccd82..d008ec47 100644
--- a/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
+++ b/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
@@ -3,6 +3,7 @@
#include <nodelet/nodelet.h>
#include <ros/ros.h>
+#include <logging/MessageOStream.h>
#include "car/laneDataMsg.h"
#include "car/camDataMsg.h"
@@ -12,18 +13,13 @@ namespace car
class Lanekeeping : public nodelet::Nodelet
{
public:
- virtual void onInit();
-
- Lanekeeping(ros::NodeHandle & nh, std::string & name);
-
Lanekeeping();
- ~Lanekeeping();
+ void onInit() override;
private:
- ros::NodeHandle nh_;
- std::string name_;
-
+ ros::NodeHandle nh;
+ MessageOStream messageOStream;
ros::Publisher laneData;
ros::Subscriber camData;
diff --git a/modules/catkin_ws/src/car/include/logging/Logging.h b/modules/catkin_ws/src/car/include/logging/Logging.h
index 8c9ea5ee..1d2f1641 100644
--- a/modules/catkin_ws/src/car/include/logging/Logging.h
+++ b/modules/catkin_ws/src/car/include/logging/Logging.h
@@ -29,7 +29,7 @@ class Logging : public nodelet::Nodelet
public:
static constexpr std::size_t PORT{10207};
static constexpr std::size_t MAX_MESSAGE_SIZE{1024};
- static constexpr std::size_t MAX_BUFFER_SIZE{0x100000}; // 1MB
+ static constexpr std::size_t MAX_BUFFER_SIZE{std::numeric_limits<std::size_t>::max()};
Logging();
@@ -42,26 +42,6 @@ private:
using ResponseMessage = std::string;
};
- class OSyncStream
- {
- public:
- OSyncStream(boost::asio::streambuf & buffer, std::mutex & mutex)
- : stream(&buffer), mutex(mutex)
- {}
-
- template<typename T>
- friend OSyncStream & operator<<(OSyncStream & oSyncStream, const T & value)
- {
- std::lock_guard<std::mutex> lock{oSyncStream.mutex};
- oSyncStream.stream << value;
- return oSyncStream;
- }
-
- private:
- std::ostream stream;
- std::mutex & mutex;
- };
-
ros::NodeHandle nh_;
std::string name_;
@@ -77,7 +57,7 @@ private:
std::mutex loggingMutex;
boost::asio::streambuf loggingBuffer;
- OSyncStream loggingOStream;
+ std::ostream loggingOStream;
networking::Networking net;
networking::service::Server<LoggingService>::Ptr loggingServer;
diff --git a/modules/catkin_ws/src/car/include/logging/MessageOStream.h b/modules/catkin_ws/src/car/include/logging/MessageOStream.h
index efc179f5..0e568c4d 100644
--- a/modules/catkin_ws/src/car/include/logging/MessageOStream.h
+++ b/modules/catkin_ws/src/car/include/logging/MessageOStream.h
@@ -8,6 +8,7 @@
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <string>
+#include <utility>
#include "car/logStringMsg.h"
namespace car
@@ -18,32 +19,63 @@ class MessageOStream
public:
using Manipulator = MessageOStream & (*)(MessageOStream &);
- explicit MessageOStream(ros::NodeHandle & nh)
- : logStringPublisher(nh.advertise<logStringMsg>("logString", 1))
+ explicit MessageOStream(ros::NodeHandle & nh, std::string module)
+ : module(std::move(module))
+ , logStringPublisher(nh.advertise<logStringMsg>("logString", 1))
+ , currStream{&ossValue}
{}
template<typename T>
- friend MessageOStream & operator<<(MessageOStream & stream, const T & value)
+ friend MessageOStream & operator<<(MessageOStream & stream, const T & data)
{
- stream.oss << value;
+ (*stream.currStream) << data;
return stream;
}
friend MessageOStream & operator<<(MessageOStream & stream, Manipulator manipulator)
- { return manipulator(stream); }
+ {
+ return manipulator(stream);
+ }
+
+ template<typename Key, typename Value>
+ MessageOStream & write(const Key & key, const Value & value)
+ {
+ return *this << MessageOStream::key << key
+ << MessageOStream::value << value
+ << MessageOStream::flush;
+ }
static MessageOStream & flush(MessageOStream & stream)
{
logStringMsg msg;
- msg.logMsg = stream.oss.str();
+ std::ostringstream result;
+ result << ">> " << stream.module << " [" << stream.ossKey.str() << "] " << stream.ossValue.str() << "\n";
+ msg.logMsg = result.str();
stream.logStringPublisher.publish(msg);
- stream.oss = std::ostringstream{};
+ stream.ossValue = std::ostringstream{};
+ stream.ossKey = std::ostringstream{};
+ stream.currStream = &stream.ossValue;
+ return stream;
+ }
+
+ static MessageOStream & value(MessageOStream & stream)
+ {
+ stream.currStream = &stream.ossValue;
+ return stream;
+ }
+
+ static MessageOStream & key(MessageOStream & stream)
+ {
+ stream.currStream = &stream.ossKey;
return stream;
}
private:
+ std::string module;
ros::Publisher logStringPublisher;
- std::ostringstream oss;
+ std::ostringstream ossValue;
+ std::ostringstream ossKey;
+ std::ostringstream * currStream;
};
}
diff --git a/modules/catkin_ws/src/car/launch/fullstart.launch b/modules/catkin_ws/src/car/launch/fullstart.launch
index 17c5e8a9..cf9e223c 100644
--- a/modules/catkin_ws/src/car/launch/fullstart.launch
+++ b/modules/catkin_ws/src/car/launch/fullstart.launch
@@ -3,13 +3,13 @@ dsdf <node pkg="nodelet" type="nodelet" name="Master" args="manager" output="sc
<param name="num_worker_threads" value="16" />
</node>
+ <node pkg="nodelet" type="nodelet" name="nodelet_logging" args="load car/logging Master"/>
<node pkg="nodelet" type="nodelet" name="nodelet_main_node" args="load car/main_node Master"/>
<node pkg="nodelet" type="nodelet" name="nodelet_environment" args="load car/environment Master"/>
<node pkg="nodelet" type="nodelet" name="nodelet_ultrasonic" args="load car/ultrasonic Master"/>
<node pkg="nodelet" type="nodelet" name="nodelet_camera" args="load car/camera Master"/>
<node pkg="nodelet" type="nodelet" name="nodelet_lanekeeping" args="load car/lanekeeping Master"/>
<node pkg="nodelet" type="nodelet" name="nodelet_mav_link" args="load car/mav_link Master"/>
- <node pkg="nodelet" type="nodelet" name="nodelet_logging" args="load car/logging Master"/>
</launch>
<!--
diff --git a/modules/catkin_ws/src/car/src/camera/Camera.cpp b/modules/catkin_ws/src/car/src/camera/Camera.cpp
index fb71a63f..598d4e96 100644
--- a/modules/catkin_ws/src/car/src/camera/Camera.cpp
+++ b/modules/catkin_ws/src/car/src/camera/Camera.cpp
@@ -10,20 +10,16 @@ PLUGINLIB_EXPORT_CLASS(car::Camera, nodelet::Nodelet);
namespace car
{
-Camera::Camera(ros::NodeHandle & nh, std::string & name) : nh_(nh), name_(name)
-{}
-
Camera::Camera()
-{}
-
-Camera::~Camera()
+ : messageOStream(nh, "Camera")
{}
void Camera::onInit()
{
- NODELET_INFO("Camera::onInit -- START");
- camData = nh_.advertise<camDataMsg>("camData", 1);
- main = boost::thread(
+ messageOStream.write("onInit", "START");
+
+ camData = nh.advertise<camDataMsg>("camData", 1);
+ cameraThread = std::thread(
[this]()
{
ros::Rate rate{1};
@@ -35,6 +31,6 @@ void Camera::onInit()
}
});
- NODELET_INFO("Camera::onInit -- END");
+ messageOStream.write("onInit", "END");
}
}
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index 303c6f9a..f44b51b5 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -12,20 +12,24 @@ namespace car
{
Environment::Environment()
- : messageOStream(nh)
+ : messageOStream(nh, "Environment")
, distance(0)
, relativeSpeed(0)
{}
void Environment::onInit()
{
- messageOStream << "Environment::onInit -- START" << MessageOStream::flush;
+ messageOStream << MessageOStream::key << "onInit()"
+ << MessageOStream::value << "START"
+ << MessageOStream::flush;
environmentData = nh.advertise<environmentDataMsg>("environmentData", 1);
ussData = nh.subscribe("ussData", 1, &Environment::ussDataCallback, this);
camData = nh.subscribe("camData", 1, &Environment::camDataCallback, this);
- messageOStream << "Environment::onInit -- END" << MessageOStream::flush;
+ messageOStream << MessageOStream::key << "onInit()"
+ << MessageOStream::value << "END"
+ << MessageOStream::flush;
}
void Environment::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
diff --git a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp b/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
index 499be717..f059a82d 100644
--- a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
+++ b/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
@@ -10,22 +10,19 @@ PLUGINLIB_EXPORT_CLASS(car::Lanekeeping, nodelet::Nodelet);
namespace car
{
-Lanekeeping::Lanekeeping(ros::NodeHandle & nh, std::string & name) :
- nh_(nh), name_(name)
-{}
Lanekeeping::Lanekeeping()
-{}
-
-Lanekeeping::~Lanekeeping()
+ : messageOStream(nh, "Lanekeeping")
{}
void Lanekeeping::onInit()
{
- NODELET_INFO("Lanekeeping::onInit -- START");
- laneData = nh_.advertise<laneDataMsg>("laneData", 1);
- camData = nh_.subscribe("camData", 1, &Lanekeeping::camDataCallback, this);
- NODELET_INFO("Lanekeeping::onInit -- END");
+ messageOStream.write("onInit", "START");
+
+ laneData = nh.advertise<laneDataMsg>("laneData", 1);
+ camData = nh.subscribe("camData", 1, &Lanekeeping::camDataCallback, this);
+
+ messageOStream.write("onInit", "END");
}
void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr & inMsg)
@@ -33,5 +30,4 @@ void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr & inMsg)
// std::cout << "Lanekeeping recived new cam data ( ).\n";
}
-
}
diff --git a/modules/catkin_ws/src/car/src/logging/Logging.cpp b/modules/catkin_ws/src/car/src/logging/Logging.cpp
index 1e634f17..968aefd6 100644
--- a/modules/catkin_ws/src/car/src/logging/Logging.cpp
+++ b/modules/catkin_ws/src/car/src/logging/Logging.cpp
@@ -17,7 +17,7 @@ constexpr std::size_t Logging::MAX_BUFFER_SIZE;
Logging::Logging()
: loggingBuffer(MAX_BUFFER_SIZE)
- , loggingOStream(loggingBuffer, loggingMutex)
+ , loggingOStream(&loggingBuffer)
{}
void Logging::onInit()
@@ -29,7 +29,7 @@ void Logging::onInit()
environmentData = nh_.subscribe("environmentData", 1, &Logging::environmentDataCallback, this);
laneData = nh_.subscribe("laneData", 1, &Logging::laneDataCallback, this);
logEnabled = nh_.subscribe("logEnabled", 1, &Logging::logEnabledCallback, this);
- logString = nh_.subscribe("logString", 1, &Logging::logStringCallback, this);
+ logString = nh_.subscribe("logString", 1000, &Logging::logStringCallback, this);
rcEnabled = nh_.subscribe("rcEnabled", 1, &Logging::rcEnabledCallback, this);
stmData = nh_.subscribe("stmData", 1, &Logging::stmDataCallback, this);
ussData = nh_.subscribe("ussData", 1, &Logging::ussDataCallback, this);
@@ -58,7 +58,8 @@ void Logging::camDataCallback(const camDataMsg::ConstPtr & inMsg)
void Logging::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
{
- loggingOStream << "environment [distance]: " << "\n";
+ std::lock_guard<std::mutex> lock{loggingMutex};
+ loggingOStream << ">> environmentData [distance] " << inMsg->distance << "\n";
}
void Logging::laneDataCallback(const laneDataMsg::ConstPtr & inMsg)
@@ -73,7 +74,8 @@ void Logging::logEnabledCallback(const logEnabledMsg::ConstPtr & inMsg)
void Logging::logStringCallback(const logStringMsg::ConstPtr & inMsg)
{
- loggingOStream << "message: " << inMsg->logMsg << "\n";
+ std::lock_guard<std::mutex> lock{loggingMutex};
+ loggingOStream << inMsg->logMsg;
}
void Logging::rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg)
@@ -88,7 +90,8 @@ void Logging::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
void Logging::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
{
- loggingOStream << "uss [distance]: " << inMsg->distance << "\n";
+ std::lock_guard<std::mutex> lock{loggingMutex};
+ loggingOStream << ">> ussData [distance] " << inMsg->distance << "\n";
}
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
index 689073a1..6d4ac4ac 100644
--- a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -9,7 +9,7 @@ CruiseController::CruiseController(ros::NodeHandle & nh,
platoonProtocol::VehicleFacade & c2c,
EgoMotion & egoMotion,
PlatoonController & platoonController)
- : messageOStream(nh)
+ : messageOStream(nh, "CruiseController")
, c2c(c2c)
, egoMotion(egoMotion)
, platoonController(platoonController)
@@ -43,19 +43,21 @@ void CruiseController::run()
void CruiseController::run_ACC(float target_speed)
{
- messageOStream << "CruiseController::run_ACC(" << target_speed << ")" << MessageOStream::flush;
+ messageOStream << MessageOStream::key << "run_ACC"
+ << MessageOStream::value << "target_speed: " << target_speed;
+
float v_self = egoMotion.getOwnSpeed();
float d = egoMotion.getDistance();
float crash_dist = crash_time * v_self;
if (d < crash_dist)
{
- messageOStream << "in crash_dist" << MessageOStream::flush;
+ messageOStream << MessageOStream::value << "\nin crash_dist";
speed = target_speed * (d - stop_dist) / crash_dist;
}
else
{
- messageOStream << "not in crash_dist" << MessageOStream::flush;
+ messageOStream << MessageOStream::value << "\nnot in crash_dist";
speed = target_speed;
}
@@ -64,11 +66,14 @@ void CruiseController::run_ACC(float target_speed)
ccDataMsg outMsg;
outMsg.speed = speed;
ccData.publish(outMsg);
+
+ messageOStream << MessageOStream::flush;
}
void CruiseController::run_CACC()
{
- messageOStream << "CruiseController::run_CACC" << MessageOStream::flush;
+ messageOStream << MessageOStream::key << "run_CACC"
+ << MessageOStream::flush;
auto config = platoonController.getPlatoonConfig().get();
float PS = config.platoonSpeed;
diff --git a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
index 301a4646..ce6e299f 100644
--- a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
@@ -5,7 +5,7 @@ namespace car
{
EgoMotion::EgoMotion(ros::NodeHandle nh)
- : messageOStream(nh)
+ : messageOStream(nh, "EgoMotion")
, platoonControllerNotify()
, cruiseControllerNotify()
{
@@ -15,23 +15,19 @@ EgoMotion::EgoMotion(ros::NodeHandle nh)
void EgoMotion::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
{
- messageOStream << "EgoMotion recived new environmentData data ("
- << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << MessageOStream::flush;
distance = inMsg->distance;
relSpeed = inMsg->relativeSpeed;
computeAndNotify();
- messageOStream << "EgoMotion done with environmentData data ("
- << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << MessageOStream::flush;
+
+ messageOStream.write("distance", distance).write("relSpeed", relSpeed);
}
void EgoMotion::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
{
- messageOStream << "EgoMotion recived new stmData data ("
- << inMsg->speed << ", " << inMsg->angle << ")." << MessageOStream::flush;
stmSpeed = inMsg->speed;
computeAndNotify();
- messageOStream << "EgoMotion done with stmData data ("
- << inMsg->speed << ", " << inMsg->angle << ")." << MessageOStream::flush;
+
+ messageOStream.write("stmSpeed", stmSpeed);
}
void EgoMotion::computeAndNotify()
diff --git a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index d50083e2..d565355d 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -61,7 +61,7 @@ platoonProtocol::NetworkInfo readNetworkInfo() {
MainNode::MainNode()
: nh()
- , messageOStream(nh)
+ , messageOStream(nh, "MainNode")
, c2cNet()
, c2c(platoonProtocol::Vehicle::Role::FOLLOWER, c2cNet, readNetworkInfo())
, pcNet()
@@ -81,8 +81,8 @@ MainNode::MainNode()
void MainNode::onInit()
{
- messageOStream << "MainNode::onInit -- START";
- messageOStream << "MainNode::onInit -- END";
+ messageOStream.write("onInit", "START");
+ messageOStream.write("onInit", "END");
}
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index eaf6d79a..58c21b3b 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -11,7 +11,7 @@ PlatoonController::PlatoonController(ros::NodeHandle & nh,
platoonProtocol::VehicleFacade & c2c,
pc2car::CommandReceiver::Ptr pc,
EgoMotion & egoMotion)
- : messageOStream(nh)
+ : messageOStream(nh, "PlatoonController")
, cruiseControllerNotify()
, c2c(c2c)
, pc(std::move(pc))
@@ -67,7 +67,8 @@ void PlatoonController::setupC2C()
void PlatoonController::run()
{
- messageOStream << "PlatoonController was run." << MessageOStream::flush;
+ messageOStream.write("run", "");
+
switch (currState.load())
{
case PlatoonState::ACC:
@@ -151,8 +152,9 @@ void PlatoonController::run_CACC_FV()
// needs to be updated, so the new value can be pulled be CC
platoonConfig = c2c.getPlatoonConfig(); // TODO <- updateC2CConfig();
- messageOStream << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
- << ", wantsPlatoon = " << wantsPlatoon << MessageOStream::flush;
+ messageOStream << MessageOStream::key << "run_CACC_FV"
+ << MessageOStream::value << "inPlatoon: " << inPlatoon << "\nwantsPlatoon: " << wantsPlatoon
+ << MessageOStream::flush;
if (inPlatoon && wantsPlatoon)
{
@@ -174,8 +176,9 @@ void PlatoonController::run_CACC_LV()
bool inPlatoon = c2c.isPlatoonRunning();
bool wantsPlatoon = pc->isPlatoonEnabled().get();
- messageOStream << "Running PlatoonController::run_CACC_LV: inPlatoon = " << inPlatoon
- << ", wantsPlatoon = " << wantsPlatoon << MessageOStream::flush;
+ messageOStream << MessageOStream::key << "run_CACC_LV"
+ << MessageOStream::value << "inPlatoon: " << inPlatoon << "\nwantsPlatoon: " << wantsPlatoon
+ << MessageOStream::flush;
if (inPlatoon && wantsPlatoon)
{
diff --git a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
index 6b362421..e463b58c 100644
--- a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
+++ b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
@@ -11,12 +11,12 @@ namespace car
{
MavLink::MavLink()
- : messageOStream(nh)
+ : messageOStream(nh, "MavLink")
{}
void MavLink::onInit()
{
- messageOStream << "MavLink::onInit -- START" << MessageOStream::flush;
+ messageOStream.write("onInit", "START");
stmData = nh.advertise<stmDataMsg>("stmData", 1);
ccData = nh.subscribe("ccData", 1, &MavLink::ccDataCallback, this);
@@ -29,9 +29,9 @@ void MavLink::onInit()
[this]
{ onStmDataReceived(); },
[this]
- { messageOStream << "UART was closed!" << MessageOStream::flush; });
+ { messageOStream.write("ERROR", "UART was closed"); });
- messageOStream << "MavLink::onInit -- END" << MessageOStream::flush;
+ messageOStream.write("onInit", "END");
}
void MavLink::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
index bac34fe8..6cda3f79 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
@@ -19,14 +19,14 @@ 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"};
+ 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);
+ wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
usleep(DELAY * 1000);
- //return wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+ return wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
}
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
index 2109e5a1..3e8df4b9 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
@@ -13,12 +13,14 @@ namespace car
Ultrasonic::Ultrasonic()
: USS_SRF02(DEVICE_ADDRESS1)
- , messageOStream(nh)
+ , messageOStream(nh, "Ultrasonic")
{}
+static std::size_t counter = 0;
+
void Ultrasonic::onInit()
{
- messageOStream << "Ultrasonic::onInit -- START" << MessageOStream::flush;
+ messageOStream.write("onInit", "START");
ussData = nh.advertise<ussDataMsg>("ussData", 1);
sensorThread = std::thread(
@@ -33,6 +35,6 @@ void Ultrasonic::onInit()
}
});
- messageOStream << "Ultrasonic::onInit -- END" << MessageOStream::flush;
+ messageOStream.write("onInit", "STOP");
}
}
--
GitLab