diff --git a/app/other/behaviour_tree_car/include/behaviour_tree/node/custom/condition/SuccessOnAverageNearbyScan.hpp b/app/other/behaviour_tree_car/include/behaviour_tree/node/custom/condition/SuccessOnAverageNearbyScan.hpp index 96d8de37..fefe2c1b 100644 --- a/app/other/behaviour_tree_car/include/behaviour_tree/node/custom/condition/SuccessOnAverageNearbyScan.hpp +++ b/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; diff --git a/app/rpi/common/include/behaviour_tree/BehaviourTreeHandler.hpp b/app/rpi/common/include/behaviour_tree/BehaviourTreeHandler.hpp index 5fad8888..a229988b 100644 --- a/app/rpi/common/include/behaviour_tree/BehaviourTreeHandler.hpp +++ b/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 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 behaviour_tree; - + std::shared_ptr car_system; + + std::shared_ptr behaviour_tree; std::shared_ptr context; int tick_count = 0; - - std::shared_ptr car_system; + + // This is initialized as 0 + std::chrono::time_point last_connected; }; } // namespace behaviour_tree diff --git a/app/rpi/common/include/car/configuration/Configuration.h b/app/rpi/common/include/car/configuration/Configuration.h index 573946b9..55da27ae 100644 --- a/app/rpi/common/include/car/configuration/Configuration.h +++ b/app/rpi/common/include/car/configuration/Configuration.h @@ -3,6 +3,7 @@ #pragma once +#include #include #include @@ -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); }; }; diff --git a/app/rpi/common/src/car/system/movement/controller/DeviceMovementController.cpp b/app/rpi/common/src/car/system/movement/controller/DeviceMovementController.cpp index e9c9e6cd..cac26888 100644 --- a/app/rpi/common/src/car/system/movement/controller/DeviceMovementController.cpp +++ b/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 diff --git a/app/rpi/daemon/src/main.cpp b/app/rpi/daemon/src/main.cpp index 43d17b33..cdd3f360 100644 --- a/app/rpi/daemon/src/main.cpp +++ b/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 = std::make_shared(Configuration{}); @@ -135,7 +135,7 @@ class rpi_daemon : public daemon } const std::chrono::time_point 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 last_connected; diff --git a/app/rpi/daemon/systemd/rpi_daemon.conf b/app/rpi/daemon/systemd/rpi_daemon.conf index 1c9b89eb..e5415278 100644 --- a/app/rpi/daemon/systemd/rpi_daemon.conf +++ b/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= diff --git a/app/rpi/tui/settings/config.jsonc b/app/rpi/tui/settings/config.jsonc index 84c42caa..7c0e40cd 100644 --- a/app/rpi/tui/settings/config.jsonc +++ b/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 } \ No newline at end of file diff --git a/app/rpi/tui/src/car/configuration/JsonConfiguration.cxx b/app/rpi/tui/src/car/configuration/JsonConfiguration.cxx index 06d0a848..70f6640e 100644 --- a/app/rpi/tui/src/car/configuration/JsonConfiguration.cxx +++ b/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)