Add behaviour_tree_update_ms_interval to slow down updates to behavio…

…ur tree

app/other/behaviour_tree_car/include/behaviour_tree/node/custom/condition/SuccessOnAverageNearbyScan.hpp

@@ -44,6 +44,7 @@ namespace behaviour_tree::node::custom::condition
 			if (angles_between_count > 0)
 			{
 				double average_distance_unit = total_distance / angles_between_count;
+                spdlog::info("Average Distance: {}; Average Distance Unit: {}", average_distance_unit, this->getAverageDistanceUnit());
 				if (average_distance_unit < this->getAverageDistanceUnit())
 				{
 					return Status::Success;

app/rpi/common/include/behaviour_tree/BehaviourTreeHandler.hpp

@@ -122,8 +122,13 @@ namespace behaviour_tree
 			}
 			if (this->context->canRun())
 			{
-				this->context->update(this->tick_count);
-				this->tick_count++;
+                const std::chrono::time_point<std::chrono::steady_clock> now = std::chrono::steady_clock::now();
+                // TODO:
+                if (this->last_connected >= this->car_system->getConfiguration()->behaviour_tree_update_ms_interval) {
+                    this->context->update(this->tick_count);
+                    this->tick_count++;
+                    this->last_connected = now;
+                }
 			}
 			else
 			{
@@ -147,13 +152,15 @@ namespace behaviour_tree
 		}
 
 	private:
-		std::shared_ptr<BehaviourTree> behaviour_tree;
-
+		std::shared_ptr<car::system::CarSystem> car_system;
+
+        std::shared_ptr<BehaviourTree> behaviour_tree;
 		std::shared_ptr<Context> context;
 
 		int tick_count = 0;
-
-		std::shared_ptr<car::system::CarSystem> car_system;
+
+        // This is initialized as 0
+        std::chrono::time_point<std::chrono::steady_clock> last_connected;
 	};
 } // namespace behaviour_tree
 

app/rpi/common/include/car/configuration/Configuration.h

@@ -3,6 +3,7 @@
 
 #pragma once
 
+#include <chrono>
 #include <optional>
 #include <string>
 
@@ -19,6 +20,8 @@ namespace car::configuration
 
 		std::string lidar_port = "";
 		bool use_lidar = true;
+
+        std::chrono::milliseconds behaviour_tree_update_ms_interval = std::chrono::milliseconds(100);
 	};
 };
 

app/rpi/common/src/car/system/movement/controller/DeviceMovementController.cpp

@@ -66,76 +66,76 @@ namespace car::system::movement::controller
 
     void DeviceMovementController::setRearWheelsSpeed(const int speed)
     {
-        spdlog::info("Both Rear Wheels speed are set to {}", speed);
+        // spdlog::info("Both Rear Wheels speed are set to {}", speed);
         this->rear_left_wheel_->setSpeed(speed);
         this->rear_right_wheel_->setSpeed(speed);
     }
 
     void DeviceMovementController::setRearLeftWheelSpeed(const int speed)
     {
-        spdlog::info("Left Rear Wheel speed is set to {}", speed);
+        // spdlog::info("Left Rear Wheel speed is set to {}", speed);
         this->rear_left_wheel_->setSpeed(speed);
     }
 
     void DeviceMovementController::setRearRightWheelSpeed(const int speed)
     {
-        spdlog::info("Right Rear Wheel speed is set to {}", speed);
+        // spdlog::info("Right Rear Wheel speed is set to {}", speed);
         this->rear_right_wheel_->setSpeed(speed);
     }
 
     void DeviceMovementController::setFrontWheelsAngle(const float angle)
     {
-        spdlog::info("Front Wheels angle is set to {}", angle);
+        // spdlog::info("Front Wheels angle is set to {}", angle);
         this->front_wheels_->setAngle(angle);
     }
 
     void DeviceMovementController::setCameraServo1Angle(const float angle)
     {
-        spdlog::info("Camera Servo 1 angle is set to {}", angle);
+        // spdlog::info("Camera Servo 1 angle is set to {}", angle);
         this->camera_servo_1_->setAngle(angle);
     }
 
     void DeviceMovementController::setCameraServo2Angle(const float angle)
     {
-        spdlog::info("Camera Servo 2 angle is set to {}", angle);
+        // spdlog::info("Camera Servo 2 angle is set to {}", angle);
         this->camera_servo_2_->setAngle(angle);
     }
 
     void DeviceMovementController::setRearWheelsDirectionToForward()
     {
-        spdlog::info("Both Rear Wheels are set to move forward");
+        // spdlog::info("Both Rear Wheels are set to move forward");
         this->rear_left_wheel_->forward();
         this->rear_right_wheel_->forward();
     }
 
     void DeviceMovementController::setRearLeftWheelDirectionToForward()
     {
-        spdlog::info("Left Rear Wheel is set to move forward");
+        // spdlog::info("Left Rear Wheel is set to move forward");
         this->rear_left_wheel_->forward();
     }
 
     void DeviceMovementController::setRearRightWheelDirectionToForward()
     {
-        spdlog::info("Right Rear Wheel is set to move forward");
+        // spdlog::info("Right Rear Wheel is set to move forward");
         this->rear_right_wheel_->forward();
     }
 
     void DeviceMovementController::setRearWheelsDirectionToBackward()
     {
-        spdlog::info("Both Rear Wheels are set to move backward");
+        // spdlog::info("Both Rear Wheels are set to move backward");
         this->rear_left_wheel_->backward();
         this->rear_right_wheel_->backward();
     }
 
     void DeviceMovementController::setRearLeftWheelDirectionToBackward()
     {
-        spdlog::info("Left Rear Wheel is set to move backward");
+        // spdlog::info("Left Rear Wheel is set to move backward");
         this->rear_left_wheel_->backward();
     }
 
     void DeviceMovementController::setRearRightWheelDirectionToBackward()
     {
-        spdlog::info("Right Rear Wheel is set to move backward");
+        // spdlog::info("Right Rear Wheel is set to move backward");
         this->rear_right_wheel_->backward();
     }
 } // namespace car::system::movement::controller

