diff --git a/images/Airplane.png b/images/Airplane.png new file mode 100644 index 0000000..59f32d9 Binary files /dev/null and b/images/Airplane.png differ diff --git a/images/Baseballbat.png b/images/Baseballbat.png new file mode 100644 index 0000000..b93d7ac Binary files /dev/null and b/images/Baseballbat.png differ diff --git a/images/Boat.png b/images/Boat.png new file mode 100644 index 0000000..7e58851 Binary files /dev/null and b/images/Boat.png differ diff --git a/images/Bus.png b/images/Bus.png new file mode 100644 index 0000000..3bda352 Binary files /dev/null and b/images/Bus.png differ diff --git a/images/Car.png b/images/Car.png new file mode 100644 index 0000000..92412d6 Binary files /dev/null and b/images/Car.png differ diff --git a/images/Stopsign.png b/images/Stopsign.png new file mode 100644 index 0000000..855e96d Binary files /dev/null and b/images/Stopsign.png differ diff --git a/images/Suitcase.png b/images/Suitcase.png new file mode 100644 index 0000000..b4864c1 Binary files /dev/null and b/images/Suitcase.png differ diff --git a/images/Tennisracket.png b/images/Tennisracket.png new file mode 100644 index 0000000..a314ba4 Binary files /dev/null and b/images/Tennisracket.png differ diff --git a/images/Umbrella.png b/images/Umbrella.png new file mode 100644 index 0000000..f6b2f41 Binary files /dev/null and b/images/Umbrella.png differ diff --git a/images/basketball.jpg b/images/basketball.jpg new file mode 100644 index 0000000..2fcba5c Binary files /dev/null and b/images/basketball.jpg differ diff --git a/include/camera/interface.hpp b/include/camera/interface.hpp index be80708..9f8a00f 100644 --- a/include/camera/interface.hpp +++ b/include/camera/interface.hpp @@ -4,11 +4,13 @@ #include #include #include +#include #include "utilities/base64.hpp" // Struct to hold the image struct ImageData { cv::Mat DATA; + std::string filename; }; // Utility function for converting an image loaded with OpenCV into base-64 diff --git a/include/camera/mock.hpp b/include/camera/mock.hpp index 86fe81c..cd81b5d 100644 --- a/include/camera/mock.hpp +++ b/include/camera/mock.hpp @@ -14,7 +14,7 @@ const int DUMMY_INTERVAL = 1000; const int DUMMY_TIMEOUT = 1000; const int IMAGE_COUNT = 5; -const std::string images_dir = "/workspaces/tests/images/"; +const std::string images_dir = "/workspaces/images/"; /** * diff --git a/include/core/mission_state.hpp b/include/core/mission_state.hpp index ef76bd2..edeca0d 100644 --- a/include/core/mission_state.hpp +++ b/include/core/mission_state.hpp @@ -18,9 +18,11 @@ class MissionState { bool is_prepared = false; int task_progress = 0; std::string current_tick_name; + std::string image_object; // Mutex to protect shared data between the main loop and the server thread std::mutex state_mut; + std::mutex image_mut; MissionState(); ~MissionState(); diff --git a/include/network/gcs_macros.hpp b/include/network/gcs_macros.hpp index 5ff837a..134274d 100644 --- a/include/network/gcs_macros.hpp +++ b/include/network/gcs_macros.hpp @@ -10,4 +10,4 @@ #define GCS_HANDLE(Method, uri) Method##_##uri #define DEF_GCS_HANDLE(Method, uri) void GCS_HANDLE(Method, uri) (GCS_HANDLER_PARAMS) -#endif // INCLUDE_NETWORK_GCS_MACROS_HPP_ \ No newline at end of file +#endif // INCLUDE_NETWORK_GCS_MACROS_HPP_ \ No newline at end of file diff --git a/include/network/gcs_routes.hpp b/include/network/gcs_routes.hpp index 836ae0d..d94c206 100644 --- a/include/network/gcs_routes.hpp +++ b/include/network/gcs_routes.hpp @@ -2,12 +2,17 @@ #define INCLUDE_NETWORK_GCS_ROUTES_HPP_ #include + #include + #include "core/mission_state.hpp" #include "network/gcs_macros.hpp" DEF_GCS_HANDLE(Get, status); + DEF_GCS_HANDLE(Get, tick); DEF_GCS_HANDLE(Get, capture); -#endif // INCLUDE_NETWORK_GCS_ROUTES_HPP_ \ No newline at end of file +DEF_GCS_HANDLE(Post, message); + +#endif // INCLUDE_NETWORK_GCS_ROUTES_HPP_ \ No newline at end of file diff --git a/protos/onboarding.proto b/protos/onboarding.proto index 05f6636..271a2f6 100644 --- a/protos/onboarding.proto +++ b/protos/onboarding.proto @@ -2,8 +2,34 @@ syntax = "proto3"; option go_package = "gcs-onboarding/internal/protos"; +enum ODLCObjects { + Undefined = 0; + Mannequin = 1; + Car = 2; + Motorcycle = 3; + Airplane = 4; + Bus = 5; + Boat = 6; + Stopsign = 7; + Snowboard = 8; + Umbrella = 9; + SoccerBall = 10; + Basketball = 11; + Volleyball = 12; + Football = 13; + Baseballbat = 14; + Mattress = 15; + Tennisracket = 16; + Suitcase = 17; + Skis = 18; +} + message OBCStatus { string current_tick_name = 1; bool is_connected = 2; double mission_progress_percent = 3; +} + +message DetectedObject { + ODLCObjects object = 1; } \ No newline at end of file diff --git a/src/camera/mock.cpp b/src/camera/mock.cpp index 77976a8..7b0f87b 100644 --- a/src/camera/mock.cpp +++ b/src/camera/mock.cpp @@ -22,8 +22,19 @@ std::optional MockCamera::takePicture(const std::chrono::milliseconds entries.push_back(entry); } - cv::Mat captured_image = cv::imread(entries[image_index].path().string()); - image_index++; // + // Debug: show how many entries we found + std::cout << "MockCamera: entries found = " << entries.size() << std::endl; + + if (entries.empty()) { + std::cout << "MockCamera: no entries in images_dir, returning empty image" << std::endl; + return {}; + } + + size_t idx = static_cast(image_index) % entries.size(); + std::string path_str = entries[idx].path().string(); + std::cout << "MockCamera: loading index " << idx << " from path: " << path_str << std::endl; + cv::Mat captured_image = cv::imread(path_str); + image_index++; auto now = std::chrono::steady_clock::now(); @@ -31,8 +42,14 @@ std::optional MockCamera::takePicture(const std::chrono::milliseconds return {}; } + if (captured_image.empty()) { + std::cout << "MockCamera: imread returned empty image for path: " << path_str << std::endl; + return {}; + } + ImageData image_data; image_data.DATA = captured_image; + image_data.filename = entries[idx].path().stem().string(); return image_data; } diff --git a/src/network/gcs.cpp b/src/network/gcs.cpp index 0c01cb3..7925462 100644 --- a/src/network/gcs.cpp +++ b/src/network/gcs.cpp @@ -1,10 +1,10 @@ #include "network/gcs.hpp" + #include + #include "network/gcs_routes.hpp" -GCSServer::GCSServer(uint16_t port, std::shared_ptr state) - : state{state} { - +GCSServer::GCSServer(uint16_t port, std::shared_ptr state) : state{state} { this->_bindHandlers(); this->server_thread = std::thread([this, port]() { @@ -24,5 +24,6 @@ void GCSServer::_bindHandlers() { // Use our macro to bind the GET /status route BIND_HANDLER(Get, status); BIND_HANDLER(Get, tick); + BIND_HANDLER(Post, message); BIND_HANDLER(Get, capture); } \ No newline at end of file diff --git a/src/network/gcs_routes.cpp b/src/network/gcs_routes.cpp index be50a8e..5bb15ac 100644 --- a/src/network/gcs_routes.cpp +++ b/src/network/gcs_routes.cpp @@ -1,7 +1,10 @@ #include "network/gcs_routes.hpp" -#include "core/mission_state.hpp" + #include +#include "core/mission_state.hpp" +#include "nlohmann/json.hpp" + // This needs to be included to get the definition of our proto message #include "onboarding.pb.h" @@ -34,12 +37,60 @@ DEF_GCS_HANDLE(Get, tick) { response.status = 200; } +DEF_GCS_HANDLE(Post, message) { + std::string received_message = request.body; + + DetectedObject detected_proto; + auto parse_status = google::protobuf::util::JsonStringToMessage(received_message, &detected_proto); + if (!parse_status.ok()) { + response.set_content("Invalid JSON for DetectedObject", "text/plain"); + response.status = 400; + return; + } + + const std::string detected_name = ODLCObjects_Name(detected_proto.object()); + + { + std::lock_guard lock(state->image_mut); + + // Compare detected object name to current image filename (stem) + if (state->image.has_value()) { + const std::string &filename = state->image->filename; + state->image_object = detected_name; + + std::cout << "Detected Objecct: " << detected_name << "\n Ground Truth:" << filename << std::endl; + if (!detected_name.empty() && detected_name == filename) { + state->image_state = MissionState::ImageState::VALID; + std::cout << "Message Accepted!" << std::endl; + response.set_content("Matched object is correct. Ending mission.", "application/json"); + response.status = 200; + } else { + state->image_state = MissionState::ImageState::INVALID; + std::cout << "Message Rejected" << std::endl; + response.set_content("Matched object is incorrect. Going to take another picture.", "application/json"); + response.status = 200; + } + } else { + state->image_state = MissionState::ImageState::INVALID; + } + } + +} + DEF_GCS_HANDLE(Get, capture) { + std::lock_guard lock(state->state_mut); std::optional img = state->image; + std::cout << "img.has_value() = " << img.has_value() << std::endl; + if (!img.has_value()) { + std::cout << "No image available" << std::endl; + response.set_content("No image available", "text/plain"); + response.status = 503; + return; + } std::string img_b64 = cvMatToBase64(img->DATA); state->has_captured = true; response.set_content(img_b64, "text/plain"); response.status = 200; -} \ No newline at end of file +} diff --git a/src/ticks/cvloiter.cpp b/src/ticks/cvloiter.cpp index 2017b40..d9a22a7 100644 --- a/src/ticks/cvloiter.cpp +++ b/src/ticks/cvloiter.cpp @@ -9,6 +9,7 @@ CVLoiterTick::CVLoiterTick(std::shared_ptr state) : Tick(state, TickID::CVLoiter) {} void CVLoiterTick::init() { + std::cout << "Initializing CVLoiter..." << std::endl; state->has_captured = false; } @@ -18,10 +19,10 @@ std::chrono::milliseconds CVLoiterTick::getWait() const { Tick* CVLoiterTick::tick() { - + std::cout << "CVLoiterTick: has_captured = " << state->has_captured << std::endl; if (state->has_captured) { return new SwitchTick(state); } else { - return new CVLoiterTick(state); + return nullptr; } } \ No newline at end of file diff --git a/src/ticks/verify.cpp b/src/ticks/verify.cpp index ff88324..57f1438 100644 --- a/src/ticks/verify.cpp +++ b/src/ticks/verify.cpp @@ -22,7 +22,7 @@ Tick* VerifyTick::tick() { // Waiting for verification from handler if (state->image_state == MissionState::ImageState::WAITING) { - return new VerifyTick(state); + return nullptr; } else if (state->image_state == MissionState::ImageState::VALID) { // Image is valid, we can end the mission return new EndTick(state); diff --git a/tests/integration/take_pictures.cpp b/tests/integration/take_pictures.cpp new file mode 100644 index 0000000..9ae15b8 --- /dev/null +++ b/tests/integration/take_pictures.cpp @@ -0,0 +1,20 @@ +#include +#include +#include + +#include "camera/mock.hpp" + +int main() { + MockCamera camera; + int init = camera.getImageCount(); + + camera.startTakingPictures(std::chrono::milliseconds(DUMMY_TIMEOUT)); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + camera.stopTakingPictures(); + + int post = camera.getImageCount(); + std::cout << "# of images taken:" << post << std::endl; + + return (post > init) ? 0 : 2; +} + diff --git a/tests/unit/mock_camera_test.cpp b/tests/unit/mock_camera_test.cpp new file mode 100644 index 0000000..69189f3 --- /dev/null +++ b/tests/unit/mock_camera_test.cpp @@ -0,0 +1,23 @@ +#include +#include + +#include "camera/mock.hpp" + +TEST(MockCameraTest, FilePathValid) { + MockCamera camera; + + std::vector entries; + for (auto& entry : std::filesystem::directory_iterator(images_dir)) { + entries.push_back(entry); + } + + // Expect set filepath has images + EXPECT_FALSE(entries.empty()); +} + +TEST(MockCameraTest, TestTakingPicture) { + MockCamera camera; + + std::optional img = camera.takePicture(std::chrono::milliseconds(DUMMY_TIMEOUT)); + EXPECT_TRUE(img.has_value()); +}