diff --git a/atos/modules/TruckObjectControl/CMakeLists.txt b/atos/modules/TruckObjectControl/CMakeLists.txt index 2096276c2..84c585449 100644 --- a/atos/modules/TruckObjectControl/CMakeLists.txt +++ b/atos/modules/TruckObjectControl/CMakeLists.txt @@ -56,6 +56,30 @@ ament_target_dependencies(atos_truck_simulator ament_index_cpp ) +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + file(GLOB TESTFILES "tests/*.cpp") + + ament_add_ros_isolated_gtest(${PROJECT_NAME}_test + ${TESTFILES} + ${CMAKE_CURRENT_SOURCE_DIR}/src/truckobjectcontrol.cpp + ) + target_include_directories(${PROJECT_NAME}_test PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../common + ) + target_link_libraries(${PROJECT_NAME}_test + ${COREUTILS_LIBRARY} + OpenSSL::SSL + OpenSSL::Crypto + ) + ament_target_dependencies(${PROJECT_NAME}_test + rclcpp + std_msgs + ament_index_cpp + ) +endif() + install(CODE "MESSAGE(STATUS \"Installing target ${PROJECT_NAME}\")") install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION "${CMAKE_INSTALL_LIBDIR}/atos" diff --git a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp index 0a7fa3cda..9938a3518 100644 --- a/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp +++ b/atos/modules/TruckObjectControl/src/truckobjectcontrol.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -1024,6 +1025,7 @@ void TruckObjectControl::handleTcpClient(const int client_fd, const std::string& void TruckObjectControl::evaluateAndPublishSpeedCommand() { constexpr double kMaxDistanceToPathM = 7.5; + constexpr double kMaxNearPathDistanceM = 15.0; struct ConnectedTruck { std::string id; @@ -1033,6 +1035,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { double distance_to_path_m = 0.0; int path_index = -1; std::string previous_command; + bool on_path = true; }; std::vector connected; @@ -1059,14 +1062,14 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { m_cot_timeout_seconds); continue; } - if (state.distance_to_path_m > kMaxDistanceToPathM) { + if (state.distance_to_path_m > kMaxNearPathDistanceM) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, "Skipping uid=%s from command eval: off-path by %.1f m (max %.1f m)", id.c_str(), state.distance_to_path_m, - kMaxDistanceToPathM); + kMaxNearPathDistanceM); continue; } connected.push_back(ConnectedTruck{id, @@ -1075,11 +1078,15 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { state.speed_mps, state.distance_to_path_m, state.path_index, - state.last_control_command}); + state.last_control_command, + state.distance_to_path_m <= kMaxDistanceToPathM}); } } - if (connected.empty()) { + const size_t on_path_count = static_cast( + std::count_if(connected.begin(), connected.end(), [](const ConnectedTruck& t) { return t.on_path; })); + + if (on_path_count == 0) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 3000, "No TCP-connected trucks available for command evaluation."); return; @@ -1092,14 +1099,14 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { { std::ostringstream summary; - summary << "Connected trucks=" << connected.size() << " ["; + summary << "Connected trucks=" << on_path_count << " ["; for (size_t i = 0; i < connected.size(); ++i) { const auto& t = connected[i]; if (i > 0) { summary << " | "; } summary << "uid=" << t.id << ",path=" << t.path_name << ",d=" << std::fixed << std::setprecision(1) - << t.distance_m; + << t.distance_m << (t.on_path ? "" : ",near_path"); } summary << "]"; RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 3000, "%s", summary.str().c_str()); @@ -1127,72 +1134,131 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { path_length_m = trajectory->back().distance_m; } + // Circular gap from one truck to another; <= 0 means no valid truck ahead. + const auto computeGap = [&](const ConnectedTruck& from, const ConnectedTruck& to) { + double gap = -1.0; + const int curr_path_index = from.path_index; + const int ahead_index = to.path_index; + + // Prefer index-based circular gap when both indices are valid for this trajectory. + bool used_index_gap = false; + if (trajectory && !trajectory->empty() && curr_path_index >= 0 && ahead_index >= 0 && + static_cast(curr_path_index) < trajectory->size() && + static_cast(ahead_index) < trajectory->size() && path_length_m > 0.0) { + const double curr_d = (*trajectory)[static_cast(curr_path_index)].distance_m; + const double ahead_d = (*trajectory)[static_cast(ahead_index)].distance_m; + gap = ahead_d - curr_d; + if (gap <= 0.0) { + // Wraparound: distance to end + distance from beginning. + gap += path_length_m; + } + used_index_gap = true; + } + + if (!used_index_gap) { + gap = to.distance_m - from.distance_m; + if (gap <= 0.0 && path_length_m > 0.0) { + gap += path_length_m; + } + } + return gap; + }; + for (size_t i = 0; i < group.size(); ++i) { - const bool has_peers = group.size() > 1; - bool has_ahead = has_peers; - std::string ahead_uid = "none"; - double ahead_gap = -1.0; - int ahead_path_index = -1; - - if (has_peers) { - const size_t ahead_index = (i + 1) % group.size(); - ahead_uid = group[ahead_index].id; - ahead_path_index = group[ahead_index].path_index; - const int curr_path_index = group[i].path_index; - - // Prefer index-based circular gap when both indices are valid for this trajectory. - bool used_index_gap = false; - if (trajectory && !trajectory->empty() && curr_path_index >= 0 && ahead_path_index >= 0 && - static_cast(curr_path_index) < trajectory->size() && - static_cast(ahead_path_index) < trajectory->size() && path_length_m > 0.0) { - const double curr_d = (*trajectory)[static_cast(curr_path_index)].distance_m; - const double ahead_d = (*trajectory)[static_cast(ahead_path_index)].distance_m; - ahead_gap = ahead_d - curr_d; - if (ahead_gap <= 0.0) { - // Wraparound: distance to end + distance from beginning. - ahead_gap += path_length_m; - } - used_index_gap = true; + if (!group[i].on_path) { + continue; + } + + const bool has_peers = group.size() > 1; + + // Nearest on-path and nearest near-path truck ahead, in circular order. + std::optional on_path_ahead; + std::optional near_path_ahead; + for (size_t k = 1; has_peers && k < group.size() && (!on_path_ahead || !near_path_ahead); ++k) { + const size_t candidate_index = (i + k) % group.size(); + if (group[candidate_index].on_path && !on_path_ahead) { + on_path_ahead = candidate_index; + } else if (!group[candidate_index].on_path && !near_path_ahead) { + near_path_ahead = candidate_index; } + } - if (!used_index_gap) { - ahead_gap = group[ahead_index].distance_m - group[i].distance_m; - if (ahead_gap <= 0.0 && path_length_m > 0.0) { - ahead_gap += path_length_m; - } + bool has_ahead = false; + std::string ahead_uid = "none"; + double ahead_gap = -1.0; + int ahead_path_index = -1; + if (on_path_ahead) { + const ConnectedTruck& ahead = group[*on_path_ahead]; + const double gap = computeGap(group[i], ahead); + if (gap > 0.0) { + has_ahead = true; + ahead_uid = ahead.id; + ahead_gap = gap; + ahead_path_index = ahead.path_index; } - if (ahead_gap <= 0.0) { - has_ahead = false; - ahead_uid = "none"; - ahead_path_index = -1; - ahead_gap = -1.0; + } + + bool has_near_ahead = false; + std::string near_uid = "none"; + double near_gap = -1.0; + int near_path_index = -1; + if (near_path_ahead) { + const ConnectedTruck& near_ahead = group[*near_path_ahead]; + const double gap = computeGap(group[i], near_ahead); + if (gap > 0.0) { + has_near_ahead = true; + near_uid = near_ahead.id; + near_gap = gap; + near_path_index = near_ahead.path_index; } } if (has_ahead) { min_gap_m = std::min(min_gap_m, ahead_gap); } + if (has_near_ahead) { + min_gap_m = std::min(min_gap_m, near_gap); + } const std::string previous_command = group[i].previous_command; - std::string control_command = previous_command.empty() ? "DRIVE" : previous_command; + std::string on_path_command = previous_command.empty() ? "DRIVE" : previous_command; if (!has_ahead) { - control_command = "DRIVE"; + on_path_command = "DRIVE"; } const bool in_warning_band = has_ahead && (ahead_gap <= 500.0 && ahead_gap > 200.0); const bool in_stop_band = has_ahead && (ahead_gap <= 200.0); if (in_warning_band) { - control_command = "SLOWDOWN"; + on_path_command = "SLOWDOWN"; } if (in_stop_band) { - control_command = "STOP"; + on_path_command = "STOP"; } if (has_ahead && ahead_gap > 300.0 && previous_command == "STOP") { - control_command = "SLOWDOWN"; + on_path_command = "SLOWDOWN"; } if (has_ahead && ahead_gap > 700.0 && previous_command == "SLOWDOWN") { - control_command = "DRIVE"; + on_path_command = "DRIVE"; + } + + // Near-path trucks can only cause SLOWDOWN, held until the gap exceeds 700 m. + const bool near_slowdown = + has_near_ahead && (near_gap <= 500.0 || (near_gap <= 700.0 && previous_command == "SLOWDOWN")); + const std::string near_path_command = near_slowdown ? "SLOWDOWN" : "DRIVE"; + + const auto severity = [](const std::string& cmd) { + return cmd == "STOP" ? 2 : (cmd == "SLOWDOWN" ? 1 : 0); + }; + const std::string control_command = + severity(near_path_command) > severity(on_path_command) ? near_path_command : on_path_command; + + const bool near_path_caused = control_command == "SLOWDOWN" && near_slowdown && + (on_path_command != "SLOWDOWN" || !has_ahead || near_gap < ahead_gap); + if (near_path_caused) { + ahead_uid = near_uid; + ahead_gap = near_gap; + ahead_path_index = near_path_index; } std::string target_speed_mps_value = std::to_string(std::max(0.0, group[i].speed_mps)); @@ -1215,7 +1281,9 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { const std::string reason = (control_command == "STOP") ? "truck_ahead_below_stop_distance" - : (control_command == "SLOWDOWN" ? "truck_ahead_slowdown_state" : "truck_ahead_drive_state"); + : (control_command == "SLOWDOWN" + ? (near_path_caused ? "truck_ahead_near_path_slowdown" : "truck_ahead_slowdown_state") + : "truck_ahead_drive_state"); uint64_t cmd_seq = 0; { std::lock_guard lock(m_state_mutex); @@ -1226,8 +1294,8 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { ";distance_to_truck_ahead_m=" + std::to_string(ahead_gap) + ";truck_ahead_path_index=" + std::to_string(ahead_path_index) + ";truck_ahead_uid=" + ahead_uid + ";reason=" + reason + ";min_gap_m=" + (std::isfinite(min_gap_m) ? std::to_string(min_gap_m) : "-1") + - ";connected_count=" + std::to_string(connected.size()) + ";path_name=" + path_name + - ";cmd_seq=" + std::to_string(cmd_seq); + ";connected_count=" + std::to_string(on_path_count) + ";path_name=" + path_name + + ";truck_ahead_near_path=" + (near_path_caused ? "1" : "0") + ";cmd_seq=" + std::to_string(cmd_seq); RCLCPP_INFO(get_logger(), "Prepared TCP command for uid=%s: %s", group[i].id.c_str(), tcp_command.c_str()); @@ -1269,7 +1337,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { std_msgs::msg::String command; command.data = "target_speed_mps=" + fleet_target_speed_mps_value + ";scope=all_connected_with_valid_tcp" + ";reason=" + fleet_reason + ";min_gap_m=" + std::to_string(min_gap_m) + - ";connected_count=" + std::to_string(connected.size()); + ";connected_count=" + std::to_string(on_path_count); m_speed_command_pub->publish(command); RCLCPP_WARN( @@ -1278,7 +1346,7 @@ void TruckObjectControl::evaluateAndPublishSpeedCommand() { fleet_target_speed_mps_value.c_str(), fleet_reason.c_str(), min_gap_m, - connected.size(), + on_path_count, sent_commands); } diff --git a/atos/modules/TruckObjectControl/tests/main.cpp b/atos/modules/TruckObjectControl/tests/main.cpp new file mode 100644 index 000000000..83cca6f1a --- /dev/null +++ b/atos/modules/TruckObjectControl/tests/main.cpp @@ -0,0 +1,6 @@ +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/atos/modules/TruckObjectControl/tests/test_truckobjectcontrol.cpp b/atos/modules/TruckObjectControl/tests/test_truckobjectcontrol.cpp new file mode 100644 index 000000000..53e01d497 --- /dev/null +++ b/atos/modules/TruckObjectControl/tests/test_truckobjectcontrol.cpp @@ -0,0 +1,353 @@ +#include "truckobjectcontrol.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +using json = nlohmann::json; +using KeyValues = std::map; + +namespace { +constexpr double kEarthRadiusM = 6378137.0; +constexpr double kPathLengthM = 3000.0; +constexpr double kVertexSpacingM = 10.0; + +// Margin around the 7.5 m / 15.0 m limits; exact values are not reproducible through lat/lon. +constexpr double kJustOnPathM = 7.49; +constexpr double kJustOffPathM = 7.51; +constexpr double kJustNearPathM = 14.99; +constexpr double kJustTooFarM = 15.01; + +struct TruckSpec { + std::string uid; + double along_m; + double lateral_m; +}; + +double metersToLatDeg(const double meters) { + return meters / kEarthRadiusM * 180.0 / M_PI; +} + +// Straight north-going path along lon=0, so along-track metres map linearly to latitude. +void writePathFile(const std::string& path) { + std::ostringstream coordinates; + coordinates << std::setprecision(15); + const int points = static_cast(kPathLengthM / kVertexSpacingM) + 1; + for (int i = 0; i < points; ++i) { + coordinates << (i > 0 ? "," : "") << "[0," << metersToLatDeg(i * kVertexSpacingM) << "]"; + } + std::ofstream out(path); + out << R"({"type":"FeatureCollection","features":[{"type":"Feature","id":"PathSection",)" + << R"("geometry":{"type":"LineString","coordinates":[)" << coordinates.str() << "]}}]}"; +} + +std::string makeCotXml(const TruckSpec& truck) { + const double lat_deg = metersToLatDeg(truck.along_m); + const double lon_deg = truck.lateral_m / (kEarthRadiusM * std::cos(lat_deg * M_PI / 180.0)) * 180.0 / M_PI; + std::ostringstream xml; + xml << std::setprecision(12) << "" + << "" + << ""; + return xml.str(); +} + +KeyValues parseKeyValues(const std::string& text) { + KeyValues out; + std::stringstream stream(text); + std::string token; + while (std::getline(stream, token, ';')) { + const auto sep = token.find('='); + if (sep != std::string::npos) { + out[token.substr(0, sep)] = token.substr(sep + 1); + } + } + return out; +} +} // namespace + +class TruckObjectControlTest : public ::testing::Test { +public: + static void SetUpTestSuite() { + s_pathFile = (std::filesystem::temp_directory_path() / + ("truck_object_control_test_" + std::to_string(::getpid()) + ".geojson")) + .string(); + writePathFile(s_pathFile); + + // Global ROS args are applied as parameter overrides to the node under test. + std::vector args{"truck_object_control_test", + "--ros-args", + "-p", + "trajectory_geojson_path:=" + s_pathFile, + "-p", + "cot_tcp_port:=0"}; + std::vector argv; + for (auto& arg : args) { + argv.push_back(arg.data()); + } + rclcpp::init(static_cast(argv.size()), argv.data()); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + std::filesystem::remove(s_pathFile); + } + +protected: + std::shared_ptr node; + std::shared_ptr helper; + rclcpp::Publisher::SharedPtr cotPub; + rclcpp::Subscription::SharedPtr stateSub; + rclcpp::Subscription::SharedPtr fleetSub; + rclcpp::executors::SingleThreadedExecutor executor; + + std::map states; + KeyValues fleetCommand; + + void SetUp() override { + node = std::make_shared(); + helper = rclcpp::Node::make_shared("truck_object_control_test_helper"); + + cotPub = helper->create_publisher("truck_objects/cot", 50); + stateSub = helper->create_subscription( + "truck_objects/state", 100, [this](std_msgs::msg::String::ConstSharedPtr msg) { + const json payload = json::parse(msg->data); + states[payload.at("uid").get()] = payload; + }); + fleetSub = helper->create_subscription( + "truck_objects/speed_command", 100, [this](std_msgs::msg::String::ConstSharedPtr msg) { + fleetCommand = parseKeyValues(msg->data); + }); + + executor.add_node(node); + executor.add_node(helper); + + ASSERT_TRUE(spinUntil([this]() { + return cotPub->get_subscription_count() > 0 && stateSub->get_publisher_count() > 0 && + fleetSub->get_publisher_count() > 0; + })) << "Test helper never connected to the node"; + } + + void TearDown() override { + executor.remove_node(node); + executor.remove_node(helper); + fleetSub.reset(); + stateSub.reset(); + cotPub.reset(); + helper.reset(); + node.reset(); + } + + bool spinUntil(const std::function& done, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(5000)) { + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + executor.spin_some(); + if (done()) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + return done(); + } + + // Publishes the trucks and lets the node run several 200 ms evaluation cycles on the static input. + void place(const std::vector& trucks) { + std::map expected_lat; + for (const auto& truck : trucks) { + std_msgs::msg::String msg; + msg.data = makeCotXml(truck); + cotPub->publish(msg); + expected_lat[truck.uid] = metersToLatDeg(truck.along_m); + } + + ASSERT_TRUE(spinUntil([&]() { + for (const auto& [uid, lat] : expected_lat) { + const auto it = states.find(uid); + if (it == states.end() || std::abs(it->second.at("lat").get() - lat) > 1e-9) { + return false; + } + } + return true; + })) << "Node never reported the published truck positions"; + + spinUntil([]() { return false; }, std::chrono::milliseconds(1200)); + } + + bool hasCommand(const std::string& uid) const { + const auto it = states.find(uid); + return it != states.end() && !it->second.at("last_tcp_command").get().empty(); + } + + KeyValues commandFor(const std::string& uid) const { + const auto it = states.find(uid); + if (it == states.end()) { + return {}; + } + return parseKeyValues(it->second.at("last_tcp_command").get()); + } + + static std::string s_pathFile; +}; + +std::string TruckObjectControlTest::s_pathFile; + +// Behaviour with on-path trucks only must be unchanged. + +TEST_F(TruckObjectControlTest, onPathTruckAheadAt400mGivesSlowdown) { + place({{"A", 500.0, 0.0}, {"B", 900.0, 0.0}}); + + const auto a = commandFor("A"); + EXPECT_EQ(a.at("command"), "SLOWDOWN"); + EXPECT_EQ(a.at("reason"), "truck_ahead_slowdown_state"); + EXPECT_EQ(a.at("truck_ahead_uid"), "B"); + EXPECT_EQ(a.at("truck_ahead_near_path"), "0"); + EXPECT_EQ(a.at("connected_count"), "2"); + EXPECT_EQ(commandFor("B").at("command"), "DRIVE"); +} + +TEST_F(TruckObjectControlTest, onPathTruckAheadAt150mGivesStop) { + place({{"A", 500.0, 0.0}, {"B", 650.0, 0.0}}); + + const auto a = commandFor("A"); + EXPECT_EQ(a.at("command"), "STOP"); + EXPECT_EQ(a.at("reason"), "truck_ahead_below_stop_distance"); + EXPECT_EQ(fleetCommand.at("reason"), "min_gap_below_stop_distance"); +} + +TEST_F(TruckObjectControlTest, singleOnPathTruckDrives) { + place({{"A", 500.0, 0.0}}); + + EXPECT_EQ(commandFor("A").at("command"), "DRIVE"); + EXPECT_EQ(commandFor("A").at("truck_ahead_uid"), "none"); +} + +// Near-path trucks cause SLOWDOWN only. + +TEST_F(TruckObjectControlTest, nearPathTruckAt400mAheadGivesSlowdown) { + place({{"A", 500.0, 0.0}, {"N", 900.0, 10.0}}); + + const auto a = commandFor("A"); + EXPECT_EQ(a.at("command"), "SLOWDOWN"); + EXPECT_EQ(a.at("reason"), "truck_ahead_near_path_slowdown"); + EXPECT_EQ(a.at("truck_ahead_uid"), "N"); + EXPECT_EQ(a.at("truck_ahead_near_path"), "1"); + EXPECT_NEAR(std::stod(a.at("distance_to_truck_ahead_m")), 400.0, 15.0); + EXPECT_EQ(a.at("connected_count"), "1"); +} + +TEST_F(TruckObjectControlTest, nearPathTruckAt100mAheadGivesSlowdownNotStop) { + place({{"A", 500.0, 0.0}, {"N", 600.0, 10.0}}); + + EXPECT_EQ(commandFor("A").at("command"), "SLOWDOWN"); + EXPECT_EQ(commandFor("A").at("reason"), "truck_ahead_near_path_slowdown"); + EXPECT_EQ(fleetCommand.at("reason"), "min_gap_below_warning_distance"); +} + +TEST_F(TruckObjectControlTest, nearPathTruckReceivesNoCommand) { + place({{"A", 500.0, 0.0}, {"N", 900.0, 10.0}}); + + EXPECT_TRUE(hasCommand("A")); + EXPECT_FALSE(hasCommand("N")); +} + +TEST_F(TruckObjectControlTest, nearPathTruckFarAheadIsIgnored) { + place({{"A", 500.0, 0.0}, {"N", 1500.0, 10.0}}); + + EXPECT_EQ(commandFor("A").at("command"), "DRIVE"); +} + +TEST_F(TruckObjectControlTest, nearPathTruckAheadAcrossPathWrapGivesSlowdown) { + place({{"A", 2800.0, 0.0}, {"N", 100.0, 10.0}}); + + const auto a = commandFor("A"); + EXPECT_EQ(a.at("command"), "SLOWDOWN"); + EXPECT_EQ(a.at("truck_ahead_uid"), "N"); + EXPECT_NEAR(std::stod(a.at("distance_to_truck_ahead_m")), 300.0, 15.0); +} + +TEST_F(TruckObjectControlTest, nearPathTruckBetweenOnPathTrucksDoesNotHideStop) { + place({{"A", 500.0, 0.0}, {"N", 600.0, 10.0}, {"C", 650.0, 0.0}}); + + const auto a = commandFor("A"); + EXPECT_EQ(a.at("command"), "STOP"); + EXPECT_EQ(a.at("truck_ahead_uid"), "C"); + EXPECT_EQ(a.at("truck_ahead_near_path"), "0"); +} + +TEST_F(TruckObjectControlTest, nearPathTruckDoesNotReleaseStopCausedByOnPathTruck) { + place({{"A", 500.0, 0.0}, {"B", 600.0, 0.0}, {"N", 1100.0, 10.0}}); + + EXPECT_EQ(commandFor("A").at("command"), "STOP"); + EXPECT_EQ(commandFor("A").at("truck_ahead_uid"), "B"); +} + +TEST_F(TruckObjectControlTest, onlyNearPathTrucksGiveNoCommands) { + place({{"N1", 500.0, 10.0}, {"N2", 600.0, 10.0}}); + + EXPECT_FALSE(hasCommand("N1")); + EXPECT_FALSE(hasCommand("N2")); +} + +// Lateral distance limits. + +TEST_F(TruckObjectControlTest, truckJustInsideOnPathLimitIsCommandedWithFullRules) { + place({{"A", 500.0, 0.0}, {"B", 650.0, kJustOnPathM}}); + + EXPECT_EQ(commandFor("A").at("command"), "STOP"); + EXPECT_TRUE(hasCommand("B")); +} + +TEST_F(TruckObjectControlTest, truckJustOutsideOnPathLimitIsNearPath) { + place({{"A", 500.0, 0.0}, {"B", 650.0, kJustOffPathM}}); + + EXPECT_EQ(commandFor("A").at("command"), "SLOWDOWN"); + EXPECT_EQ(commandFor("A").at("truck_ahead_near_path"), "1"); + EXPECT_FALSE(hasCommand("B")); +} + +TEST_F(TruckObjectControlTest, truckJustInsideNearPathLimitCounts) { + place({{"A", 500.0, 0.0}, {"N", 650.0, kJustNearPathM}}); + + EXPECT_EQ(commandFor("A").at("command"), "SLOWDOWN"); +} + +TEST_F(TruckObjectControlTest, truckJustBeyondNearPathLimitIsIgnored) { + place({{"A", 500.0, 0.0}, {"N", 650.0, kJustTooFarM}}); + + EXPECT_EQ(commandFor("A").at("command"), "DRIVE"); + EXPECT_FALSE(hasCommand("N")); +} + +// Hysteresis for near-path trucks follows the SLOWDOWN -> DRIVE rule (release above 700 m). + +TEST_F(TruckObjectControlTest, nearPathSlowdownIsHeldUntilGapExceeds700m) { + place({{"A", 500.0, 0.0}, {"N", 900.0, 10.0}}); + EXPECT_EQ(commandFor("A").at("command"), "SLOWDOWN"); + + place({{"N", 1100.0, 10.0}}); + EXPECT_EQ(commandFor("A").at("command"), "SLOWDOWN"); + + place({{"N", 1300.0, 10.0}}); + EXPECT_EQ(commandFor("A").at("command"), "DRIVE"); +} + +TEST_F(TruckObjectControlTest, nearPathTruckBetween500mAnd700mDoesNotStartSlowdown) { + place({{"A", 500.0, 0.0}, {"N", 1100.0, 10.0}}); + + EXPECT_EQ(commandFor("A").at("command"), "DRIVE"); +}