From 583b375df3b4b55177c9d43c29eda75a9ea18025 Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Wed, 9 May 2018 03:42:37 +0200
Subject: [PATCH] improved logging by implementing MessageOStream
---
modules/catkin_ws/src/PC/src/main.cpp | 2 +-
modules/catkin_ws/src/car/CMakeLists.txt | 49 +++++--
.../src/car/include/environment/Environment.h | 11 +-
.../src/car/include/logging/Logging.h | 30 +++--
.../src/car/include/logging/MessageOStream.h | 51 ++++++++
.../car/include/mainNode/CruiseController.h | 33 ++---
.../src/car/include/mainNode/EgoMotion.h | 6 +-
.../src/car/include/mainNode/MainNode.h | 9 +-
.../car/include/mainNode/PlatoonController.h | 9 +-
.../src/car/include/mavLink/MavLink.h | 12 +-
.../include/ultrasonic/StreamMedianFilter.h | 4 +-
.../src/car/include/ultrasonic/USS_SRF02.h | 30 +++--
.../src/car/include/ultrasonic/Ultrasonic.h | 30 +++--
.../src/car/src/environment/Environment.cpp | 46 +++----
.../catkin_ws/src/car/src/logging/Logging.cpp | 40 ++----
.../src/car/src/mainNode/CruiseController.cpp | 101 +++++++++------
.../src/car/src/mainNode/EgoMotion.cpp | 21 +--
.../src/car/src/mainNode/MainNode.cpp | 122 +++++++-----------
.../car/src/mainNode/PlatoonController.cpp | 101 ++++++++++-----
.../catkin_ws/src/car/src/mavLink/MavLink.cpp | 26 ++--
.../car/src/ultrasonic/StreamMedianFilter.cpp | 4 +-
.../src/car/src/ultrasonic/USS_SRF02.cpp | 59 +++------
.../src/car/src/ultrasonic/Ultrasonic.cpp | 18 ++-
23 files changed, 435 insertions(+), 379 deletions(-)
create mode 100644 modules/catkin_ws/src/car/include/logging/MessageOStream.h
diff --git a/modules/catkin_ws/src/PC/src/main.cpp b/modules/catkin_ws/src/PC/src/main.cpp
index 79ec8f95..83b77ba7 100644
--- a/modules/catkin_ws/src/PC/src/main.cpp
+++ b/modules/catkin_ws/src/PC/src/main.cpp
@@ -6,7 +6,7 @@ int main()
using namespace std::chrono_literals;
float speed = 0.0f;
networking::Networking net;
- const std::string host{"10.5.37.195"};
+ const std::string host{"127.0.0.1"};
pc2car::CommandSender commandSender{net, host};
auto commandTimer = networking::time::Timer::create(net);
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index b71c91b9..90fc3a44 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/modules/catkin_ws/src/car/CMakeLists.txt
@@ -51,21 +51,14 @@ include_directories(
)
set(MAIN_NODE_SOURCE_FILES
- include/camera/Camera.h
- include/environment/Environment.h
+ include/logging/MessageOStream.h
include/exceptions/Exceptions.h
- include/lanekeeping/Lanekeeping.h
- include/logging/Logging.h
include/mainNode/CruiseController.h
include/mainNode/EgoMotion.h
include/mainNode/MainNode.h
include/mainNode/NotifiableThread.h
include/mainNode/PlatoonController.h
include/mainNode/PlatoonState.h
- include/mavLink/MavLink.h
- include/ultrasonic/StreamMedianFilter.h
- include/ultrasonic/Ultrasonic.h
- include/ultrasonic/USS_SRF02.h
src/mainNode/MainNode.cpp
src/mainNode/EgoMotion.cpp
src/mainNode/NotifiableThread.cpp
@@ -74,16 +67,48 @@ set(MAIN_NODE_SOURCE_FILES
)
set(ENVIRONMENT_SOURCE_FILES
+ include/logging/MessageOStream.h
+ include/environment/Environment.h
+ include/environment/KalmanFilter.h
+ include/exceptions/Exceptions.h
src/environment/Environment.cpp
src/environment/KalmanFilter.cpp
)
+set(LOGGING_SOURCE_FILES
+ include/logging/Logging.h
+ include/logging/MessageOStream.h
+ src/logging/Logging.cpp
+)
+
set(USS_SOURCE_FILES
+ include/logging/MessageOStream.h
+ include/ultrasonic/StreamMedianFilter.h
+ include/ultrasonic/USS_SRF02.h
+ include/ultrasonic/Ultrasonic.h
src/ultrasonic/StreamMedianFilter.cpp
src/ultrasonic/Ultrasonic.cpp
src/ultrasonic/USS_SRF02.cpp
)
+set(CAMERA_SOURCE_FILES
+ include/logging/MessageOStream.h
+ include/camera/Camera.h
+ src/camera/Camera.cpp
+)
+
+set(LANE_KEEPING_SOURCE_FILES
+ include/logging/MessageOStream.h
+ include/lanekeeping/Lanekeeping.h
+ src/lanekeeping/Lanekeeping.cpp
+)
+
+set(MAV_LINK_SOURCE_FILES
+ include/logging/MessageOStream.h
+ include/mavLink/MavLink.h
+ src/mavLink/MavLink.cpp
+)
+
set(LOCAL_INSTALL_DIR ${CMAKE_CURRENT_LIST_DIR}/../../install)
set(CMAKE_PREFIX_PATH "${LOCAL_INSTALL_DIR}")
@@ -110,7 +135,7 @@ add_library(environment ${ENVIRONMENT_SOURCE_FILES})
add_dependencies(environment ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(environment ${catkin_LIBRARIES})
-add_library(logging src/logging/Logging.cpp)
+add_library(logging ${LOGGING_SOURCE_FILES})
add_dependencies(logging ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(logging
${catkin_LIBRARIES}
@@ -125,15 +150,15 @@ target_link_libraries(ultrasonic
libwiringPi.so
)
-add_library(camera src/camera/Camera.cpp)
+add_library(camera ${CAMERA_SOURCE_FILES})
add_dependencies(camera ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(camera ${catkin_LIBRARIES})
-add_library(lanekeeping src/lanekeeping/Lanekeeping.cpp)
+add_library(lanekeeping ${LANE_KEEPING_SOURCE_FILES})
add_dependencies(lanekeeping ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(lanekeeping ${catkin_LIBRARIES})
-add_library(mav_link src/mavLink/MavLink.cpp)
+add_library(mav_link ${MAV_LINK_SOURCE_FILES})
add_dependencies(mav_link ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(mav_link
${catkin_LIBRARIES}
diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index 555eecbd..31ed9d49 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/modules/catkin_ws/src/car/include/environment/Environment.h
@@ -5,6 +5,7 @@
#include <ros/ros.h>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
+#include <logging/MessageOStream.h>
#include "car/ussDataMsg.h"
#include "car/camDataMsg.h"
@@ -17,15 +18,11 @@ class Environment : public nodelet::Nodelet
public:
virtual void onInit();
- Environment(ros::NodeHandle & nh, std::string & name);
-
Environment();
- ~Environment();
-
private:
- ros::NodeHandle nh_;
- std::string name_;
+ ros::NodeHandle nh;
+ MessageOStream messageOStream;
float distance;
float relativeSpeed;
@@ -36,8 +33,6 @@ private:
void ussDataCallback(const ussDataMsg::ConstPtr & inMsg);
void camDataCallback(const camDataMsg::ConstPtr & inMsg);
-
-
};
}
#endif
diff --git a/modules/catkin_ws/src/car/include/logging/Logging.h b/modules/catkin_ws/src/car/include/logging/Logging.h
index 65f13d92..8c9ea5ee 100644
--- a/modules/catkin_ws/src/car/include/logging/Logging.h
+++ b/modules/catkin_ws/src/car/include/logging/Logging.h
@@ -31,13 +31,9 @@ public:
static constexpr std::size_t MAX_MESSAGE_SIZE{1024};
static constexpr std::size_t MAX_BUFFER_SIZE{0x100000}; // 1MB
- void onInit() override;
-
- Logging(ros::NodeHandle & nh, std::string & name);
-
Logging();
- ~Logging();
+ void onInit() override;
private:
struct LoggingService
@@ -46,6 +42,26 @@ 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_;
@@ -61,13 +77,11 @@ private:
std::mutex loggingMutex;
boost::asio::streambuf loggingBuffer;
- std::ostream loggingStream;
+ OSyncStream loggingOStream;
networking::Networking net;
networking::service::Server<LoggingService>::Ptr loggingServer;
- void logMessage(const std::string & str);
-
void camDataCallback(const camDataMsg::ConstPtr & inMsg);
void ccDataCallback(const ccDataMsg::ConstPtr & inMsg);
void environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg);
diff --git a/modules/catkin_ws/src/car/include/logging/MessageOStream.h b/modules/catkin_ws/src/car/include/logging/MessageOStream.h
new file mode 100644
index 00000000..efc179f5
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/logging/MessageOStream.h
@@ -0,0 +1,51 @@
+//
+// Created by philipp on 08.05.18.
+//
+
+#ifndef CAR_LOGGINGSTREAM_H
+#define CAR_LOGGINGSTREAM_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include <string>
+#include "car/logStringMsg.h"
+
+namespace car
+{
+
+class MessageOStream
+{
+public:
+ using Manipulator = MessageOStream & (*)(MessageOStream &);
+
+ explicit MessageOStream(ros::NodeHandle & nh)
+ : logStringPublisher(nh.advertise<logStringMsg>("logString", 1))
+ {}
+
+ template<typename T>
+ friend MessageOStream & operator<<(MessageOStream & stream, const T & value)
+ {
+ stream.oss << value;
+ return stream;
+ }
+
+ friend MessageOStream & operator<<(MessageOStream & stream, Manipulator manipulator)
+ { return manipulator(stream); }
+
+ static MessageOStream & flush(MessageOStream & stream)
+ {
+ logStringMsg msg;
+ msg.logMsg = stream.oss.str();
+ stream.logStringPublisher.publish(msg);
+ stream.oss = std::ostringstream{};
+ return stream;
+ }
+
+private:
+ ros::Publisher logStringPublisher;
+ std::ostringstream oss;
+};
+
+}
+
+#endif //CAR_LOGGINGSTREAM_H
diff --git a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
index c2f16444..ad53a360 100644
--- a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
@@ -3,6 +3,7 @@
#include <nodelet/nodelet.h>
#include <ros/ros.h>
+#include <logging/MessageOStream.h>
#include "PlatoonProtocolLib/VehicleFacade.h"
#include "PlatoonController.h"
#include "EgoMotion.h"
@@ -10,22 +11,21 @@
namespace car
{
-class CruiseController
+class CruiseController
{
- public:
- CruiseController(platoonProtocol::VehicleFacade& c2c,
- EgoMotion& egoMotion,
- PlatoonController& platoonController,
- ros::NodeHandle & nh);
-
- ~CruiseController(){};
+public:
+ CruiseController(ros::NodeHandle & nh,
+ platoonProtocol::VehicleFacade & c2c,
+ EgoMotion & egoMotion,
+ PlatoonController & platoonController);
void run();
-
- private:
- platoonProtocol::VehicleFacade& c2c;
- EgoMotion& egoMotion;
- PlatoonController& platoonController;
+
+private:
+ MessageOStream messageOStream;
+ platoonProtocol::VehicleFacade & c2c;
+ EgoMotion & egoMotion;
+ PlatoonController & platoonController;
float min_speed{0};
float max_speed{20};
@@ -36,12 +36,13 @@ class CruiseController
float speed{0};
float units_conv_fac{1};
- float dist_pow {1.5};
+ float dist_pow{1.5};
float speed_pow{0.5};
-
+
void run_ACC(float target_speed);
+
void run_CACC();
-
+
ros::Publisher ccData;
};
}
diff --git a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
index 785eec3e..3817304c 100644
--- a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
+++ b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
@@ -6,6 +6,7 @@
#include <functional>
#include <atomic>
+#include <logging/MessageOStream.h>
#include "car/environmentDataMsg.h"
#include "car/stmDataMsg.h"
@@ -17,8 +18,7 @@ class EgoMotion
public:
using Callback = std::function<void()>;
- EgoMotion(ros::NodeHandle nh);
- ~EgoMotion(){};
+ explicit EgoMotion(ros::NodeHandle nh);
Callback platoonControllerNotify;
Callback cruiseControllerNotify;
@@ -28,6 +28,8 @@ class EgoMotion
float getOwnSpeed(){return stmSpeed;}
private:
+ MessageOStream messageOStream;
+
std::atomic<float> distance{0};
std::atomic<float> stmSpeed{0};
std::atomic<float> relSpeed{0};
diff --git a/modules/catkin_ws/src/car/include/mainNode/MainNode.h b/modules/catkin_ws/src/car/include/mainNode/MainNode.h
index 3be98c88..43d1dc4f 100644
--- a/modules/catkin_ws/src/car/include/mainNode/MainNode.h
+++ b/modules/catkin_ws/src/car/include/mainNode/MainNode.h
@@ -3,6 +3,7 @@
#include <nodelet/nodelet.h>
#include <ros/ros.h>
+#include <logging/MessageOStream.h>
#include "boost/thread.hpp"
#include "NotifiableThread.h"
@@ -20,17 +21,13 @@ namespace car
class MainNode : public nodelet::Nodelet
{
public:
- virtual void onInit();
-
- MainNode(ros::NodeHandle & nh, std::string & name);
-
MainNode();
- ~MainNode();
+ void onInit() override;
private:
ros::NodeHandle nh;
- std::string name;
+ MessageOStream messageOStream;
networking::Networking c2cNet; // thread + event queue
platoonProtocol::VehicleFacade c2c;
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index 41210518..97db8eeb 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
@@ -23,14 +23,16 @@ class PlatoonController
public:
using Callback = std::function<void()>;
- PlatoonController(platoonProtocol::VehicleFacade & c2c,
+ PlatoonController(ros::NodeHandle & nh,
+ platoonProtocol::VehicleFacade & c2c,
pc2car::CommandReceiver::Ptr pc,
EgoMotion & egoMotion);
void run();
- float getDesSpeed() { return desSpeed.get(); }
-
+ float getDesSpeed()
+ { return desSpeed.get(); }
+
Callback cruiseControllerNotify;
PlatoonState getCurrState() const
@@ -40,6 +42,7 @@ public:
{ return platoonConfig.getNonAtomicCopy(); }
private:
+ MessageOStream messageOStream;
platoonProtocol::VehicleFacade & c2c;
pc2car::CommandReceiver::Ptr pc;
EgoMotion & egoMotion;
diff --git a/modules/catkin_ws/src/car/include/mavLink/MavLink.h b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
index 63543688..29e9c3d4 100644
--- a/modules/catkin_ws/src/car/include/mavLink/MavLink.h
+++ b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
@@ -3,6 +3,7 @@
#include <nodelet/nodelet.h>
#include <ros/ros.h>
+#include <logging/MessageOStream.h>
#include "car/ccDataMsg.h"
#include "car/laneDataMsg.h"
@@ -17,22 +18,17 @@ namespace car
class MavLink : public nodelet::Nodelet
{
public:
- virtual void onInit();
-
- MavLink(ros::NodeHandle & nh, std::string & name);
+ void onInit() override;
MavLink();
- ~MavLink();
-
private:
- ros::NodeHandle nh_;
- std::string name_;
-
+ ros::NodeHandle nh;
ros::Publisher stmData;
ros::Subscriber ccData;
ros::Subscriber laneData;
ros::Subscriber rcEnabled;
+ MessageOStream messageOStream;
void ccDataCallback(const ccDataMsg::ConstPtr & inMsg);
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/StreamMedianFilter.h b/modules/catkin_ws/src/car/include/ultrasonic/StreamMedianFilter.h
index f32c547d..f3147fce 100644
--- a/modules/catkin_ws/src/car/include/ultrasonic/StreamMedianFilter.h
+++ b/modules/catkin_ws/src/car/include/ultrasonic/StreamMedianFilter.h
@@ -8,9 +8,7 @@ namespace car
class StreamMedianFilter
{
public:
- StreamMedianFilter(const int windowSize);
-
- ~StreamMedianFilter();
+ explicit StreamMedianFilter(int windowSize);
int moveWindow(int nextValue);
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h b/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
index 092d475a..698f5bb8 100644
--- a/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
+++ b/modules/catkin_ws/src/car/include/ultrasonic/USS_SRF02.h
@@ -1,20 +1,34 @@
#ifndef USS_SRF02_H_
#define USS_SRF02_H_
-class USS_SRF02{
-
+class USS_SRF02
+{
public:
- USS_SRF02(int devId);
+ //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 ;
- int getDistance();
+ // 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;
-private:
+ //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;
- void setup();
+ static constexpr int DELAY = 70; //70 ms for ranging to finish
- int devId;
+ explicit USS_SRF02(int devId);
- int fd;
+ int getDistance();
+
+private:
+ int fd;
};
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h b/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h
index c3d75a87..049822d8 100644
--- a/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h
+++ b/modules/catkin_ws/src/car/include/ultrasonic/Ultrasonic.h
@@ -6,22 +6,24 @@
#include "boost/thread.hpp"
#include "USS_SRF02.h"
#include <thread>
+#include <logging/MessageOStream.h>
namespace car
{
- class Ultrasonic : public USS_SRF02,
- public nodelet::Nodelet
- {
- public:
- virtual void onInit();
- Ultrasonic(ros::NodeHandle &nh, std::string &name);
- Ultrasonic();
- ~Ultrasonic();
- private:
- ros::NodeHandle nh_;
- std::string name_;
- ros::Publisher ussData;
- std::thread main;
- };
+
+class Ultrasonic : public USS_SRF02, public nodelet::Nodelet
+{
+public:
+ Ultrasonic();
+
+ void onInit() override;
+
+private:
+ ros::NodeHandle nh;
+ MessageOStream messageOStream;
+ ros::Publisher ussData;
+ std::thread sensorThread;
+};
+
}
#endif
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index 3fe8de92..303c6f9a 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -4,52 +4,44 @@
#include "environment/Environment.h"
#include "exceptions/Exceptions.h"
-#include "car/camDataMsg.h"
#include "car/environmentDataMsg.h"
-#include "car/ussDataMsg.h"
PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
namespace car
{
-Environment::Environment(ros::NodeHandle &nh, std::string &name) :
- nh_(nh)
- , name_(name)
- , distance(0)
- , relativeSpeed(0)
- {}
-
-Environment::Environment() :
- distance(0)
- , relativeSpeed(0)
- {}
-
-Environment::~Environment() {}
-
+
+Environment::Environment()
+ : messageOStream(nh)
+ , distance(0)
+ , relativeSpeed(0)
+{}
+
void Environment::onInit()
{
- NODELET_INFO("Environment::onInit -- START");
- environmentData = nh_.advertise<environmentDataMsg>("environmentData", 1);
- ussData = nh_.subscribe("ussData", 1, &Environment::ussDataCallback, this);
- camData = nh_.subscribe("camData", 1, &Environment::camDataCallback, this);
- NODELET_INFO("Environment::onInit -- END");
+ messageOStream << "Environment::onInit -- 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;
}
-void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
+void Environment::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
{
- // std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
distance = inMsg->distance;
relativeSpeed = 1.0;
-
+
environmentDataMsg outMsg;
outMsg.distance = distance;
outMsg.relativeSpeed = relativeSpeed;
-
+
environmentData.publish(outMsg);
}
-void Environment::camDataCallback(const camDataMsg::ConstPtr& inMsg)
+void Environment::camDataCallback(const camDataMsg::ConstPtr & inMsg)
{
- // std::cout << "Environment 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 c6ab2a44..1e634f17 100644
--- a/modules/catkin_ws/src/car/src/logging/Logging.cpp
+++ b/modules/catkin_ws/src/car/src/logging/Logging.cpp
@@ -4,7 +4,6 @@
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
-
#include "logging/Logging.h"
PLUGINLIB_EXPORT_CLASS(car::Logging, nodelet::Nodelet);
@@ -16,19 +15,9 @@ constexpr std::size_t Logging::PORT;
constexpr std::size_t Logging::MAX_MESSAGE_SIZE;
constexpr std::size_t Logging::MAX_BUFFER_SIZE;
-Logging::Logging(ros::NodeHandle & nh, std::string & name)
- : nh_(nh)
- , name_(name)
- , loggingBuffer(MAX_BUFFER_SIZE)
- , loggingStream(&loggingBuffer)
-{}
-
Logging::Logging()
: loggingBuffer(MAX_BUFFER_SIZE)
- , loggingStream(&loggingBuffer)
-{}
-
-Logging::~Logging()
+ , loggingOStream(loggingBuffer, loggingMutex)
{}
void Logging::onInit()
@@ -57,58 +46,49 @@ void Logging::onInit()
NODELET_INFO("Logging::onInit -- END");
}
-void Logging::logMessage(const std::string & msg)
-{
- std::lock_guard<std::mutex> lock{loggingMutex};
- loggingStream << msg << "\n";
-}
-
void Logging::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
{
- // std::cout << "Logging received new cc data ( ).\n";
+
}
void Logging::camDataCallback(const camDataMsg::ConstPtr & inMsg)
{
- // std::cout << "Logging received new cam data ( ).\n";
+
}
void Logging::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
{
- std::cout << "Logging received new environment data ( ).\n";
- logMessage(std::string{"Environment [distance]: "} + std::to_string(inMsg->distance));
+ loggingOStream << "environment [distance]: " << "\n";
}
void Logging::laneDataCallback(const laneDataMsg::ConstPtr & inMsg)
{
- // std::cout << "Logging received new lane data ( ).\n";
+
}
void Logging::logEnabledCallback(const logEnabledMsg::ConstPtr & inMsg)
{
- // std::cout << "Logging received new log enabled ( ).\n";
+
}
void Logging::logStringCallback(const logStringMsg::ConstPtr & inMsg)
{
- // std::cout << "Logging received new log string ( ).\n";
+ loggingOStream << "message: " << inMsg->logMsg << "\n";
}
void Logging::rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg)
{
- // std::cout << "Logging received new rc enabled ( ).\n";
+
}
void Logging::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
{
- // std::cout << "Logging received new stm data ( ).\n";
+
}
void Logging::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
{
- auto distance = std::to_string(inMsg->distance);
- std::cout << "Logging received new uss data ( distance: " << distance << " ).\n";
- logMessage(std::string{"uss [distance]: "} + distance);
+ loggingOStream << "uss [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 49dfdb02..689073a1 100644
--- a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -1,80 +1,95 @@
#include "mainNode/CruiseController.h"
-#include "PlatoonProtocolLib/VehicleFacade.h"
-#include "PlatoonProtocolLib/Protocol.h"
-
#include "car/ccDataMsg.h"
-#include <iostream>
-#include <algorithm>
-
-namespace car {
+namespace car
+{
-CruiseController::CruiseController(platoonProtocol::VehicleFacade& c2c,
- EgoMotion& egoMotion,
- PlatoonController& platoonController,
- ros::NodeHandle& nh)
- : c2c(c2c)
+CruiseController::CruiseController(ros::NodeHandle & nh,
+ platoonProtocol::VehicleFacade & c2c,
+ EgoMotion & egoMotion,
+ PlatoonController & platoonController)
+ : messageOStream(nh)
+ , c2c(c2c)
, egoMotion(egoMotion)
, platoonController(platoonController)
- {
- ccData = nh.advertise<ccDataMsg>("ccData", 1);
- }
+{
+ ccData = nh.advertise<ccDataMsg>("ccData", 1);
+}
-void CruiseController::run() {
- switch (platoonController.getCurrState()) {
- case PlatoonState::ACC: {
+void CruiseController::run()
+{
+ switch (platoonController.getCurrState())
+ {
+ case PlatoonState::ACC:
+ {
run_ACC(platoonController.getDesSpeed());
break;
}
- case PlatoonState::CACC_LV: {
+ case PlatoonState::CACC_LV:
+ {
auto config = platoonController.getPlatoonConfig().get();
run_ACC(config.platoonSpeed);
c2c.setPlatoonSpeed(speed);
break;
}
- case PlatoonState::CACC_FV: { run_CACC(); break; }
+ case PlatoonState::CACC_FV:
+ {
+ run_CACC();
+ break;
+ }
}
}
-void CruiseController::run_ACC(float target_speed) {
-
- std::cout << "CruiseController::run_ACC("<< target_speed <<")" << std::endl;
+void CruiseController::run_ACC(float target_speed)
+{
+ messageOStream << "CruiseController::run_ACC(" << target_speed << ")" << MessageOStream::flush;
float v_self = egoMotion.getOwnSpeed();
float d = egoMotion.getDistance();
float crash_dist = crash_time * v_self;
-
- if ( d < crash_dist ) {
- std::cout << "in crash_dist" << std::endl;
- speed = target_speed*(d - stop_dist)/crash_dist;
- } else {
- std::cout << "not in crash_dist" << std::endl;
+
+ if (d < crash_dist)
+ {
+ messageOStream << "in crash_dist" << MessageOStream::flush;
+ speed = target_speed * (d - stop_dist) / crash_dist;
+ }
+ else
+ {
+ messageOStream << "not in crash_dist" << MessageOStream::flush;
speed = target_speed;
}
-
+
speed = std::min(std::max(speed, min_speed), max_speed);
-
+
ccDataMsg outMsg;
outMsg.speed = speed;
ccData.publish(outMsg);
}
-void CruiseController::run_CACC() {
-
- std::cout << "CruiseController::run_CACC" << std::endl;
+void CruiseController::run_CACC()
+{
+ messageOStream << "CruiseController::run_CACC" << MessageOStream::flush;
auto config = platoonController.getPlatoonConfig().get();
- float PS = config.platoonSpeed;
+ float PS = config.platoonSpeed;
float IPD = config.innerPlatoonDistance;
- float vp = egoMotion.getPrvSpeed();
- float d = egoMotion.getDistance();
+ float vp = egoMotion.getPrvSpeed();
+ float d = egoMotion.getDistance();
- if ( IPD < 1 ) { IPD = 1; }
- if ( vp < eps && vp > -eps ) {
- if ( d < IPD ) { speed = 0; return; }
- speed = units_conv_fac*0.5*(d-IPD)/IPD; // THIS IS DEPENDEND ON THE UNITS OF speed AND d!!!
- } else {
- speed = pow(d/IPD,dist_pow) * pow(PS/vp,speed_pow) * vp;
+ if (IPD < 1)
+ { IPD = 1; }
+ if (vp < eps && vp > -eps)
+ {
+ if (d < IPD)
+ {
+ speed = 0;
+ return;
+ }
+ speed = units_conv_fac * 0.5 * (d - IPD) / IPD; // THIS IS DEPENDEND ON THE UNITS OF speed AND d!!!
+ }
+ else
+ {
+ speed = pow(d / IPD, dist_pow) * pow(PS / vp, speed_pow) * vp;
}
speed = std::min(std::max(speed, min_speed), max_speed);
diff --git a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
index c5d34c90..301a4646 100644
--- a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
@@ -1,10 +1,12 @@
#include "mainNode/EgoMotion.h"
-namespace car {
-
+namespace car
+{
+
EgoMotion::EgoMotion(ros::NodeHandle nh)
- : platoonControllerNotify()
+ : messageOStream(nh)
+ , platoonControllerNotify()
, cruiseControllerNotify()
{
environmentData = nh.subscribe("environmentData", 1, &EgoMotion::environmentDataCallback, this);
@@ -13,20 +15,23 @@ EgoMotion::EgoMotion(ros::NodeHandle nh)
void EgoMotion::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
{
- std::cout << "EgoMotion recived new environmentData data (" << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << std::endl;
+ messageOStream << "EgoMotion recived new environmentData data ("
+ << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << MessageOStream::flush;
distance = inMsg->distance;
relSpeed = inMsg->relativeSpeed;
computeAndNotify();
- std::cout << "EgoMotion done with environmentData data (" << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << std::endl;
+ messageOStream << "EgoMotion done with environmentData data ("
+ << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << MessageOStream::flush;
}
-
void EgoMotion::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
{
- std::cout << "EgoMotion recived new stmData data (" << inMsg->speed << ", " << inMsg->angle << ")." << std::endl;
+ messageOStream << "EgoMotion recived new stmData data ("
+ << inMsg->speed << ", " << inMsg->angle << ")." << MessageOStream::flush;
stmSpeed = inMsg->speed;
computeAndNotify();
- std::cout << "EgoMotion done with stmData data (" << inMsg->speed << ", " << inMsg->angle << ")." << std::endl;
+ messageOStream << "EgoMotion done with stmData data ("
+ << inMsg->speed << ", " << inMsg->angle << ")." << MessageOStream::flush;
}
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 452ded78..d50083e2 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -19,94 +19,70 @@ namespace car
{
platoonProtocol::NetworkInfo readNetworkInfo() {
- /** This reads ~/.CarConfig/platoon.config */
-
- // open config file
- std::string userHome = getenv("HOME");
- std::ifstream configFile;
- configFile.open(userHome + "/CarConfig/platoon.config", std::ifstream::in);
- if (!configFile.is_open()) { throw FileNotFound(); }
-
- // desired parameters
- platoonProtocol::VehicleId vehicleId;
- bool foundVehicleId = false;
- std::string broadCastAdress;
- bool foundBroadCastAdress = false;
-
- std::string contentLine;
- while (!configFile.eof()) {
+ /** This reads ~/.CarConfig/platoon.config */
+
+ // open config file
+ std::string userHome = getenv("HOME");
+ std::ifstream configFile;
+ configFile.open(userHome + "/CarConfig/platoon.config", std::ifstream::in);
+ if (!configFile.is_open()) { throw FileNotFound(); }
+
+ // desired parameters
+ platoonProtocol::VehicleId vehicleId;
+ bool foundVehicleId = false;
+ std::string broadCastAdress;
+ bool foundBroadCastAdress = false;
+
+ std::string contentLine;
+ while (!configFile.eof()) {
std::getline(configFile, contentLine);
-
+
// split this line
std::istringstream iss(contentLine);
std::vector<std::string> words {std::istream_iterator<std::string>(iss), {} };
-
+
if ( words.size() > 1) { // this line contains parameter[at(0)] and value[at(1)]
- if ( words.at(0) == "VehicleId:" ) {
- vehicleId = std::stoi(words.at(1));
- foundVehicleId = true;
- } else if ( words.at(0) == "BroadCast:" ) {
- broadCastAdress = words.at(1);
- foundBroadCastAdress = true;
- }
+ if ( words.at(0) == "VehicleId:" ) {
+ vehicleId = std::stoi(words.at(1));
+ foundVehicleId = true;
+ } else if ( words.at(0) == "BroadCast:" ) {
+ broadCastAdress = words.at(1);
+ foundBroadCastAdress = true;
+ }
}
-
- }
-
- if ( !(foundVehicleId && foundBroadCastAdress) ) { throw MissingParameters(); }
- configFile.close();
- return platoonProtocol::NetworkInfo{vehicleId, broadCastAdress};
+ }
-}
-
-MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
- nh(nh)
- , name(name)
- , c2cNet()
- , c2c(platoonProtocol::Vehicle::Role::FOLLOWER, c2cNet, readNetworkInfo())
- , pcNet()
- , pc(pc2car::CommandReceiver::create(pcNet))
- , egoMotion(nh)
- , platoonController(c2c, pc, egoMotion)
- , platoonControllerThread([this] { platoonController.run(); })
- , cruiseController(c2c, egoMotion, platoonController, nh)
- , cruiseControllerThread([this] { cruiseController.run(); })
-{
- egoMotion.cruiseControllerNotify = cruiseControllerNotify;
- egoMotion.platoonControllerNotify = platoonControllerNotify;
- platoonController.cruiseControllerNotify = cruiseControllerNotify;
- pc->receiveCommands([this] (auto commandCode) {platoonControllerThread.notify();} );
- c2c.setCallback( [this] {platoonControllerThread.notify();} );
+ if ( !(foundVehicleId && foundBroadCastAdress) ) { throw MissingParameters(); }
+ configFile.close();
+
+ return platoonProtocol::NetworkInfo{vehicleId, broadCastAdress};
}
-MainNode::MainNode() :
- nh()
- , name()
- , c2cNet()
- , c2c(platoonProtocol::Vehicle::Role::FOLLOWER, c2cNet, readNetworkInfo())
- , pcNet()
- , pc(pc2car::CommandReceiver::create(pcNet))
- , egoMotion(nh)
- , platoonController(c2c, pc, egoMotion)
- , platoonControllerThread([this] { platoonController.run(); })
- , cruiseController(c2c, egoMotion, platoonController, nh)
- , cruiseControllerThread([this] { cruiseController.run(); })
+MainNode::MainNode()
+ : nh()
+ , messageOStream(nh)
+ , c2cNet()
+ , c2c(platoonProtocol::Vehicle::Role::FOLLOWER, c2cNet, readNetworkInfo())
+ , pcNet()
+ , pc(pc2car::CommandReceiver::create(pcNet))
+ , egoMotion(nh)
+ , platoonController(nh, c2c, pc, egoMotion)
+ , platoonControllerThread([this] { platoonController.run(); })
+ , cruiseController(nh, c2c, egoMotion, platoonController)
+ , cruiseControllerThread([this] { cruiseController.run(); })
{
- egoMotion.cruiseControllerNotify = cruiseControllerNotify;
- egoMotion.platoonControllerNotify = platoonControllerNotify;
- platoonController.cruiseControllerNotify = cruiseControllerNotify;
- pc->receiveCommands([this] (auto commandCode) {platoonControllerThread.notify();} );
- c2c.setCallback( [this] {platoonControllerThread.notify();} );
+ egoMotion.cruiseControllerNotify = cruiseControllerNotify;
+ egoMotion.platoonControllerNotify = platoonControllerNotify;
+ platoonController.cruiseControllerNotify = cruiseControllerNotify;
+ pc->receiveCommands([this] (auto commandCode) {platoonControllerThread.notify();} );
+ c2c.setCallback( [this] {platoonControllerThread.notify();} );
}
-MainNode::~MainNode()
-{}
-
void MainNode::onInit()
{
- NODELET_INFO("MainNode::onInit -- START");
- NODELET_INFO("MainNode::onInit -- END");
+ messageOStream << "MainNode::onInit -- START";
+ messageOStream << "MainNode::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 b2d853d7..eaf6d79a 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -3,18 +3,18 @@
//
#include "../../include/mainNode/PlatoonController.h"
-#include "PlatoonProtocolLib/VehicleFacade.h"
-#include "NetworkingLib/TimedValue.h"
-#include <iostream>
namespace car
{
-PlatoonController::PlatoonController(platoonProtocol::VehicleFacade & c2c,
- pc2car::CommandReceiver::Ptr pc, EgoMotion & egoMotion)
- : cruiseControllerNotify()
+PlatoonController::PlatoonController(ros::NodeHandle & nh,
+ platoonProtocol::VehicleFacade & c2c,
+ pc2car::CommandReceiver::Ptr pc,
+ EgoMotion & egoMotion)
+ : messageOStream(nh)
+ , cruiseControllerNotify()
, c2c(c2c)
- , pc(pc)
+ , pc(std::move(pc))
, egoMotion(egoMotion)
, platoonConfig(c2c.getPlatoonConfig())
{}
@@ -67,11 +67,24 @@ void PlatoonController::setupC2C()
void PlatoonController::run()
{
- std::cout << "PlatoonController was run." << std::endl;
- switch (currState.load()) {
- case PlatoonState::ACC: { run_ACC(); break; }
- case PlatoonState::CACC_FV: { run_CACC_FV(); break; }
- case PlatoonState::CACC_LV: { run_CACC_LV(); break; }
+ messageOStream << "PlatoonController was run." << MessageOStream::flush;
+ switch (currState.load())
+ {
+ case PlatoonState::ACC:
+ {
+ run_ACC();
+ break;
+ }
+ case PlatoonState::CACC_FV:
+ {
+ run_CACC_FV();
+ break;
+ }
+ case PlatoonState::CACC_LV:
+ {
+ run_CACC_LV();
+ break;
+ }
}
}
@@ -79,33 +92,43 @@ void PlatoonController::run_ACC()
{
bool wantsPlatoon = pc->isPlatoonEnabled().get();
- if (!wantsPlatoon) {
- if (c2cAlive) { c2c.leavePlatoon(); c2cAlive = false;}
+ if (!wantsPlatoon)
+ {
+ if (c2cAlive)
+ {
+ c2c.leavePlatoon();
+ c2cAlive = false;
+ }
updateDesSpeed();
return;
- }
-
+ }
+
// wantsPlatoon
- if (!c2cAlive) {
+ if (!c2cAlive)
+ {
updateDesSpeed();
setupC2C();
return;
}
// c2cAlive + wantsPlatoon
- if (c2c.isPlatoonRunning()) {
- if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {
+ if (c2c.isPlatoonRunning())
+ {
+ if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER)
+ {
updatePcConfig();
- currState = PlatoonState::CACC_LV;
+ currState = PlatoonState::CACC_LV;
}
- else {currState = PlatoonState::CACC_FV;}
+ else
+ { currState = PlatoonState::CACC_FV; }
return;
}
// !inPlatoon + c2cAlive + wantsPlatoon
bool isLeader = c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER;
bool hasFiniteDistance = egoMotion.getDistance() < std::numeric_limits<float>::infinity();
- if ( isLeader != hasFiniteDistance ) { // role does not fit
+ if (isLeader != hasFiniteDistance)
+ { // role does not fit
c2c.leavePlatoon();
c2cAlive = false;
updateDesSpeed();
@@ -114,7 +137,8 @@ void PlatoonController::run_ACC()
}
// role fits + !inPlatoon + c2cAlive + wantsPlatoon
- if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {updatePcConfig();}
+ if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER)
+ { updatePcConfig(); }
updateDesSpeed();
}
@@ -127,13 +151,18 @@ void PlatoonController::run_CACC_FV()
// needs to be updated, so the new value can be pulled be CC
platoonConfig = c2c.getPlatoonConfig(); // TODO <- updateC2CConfig();
- std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
- << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+ messageOStream << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
+ << ", wantsPlatoon = " << wantsPlatoon << MessageOStream::flush;
+
+ if (inPlatoon && wantsPlatoon)
+ {
+ cruiseControllerNotify();
+ return;
+ }
+
+ if (inPlatoon && !wantsPlatoon)
+ { c2c.leavePlatoon(); }
- if (inPlatoon && wantsPlatoon) { cruiseControllerNotify(); return; }
-
- if (inPlatoon && !wantsPlatoon) { c2c.leavePlatoon(); }
-
c2cAlive = false;
currState = PlatoonState::ACC;
cruiseControllerNotify();
@@ -145,16 +174,18 @@ void PlatoonController::run_CACC_LV()
bool inPlatoon = c2c.isPlatoonRunning();
bool wantsPlatoon = pc->isPlatoonEnabled().get();
- std::cout << "Running PlatoonController::run_CACC_LV: inPlatoon = " << inPlatoon
- << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+ messageOStream << "Running PlatoonController::run_CACC_LV: inPlatoon = " << inPlatoon
+ << ", wantsPlatoon = " << wantsPlatoon << MessageOStream::flush;
- if (inPlatoon && wantsPlatoon) {
+ if (inPlatoon && wantsPlatoon)
+ {
updatePcConfig();
c2c.setInnerPlatoonDistance(IPD.get());
return;
- }
-
- if (inPlatoon && !wantsPlatoon) {
+ }
+
+ if (inPlatoon && !wantsPlatoon)
+ {
c2c.leavePlatoon();
}
diff --git a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
index a38efed0..6b362421 100644
--- a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
+++ b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
@@ -4,32 +4,24 @@
#include "mavLink/MavLink.h"
#include "car/stmDataMsg.h"
-#include "car/ccDataMsg.h"
-#include "car/laneDataMsg.h"
-#include "car/rcEnabledMsg.h"
PLUGINLIB_EXPORT_CLASS(car::MavLink, nodelet::Nodelet);
namespace car
{
-MavLink::MavLink(ros::NodeHandle & nh, std::string & name) :
- nh_(nh), name_(name)
-{}
MavLink::MavLink()
-{}
-
-MavLink::~MavLink()
+ : messageOStream(nh)
{}
void MavLink::onInit()
{
- NODELET_INFO("MavLink::onInit -- START");
+ messageOStream << "MavLink::onInit -- START" << MessageOStream::flush;
- stmData = nh_.advertise<stmDataMsg>("stmData", 1);
- ccData = nh_.subscribe("ccData", 1, &MavLink::ccDataCallback, this);
- laneData = nh_.subscribe("laneData", 1, &MavLink::laneDataCallback, this);
- rcEnabled = nh_.subscribe("rcEnabled", 1, &MavLink::rcEnabledCallback, this);
+ stmData = nh.advertise<stmDataMsg>("stmData", 1);
+ ccData = nh.subscribe("ccData", 1, &MavLink::ccDataCallback, this);
+ laneData = nh.subscribe("laneData", 1, &MavLink::laneDataCallback, this);
+ rcEnabled = nh.subscribe("rcEnabled", 1, &MavLink::rcEnabledCallback, this);
veloxConnection = veloxProtocol::Connection::create(net);
veloxConnection->open(
@@ -37,9 +29,9 @@ void MavLink::onInit()
[this]
{ onStmDataReceived(); },
[this]
- { NODELET_INFO("Connection to STM closed!\n"); });
+ { messageOStream << "UART was closed!" << MessageOStream::flush; });
- NODELET_INFO("MavLink::onInit -- END");
+ messageOStream << "MavLink::onInit -- END" << MessageOStream::flush;
}
void MavLink::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
@@ -49,12 +41,10 @@ void MavLink::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
void MavLink::laneDataCallback(const laneDataMsg::ConstPtr & inMsg)
{
- std::cout << "MavLink recived new lane data ( ).\n";
}
void MavLink::rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg)
{
- std::cout << "MavLink recived new rc status (" << inMsg->enabled << ").\n";
}
void MavLink::onStmDataReceived()
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/StreamMedianFilter.cpp b/modules/catkin_ws/src/car/src/ultrasonic/StreamMedianFilter.cpp
index f9771d58..b4d7ab23 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/StreamMedianFilter.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/StreamMedianFilter.cpp
@@ -4,7 +4,7 @@
namespace car
{
-StreamMedianFilter::StreamMedianFilter(const int windowSize)
+StreamMedianFilter::StreamMedianFilter(int windowSize)
: currentWindow()
, sortedIndexList(windowSize, 0)
, currentIndex(0)
@@ -13,8 +13,6 @@ StreamMedianFilter::StreamMedianFilter(const int windowSize)
std::iota(sortedIndexList.begin(), sortedIndexList.end(), 0);
}
-StreamMedianFilter::~StreamMedianFilter() {}
-
int StreamMedianFilter::moveWindow(int nextValue)
{
if (currentWindow.size() < currentWindow.capacity()) currentWindow.push_back(nextValue);
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 f153f9e5..bac34fe8 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
@@ -5,55 +5,28 @@
#include <unistd.h>
-//gpio pins where the sonar is connected
-const int SRF02_SDA = 8; //i2c data pin ;
-const int SRF02_SCL = 9; //i2c clock pin ;
-
-// addresses of the sonars, the number is also as a sticker on the devices themselves
-const int DEVICE_ADDRESS1 = 0x74;
-const int DEVICE_ADDRESS2 = 0x76;
-const int DEVICE_ADDRESS3 = 0x77;
-
-//path to i2c file
-const char * DEVICE = "/dev/i2c-1";
-
-const int COMMAND_REGISTER = 0x00;
-const int RESULT_HIGH_BYTE = 0x02;
-const int RESULT_LOW_BYTE = 0x03;
-const int RANGING_MODE_CM = 0x51;
-
-const int DELAY = 70; //70 ms for ranging to finish
+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)
{
- this->fd = -1; //no file opened yet
- this->devId = devId;
- setup();
+ //fd = wiringPiI2CSetupInterface(DEVICE, devId);
+ //if (fd == -1)
+ //throw std::runtime_error{"Device not found!\n"};
}
int USS_SRF02::getDistance()
{
- int distance;
-
- if (fd == -1)
- {
- USS_SRF02::setup();
- }
-
- wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
+ //wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
usleep(DELAY * 1000);
- distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
-
- return distance;
-}
-
-void USS_SRF02::setup()
-{
-
- fd = wiringPiI2CSetupInterface(DEVICE, this->devId);
-
- //todo error handling,
- //however wiringPiI2CSetupInterface() calls exit() if it fails
-
-
+ //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 9bf5d39f..2109e5a1 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
@@ -10,20 +10,18 @@ PLUGINLIB_EXPORT_CLASS(car::Ultrasonic, nodelet::Nodelet
namespace car
{
-Ultrasonic::Ultrasonic(ros::NodeHandle & nh, std::string & name) : USS_SRF02(0x74), nh_(nh), name_(name)
-{}
-
-Ultrasonic::Ultrasonic() : USS_SRF02(0x74)
-{}
-Ultrasonic::~Ultrasonic()
+Ultrasonic::Ultrasonic()
+ : USS_SRF02(DEVICE_ADDRESS1)
+ , messageOStream(nh)
{}
void Ultrasonic::onInit()
{
- NODELET_INFO("Ultrasonic::onInit -- START");
- ussData = nh_.advertise<ussDataMsg>("ussData", 1);
- main = std::thread(
+ messageOStream << "Ultrasonic::onInit -- START" << MessageOStream::flush;
+
+ ussData = nh.advertise<ussDataMsg>("ussData", 1);
+ sensorThread = std::thread(
[this]
{
for (;;)
@@ -35,6 +33,6 @@ void Ultrasonic::onInit()
}
});
- NODELET_INFO("Ultrasonic::onInit -- END");
+ messageOStream << "Ultrasonic::onInit -- END" << MessageOStream::flush;
}
}
--
GitLab