Fix CPU usage (#37)

* partial fixes?

* Format files

src/bsp/src/posix/include/BSP.h

@@ -1,6 +1,7 @@
 #ifndef BSP_H
 #define BSP_H
 
+#include "BaseTask.h"
 #include "bsp/IBSP.h"
 #include "ros/ros.h"
 
@@ -17,6 +18,7 @@ class BSP : public IBSP {
     std::shared_ptr<ros::NodeHandle> getNodeHandle();
 
   private:
+    BaseTask<2 * configMINIMAL_STACK_SIZE> m_rosWatchTask;
     ros::AsyncSpinner m_spinner;
     std::shared_ptr<ros::NodeHandle> m_rosNodeHandle;
     uint16_t m_UUID;

src/bsp/src/posix/src/BSP.cpp

@@ -1,16 +1,31 @@
 #include "BSP.h"
-#include "BaseTask.h"
+#include <Task.h>
 #include <thread>
 
 constexpr uint8_t gc_spinnerCores = 1;
 
+/**
+ * @brief Task that kills OS when ROS node is stopped
+ */
+void rosWatcher(void* param) {
+    (void)param;
+    const int loopRate = 100;
+
+    while (ros::ok()) {
+        ros::spinOnce();
+        Task::delay(loopRate);
+    }
+}
+
 BSP::~BSP() = default;
 
-BSP::BSP(const ros::NodeHandle& nodeHandle) : m_spinner(gc_spinnerCores) {
+BSP::BSP(const ros::NodeHandle& nodeHandle) :
+    m_rosWatchTask("ros_watch", tskIDLE_PRIORITY + 1, rosWatcher, NULL),
+    m_spinner(gc_spinnerCores) {
     m_rosNodeHandle = std::make_shared<ros::NodeHandle>(nodeHandle);
 }
 
-void BSP::initChip() { m_spinner.start(); }
+void BSP::initChip() { m_rosWatchTask.start(); }
 
 ChipInfo BSP::getChipInfo() const {
     return ChipInfo{.m_cores = (uint8_t)std::thread::hardware_concurrency(),

src/logger/src/LoggerContainer.cpp

@@ -2,6 +2,6 @@
 #include "bsp/Container.h"
 
 Logger& LoggerContainer::getLogger() {
-    static Logger s_logger = Logger(LogLevel::Info, BspContainer::getUserInterface());
+    static Logger s_logger = Logger(LogLevel::Warn, BspContainer::getUserInterface());
     return s_logger;
 }

src/message-handler/src/MessageSender.cpp

@@ -15,7 +15,7 @@ bool MessageSender::greet() {
 
 bool MessageSender::processAndSerialize() {
     if (m_inputQueue.isEmpty()) {
-        m_inputQueue.wait(500);
+        m_inputQueue.wait(1000);
     }
     const std::optional<std::reference_wrapper<const MessageDTO>> message = m_inputQueue.peek();
     if (message) {

src/network/src/ros/src/broker/main.cpp

@@ -5,7 +5,11 @@ int main(int argc, char** argv) {
     ros::init(argc, argv, "communication_broker");
 
     CommunicationBroker communicationBroker;
-    ros::spin();
+    ros::Rate r(10); // 10 hz
+    while (ros::ok()) {
+        ros::spinOnce();
+        r.sleep();
+    }
 
     return 0;
 };