app/rpi/daemon/src/main.cpp

@@ -49,7 +49,7 @@ class rpi_daemon : public daemon
             return;
         }
 
-        this->connection_interval = std::chrono::seconds(reader.GetUnsigned("RaspberryPi", "connection_interval", 1));
+        this->connection_ms_interval = std::chrono::milliseconds(reader.GetUnsigned("RaspberryPi", "connection_ms_interval", 1000));
 
         std::shared_ptr<Configuration> configuration = std::make_shared<Configuration>(Configuration{});
 
@@ -135,7 +135,7 @@ class rpi_daemon : public daemon
         }
 
         const std::chrono::time_point<std::chrono::steady_clock> now = std::chrono::steady_clock::now();
-        const bool CAN_CONNECT = !this->car_system->getMessagingSystem()->isConnected() && now - this->last_connected >= this->connection_interval;
+        const bool CAN_CONNECT = !this->car_system->getMessagingSystem()->isConnected() && now - this->last_connected >= this->connection_ms_interval;
         if (CAN_CONNECT)
         {
             this->connect(now);
@@ -147,7 +147,7 @@ class rpi_daemon : public daemon
     {
         if (!this->attempted_to_reconnect)
         {
-            dlog::notice(fmt::format(R"(Going to repeatedly attempt to connect to the WS Server "{}" at {} second intervals.)", this->car_system->getConfiguration()->host, this->connection_interval.count()));
+            dlog::notice(fmt::format(R"(Going to repeatedly attempt to connect to the WS Server "{}" at {} second intervals.)", this->car_system->getConfiguration()->host, this->connection_ms_interval.count()));
         }
         auto connection_result = this->car_system->tryConnect();
         if (!connection_result.has_value())
@@ -227,7 +227,7 @@ class rpi_daemon : public daemon
     // To print out reconnect message once
     bool attempted_to_reconnect = false;
 
-    std::chrono::seconds connection_interval = std::chrono::seconds(1);
+    std::chrono::milliseconds connection_ms_interval = std::chrono::milliseconds(1000);
 
     // This is initialized as 0
     std::chrono::time_point<std::chrono::steady_clock> last_connected;

app/rpi/daemon/systemd/rpi_daemon.conf

@@ -5,7 +5,8 @@ description=@PROJECT_DESCRIPTION@
 
 ; Configuration file for the Raspberry Pi
 [RaspberryPi]
-connection_interval=3
+connection_ms_interval=3000
+behaviour_tree_update_ms_interval=100
 
 ; Local host configuration (Electron Websocket Server)
 host=

app/rpi/tui/settings/config.jsonc

@@ -1,4 +1,5 @@
 {
     "host": "127.0.0.1:3000",
-    "lidar_port": ""
+    "lidar_port": "",
+    "behaviour_tree_update_ms_interval": 1000
 }

app/rpi/tui/src/car/configuration/JsonConfiguration.cxx

@@ -68,6 +68,7 @@ namespace car::configuration
                     configuration.lidar_port = "COM3";
 #endif
                 }
+                configuration.behaviour_tree_update_ms_interval = config_json["behaviour_tree_update_ms_interval"].GetInt();
                 return configuration;
             }
             catch (const std::exception &e)