diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..1c6125f --- /dev/null +++ b/.clang-format @@ -0,0 +1,19 @@ +BasedOnStyle: Mozilla +InsertNewlineAtEOF: true +UseTab: Always +IndentWidth: 4 +TabWidth: 4 +ColumnLimit: 120 +AccessModifierOffset: -4 +AlignConsecutiveAssignments: Consecutive +AllowShortBlocksOnASingleLine: Empty +AllowShortFunctionsOnASingleLine: Empty +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +BraceWrapping: + SplitEmptyFunction: false +BreakBeforeBraces: Custom +BreakConstructorInitializers: AfterColon +Cpp11BracedListStyle: true +FixNamespaceComments: true +PackConstructorInitializers: Never diff --git a/Dockerfile b/Dockerfile index 69e76a0..31ef4b7 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,5 +1,5 @@ FROM alpine:latest AS builder -RUN apk add --no-cache cmake make git boost-dev=1.84.0-r2 g++ +RUN apk add --no-cache cmake make git boost-dev=1.84.0-r3 g++ WORKDIR /app COPY . . WORKDIR /app/build @@ -16,4 +16,4 @@ COPY --from=builder /app/build/libISO_object.so /app/build/iso22133/libISO_objec # App COPY --from=builder /app/build/ISO_objectDemo /app/build/ISO_objectDemo -ENTRYPOINT ["/app/build/ISO_objectDemo"] \ No newline at end of file +ENTRYPOINT ["/app/build/ISO_objectDemo"] diff --git a/demoIsoObject.cpp b/demoIsoObject.cpp index db50c7f..2cb9c45 100644 --- a/demoIsoObject.cpp +++ b/demoIsoObject.cpp @@ -1,7 +1,6 @@ #include - #include "iso22133object.hpp" #include "printUtil.hpp" #include @@ -14,20 +13,19 @@ namespace po = boost::program_options; * \param argv The arguments. * \return The parsed arguments. */ -static po::variables_map parseArguments( - int argc, - char** argv) -{ +static po::variables_map parseArguments(int argc, char** argv) { po::variables_map ret; po::options_description desc("Allowed options"); - desc.add_options() - ("help,h", "print this message") - ("listen-ip,i", po::value()->default_value("0.0.0.0"), "The IP address that the isoObject will listen on.") - ("behaviour,b", po::value()->default_value("follow-trajectory"), "The behaviour of the isoObject. Options are 'follow-trajectory', 'dynamic', and 'circle'") - ; + desc.add_options()("help,h", "print this message"); + desc.add_options()("listen-ip,i", + po::value()->default_value("0.0.0.0"), + "The IP address that the isoObject will listen on."); + desc.add_options()("behaviour,b", + po::value()->default_value("follow-trajectory"), + "The behaviour of the isoObject. Options are 'follow-trajectory', 'dynamic', and 'circle'"); po::store(po::parse_command_line(argc, argv, desc), ret); po::notify(ret); - if (ret.count("help")){ + if (ret.count("help")) { std::cout << desc << std::endl; exit(EXIT_FAILURE); } @@ -36,368 +34,361 @@ static po::variables_map parseArguments( class myDisarmed : public ISO22133::Disarmed { public: - /** - * @brief Called once when entering state - * - * @param obj - */ - void onEnter(ISO22133::TestObject& obj) override { - std::cout << "Entering disarmed" << std::endl; - } - - /** - * @brief Called once when leaving state - * - */ - void onExit(ISO22133::TestObject&) override{ - std::cout << "Leaving disarmed" << std::endl; - } + /** + * @brief Called once when entering state + * + * @param obj + */ + void onEnter(ISO22133::TestObject& obj) override { + std::cout << "Entering disarmed" << std::endl; + } + /** + * @brief Called once when leaving state + * + */ + void onExit(ISO22133::TestObject&) override { + std::cout << "Leaving disarmed" << std::endl; + } }; class myPreArming : public ISO22133::PreArming { public: - void onEnter(ISO22133::TestObject& obj) override{ - std::cout << "Entering Pre-Arming" << std::endl; - try { - this->handleEvent(obj, ISO22133::Events::N); - } - catch(const std::runtime_error& e) { - std::cerr << e.what() << '\n'; - } - } + void onEnter(ISO22133::TestObject& obj) override { + std::cout << "Entering Pre-Arming" << std::endl; + try { + this->handleEvent(obj, ISO22133::Events::N); + } catch (const std::runtime_error& e) { + std::cerr << e.what() << '\n'; + } + } }; - - class myObject : public ISO22133::TestObject { public: - std::vector trajectory; - - void setMonr(double x, - double y, - double z, - double heading_rad, - double lateral_m_s, - double lonitudinal_m_s) { - // Initialize required fields in MONR - CartesianPosition pos; - SpeedType spd; - pos.xCoord_m = x; - pos.yCoord_m = y; - pos.zCoord_m = z; - pos.heading_rad = heading_rad; - pos.isHeadingValid = true; - pos.isPositionValid = true; - pos.isXcoordValid = true; - pos.isYcoordValid = true; - pos.isZcoordValid = true; - spd.lateral_m_s = lateral_m_s; - spd.longitudinal_m_s = lonitudinal_m_s; - spd.isLateralValid = true; - spd.isLongitudinalValid = true; - - this->setPosition(pos); - this->setSpeed(spd); - } - myObject(std::string ip) : - ISO22133::TestObject(ip), - dummyMember(0) { - ObjectSettingsType osem; - osem.testMode = TEST_MODE_UNAVAILABLE; - setMonr(1,2,3,0.4,5,6); - setObjectSettings(osem); - } - /** - * @brief User must override this function for handling internal - * abort prerequisites of the test object - * - */ - void handleAbort() { std::cout << "Bromsa!" << std::endl;} - - /** - * @brief Create a myDisarmed object. - * This is an example of how to override the state creation - * functions to get the new state - * - * @return ISO22133::Disarmed* - */ - ISO22133::Disarmed* createDisarmed() const override { + std::vector trajectory; + + void setMonr(double x, double y, double z, double heading_rad, double lateral_m_s, double lonitudinal_m_s) { + // Initialize required fields in MONR + CartesianPosition pos; + SpeedType spd; + pos.xCoord_m = x; + pos.yCoord_m = y; + pos.zCoord_m = z; + pos.heading_rad = heading_rad; + pos.isHeadingValid = true; + pos.isPositionValid = true; + pos.isXcoordValid = true; + pos.isYcoordValid = true; + pos.isZcoordValid = true; + spd.lateral_m_s = lateral_m_s; + spd.longitudinal_m_s = lonitudinal_m_s; + spd.isLateralValid = true; + spd.isLongitudinalValid = true; + + this->setPosition(pos); + this->setSpeed(spd); + } + myObject(std::string ip) : + ISO22133::TestObject(ip), + dummyMember(0) { + ObjectSettingsType osem; + osem.testMode = TEST_MODE_UNAVAILABLE; + setMonr(1, 2, 3, 0.4, 5, 6); + setObjectSettings(osem); + } + /** + * @brief User must override this function for handling internal + * abort prerequisites of the test object + * + */ + void handleAbort() { + std::cout << "Bromsa!" << std::endl; + } + + /** + * @brief Create a myDisarmed object. + * This is an example of how to override the state creation + * functions to get the new state + * + * @return ISO22133::Disarmed* + */ + ISO22133::Disarmed* createDisarmed() const override { return dynamic_cast(new myDisarmed); } - ISO22133::PreArming* createPreArming() const override { + ISO22133::PreArming* createPreArming() const override { return dynamic_cast(new myPreArming); } - //! overridden on*message* function. - void onOSEM(ObjectSettingsType& osem) override { - std::cout << "Object Settings Received" << std::endl; - setObjectSettings(osem); - PRINT_STRUCT(ObjectSettingsType, &osem, - PRINT_FIELD(TestModeType, testMode) - ) - - } - - void onTRAJ() override { - std::cout << "Got onTRAJ signal, fetching new traj segments" << std::endl; - std::vector newTraj; - newTraj = this->getTrajectory(); - if (this->getObjectSettings().testMode == TEST_MODE_ONLINE){ - std::cout << "Test mode is online planned, appending new trajectory to existing" << std::endl; - this->trajectory.insert(this->trajectory.end(), newTraj.begin(), newTraj.end()); - - // We might receive trajectories that overlap, we remove the duplicate points by checking the time - std::sort(this->trajectory.begin(), this->trajectory.end(), [](const TrajectoryWaypointType& t1, const TrajectoryWaypointType& t2) { - return t1.relativeTime.tv_sec * 1000000 + t1.relativeTime.tv_usec < t2.relativeTime.tv_sec * 1000000 + t2.relativeTime.tv_usec; - }); - this->trajectory.erase(std::unique(this->trajectory.begin(), this->trajectory.end(), [](const TrajectoryWaypointType& t1, const TrajectoryWaypointType& t2) { - return t1.relativeTime.tv_sec * 1000000 + t1.relativeTime.tv_usec == t2.relativeTime.tv_sec * 1000000 + t2.relativeTime.tv_usec; - }), this->trajectory.end()); - } else { - std::cout << "Test mode is preplanned, replacing existing trajectory" << std::endl; - this->trajectory = newTraj; - } - std::cout << "Trajectory size: " << this->trajectory.size() << std::endl; - } - - void onSTRT(StartMessageType&) override { - std::cout << "Object Starting" << std::endl; - } - - //! overridden vendor specific message handling - int handleVendorSpecificMessage(const int msgType, const std::vector& data) override { - int handledBytes = 0; - RemoteControlManoeuvreMessageType DCMMmsg; - switch (msgType) - { - case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_DCMM: - handledBytes = decodeDCMMMessage(data.data(), data.size(), &DCMMmsg, 0); - if(handledBytes < 0) { - throw std::invalid_argument("Error decoding DCMM"); - } - else { - std::cout << "Handled DCMM Message" << std::endl; - } - break; - - default: - break; - } - - return handledBytes; - } -private: - int dummyMember; - void dummyFunc() { - std::stringstream ss; - ss << "I am printed in a useless function" << std::endl; - std::cout << ss.str(); - }; + //! overridden on*message* function. + void onOSEM(ObjectSettingsType& osem) override { + std::cout << "Object Settings Received" << std::endl; + setObjectSettings(osem); + PRINT_STRUCT(ObjectSettingsType, &osem, PRINT_FIELD(TestModeType, testMode)) + } + + void onTRAJ() override { + std::cout << "Got onTRAJ signal, fetching new traj segments" << std::endl; + std::vector newTraj; + newTraj = this->getTrajectory(); + if (this->getObjectSettings().testMode == TEST_MODE_ONLINE) { + std::cout << "Test mode is online planned, appending new trajectory to existing" << std::endl; + this->trajectory.insert(this->trajectory.end(), newTraj.begin(), newTraj.end()); + + // We might receive trajectories that overlap, we remove the duplicate points by checking the time + std::sort(this->trajectory.begin(), + this->trajectory.end(), + [](const TrajectoryWaypointType& t1, const TrajectoryWaypointType& t2) { + return t1.relativeTime.tv_sec * 1000000 + t1.relativeTime.tv_usec < + t2.relativeTime.tv_sec * 1000000 + t2.relativeTime.tv_usec; + }); + this->trajectory.erase(std::unique(this->trajectory.begin(), + this->trajectory.end(), + [](const TrajectoryWaypointType& t1, const TrajectoryWaypointType& t2) { + return t1.relativeTime.tv_sec * 1000000 + t1.relativeTime.tv_usec == + t2.relativeTime.tv_sec * 1000000 + t2.relativeTime.tv_usec; + }), + this->trajectory.end()); + } else { + std::cout << "Test mode is preplanned, replacing existing trajectory" << std::endl; + this->trajectory = newTraj; + } + std::cout << "Trajectory size: " << this->trajectory.size() << std::endl; + } + void onSTRT(StartMessageType&) override { + std::cout << "Object Starting" << std::endl; + } + + //! overridden vendor specific message handling + int handleVendorSpecificMessage(const int msgType, const std::vector& data) override { + int handledBytes = 0; + RemoteControlManoeuvreMessageType DCMMmsg; + switch (msgType) { + case MESSAGE_ID_VENDOR_SPECIFIC_ASTAZERO_DCMM: + handledBytes = decodeDCMMMessage(data.data(), data.size(), &DCMMmsg, 0); + if (handledBytes < 0) { + throw std::invalid_argument("Error decoding DCMM"); + } else { + std::cout << "Handled DCMM Message" << std::endl; + } + break; + + default: + break; + } + + return handledBytes; + } + +private: + int dummyMember; + void dummyFunc() { + std::stringstream ss; + ss << "I am printed in a useless function" << std::endl; + std::cout << ss.str(); + }; }; // Function that can parse both number and dot notation and hostnames into IP addresses -std::string resolveIP (std::string listen_ip) { - addrinfo hints = {0}; - addrinfo* result; - in_addr_t ip; - hints.ai_family = AF_INET; // Use AF_INET6 for IPv6 - hints.ai_socktype = SOCK_STREAM; - - int status = getaddrinfo(listen_ip.c_str(), nullptr, &hints, &result); - if (status != 0) { - std::cout << "Failed to resolve address for value %s, Default to 0.0.0.0" << listen_ip << std::endl; - return "0.0.0.0"; - } - - ip = ((sockaddr_in*)result->ai_addr)->sin_addr.s_addr; - freeaddrinfo(result); - - // Convert binary IP to string for logging - char ip_str[INET_ADDRSTRLEN]; - inet_ntop(AF_INET, &ip, ip_str, INET_ADDRSTRLEN); - return ip_str; +std::string resolveIP(std::string listen_ip) { + addrinfo hints = {0}; + addrinfo* result; + in_addr_t ip; + hints.ai_family = AF_INET; // Use AF_INET6 for IPv6 + hints.ai_socktype = SOCK_STREAM; + + int status = getaddrinfo(listen_ip.c_str(), nullptr, &hints, &result); + if (status != 0) { + std::cout << "Failed to resolve address for value %s, Default to 0.0.0.0" << listen_ip << std::endl; + return "0.0.0.0"; + } + + ip = ((sockaddr_in*)result->ai_addr)->sin_addr.s_addr; + freeaddrinfo(result); + + // Convert binary IP to string for logging + char ip_str[INET_ADDRSTRLEN]; + inet_ntop(AF_INET, &ip, ip_str, INET_ADDRSTRLEN); + return ip_str; } /** * @brief ISO-object that automatically gets all the points from the trajectory when connected, * and will set its location to the first point of the trajectory when armed. It will then follow * the trajectory when running and set its location to the last point when done. - * + * */ void runFollowTrajectory(myObject& obj) { - std::vector traj; - double startX; - double endX; - double startY; - double endY; - double startZ; - double endZ; - double startYaw; - double endYaw; - - auto finishedRunning = false; - while(1) { - auto state = obj.getCurrentStateName(); - if (state == "Disarmed") { - // sleep for a while to get all trajectory points - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - traj = obj.getTrajectory(); - startX = traj[0].pos.xCoord_m; - endX = traj.back().pos.xCoord_m; - startY = traj[0].pos.yCoord_m; - endY = traj.back().pos.yCoord_m; - startZ = traj[0].pos.zCoord_m; - endZ = traj.back().pos.zCoord_m; - startYaw = traj[0].pos.heading_rad; - endYaw = traj.back().pos.heading_rad; - obj.setMonr(endX, endY, endZ, endYaw, 0.0, 0.0); - finishedRunning = false; - } - else if (state == "Armed") { - obj.setMonr(startX, startY, startZ, startYaw, 0.0, 0.0); - } - else if (finishedRunning) { - obj.setMonr(endX, endY, endZ, endYaw, 0.0, 0.0); - } - else if (state == "Running") { - for (int i = 0; i < traj.size() - 1; ++i) { - auto currentTraj = traj[i]; - auto nextTraj = traj[i+1]; - auto secondsDiff = nextTraj.relativeTime.tv_sec - currentTraj.relativeTime.tv_sec; - auto microsecondsDiff = nextTraj.relativeTime.tv_usec - currentTraj.relativeTime.tv_usec; - auto timeDiff = secondsDiff * 1000000 + microsecondsDiff; - - obj.setMonr(currentTraj.pos.xCoord_m, currentTraj.pos.yCoord_m, currentTraj.pos.zCoord_m, currentTraj.pos.heading_rad, currentTraj.spd.lateral_m_s, currentTraj.spd.longitudinal_m_s); - std::this_thread::sleep_for(std::chrono::microseconds(timeDiff)); - } - finishedRunning = true; - } - } + std::vector traj; + double startX; + double endX; + double startY; + double endY; + double startZ; + double endZ; + double startYaw; + double endYaw; + + auto finishedRunning = false; + while (1) { + auto state = obj.getCurrentStateName(); + if (state == "Disarmed") { + // sleep for a while to get all trajectory points + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + traj = obj.getTrajectory(); + startX = traj[0].pos.xCoord_m; + endX = traj.back().pos.xCoord_m; + startY = traj[0].pos.yCoord_m; + endY = traj.back().pos.yCoord_m; + startZ = traj[0].pos.zCoord_m; + endZ = traj.back().pos.zCoord_m; + startYaw = traj[0].pos.heading_rad; + endYaw = traj.back().pos.heading_rad; + obj.setMonr(endX, endY, endZ, endYaw, 0.0, 0.0); + finishedRunning = false; + } else if (state == "Armed") { + obj.setMonr(startX, startY, startZ, startYaw, 0.0, 0.0); + } else if (finishedRunning) { + obj.setMonr(endX, endY, endZ, endYaw, 0.0, 0.0); + } else if (state == "Running") { + for (int i = 0; i < traj.size() - 1; ++i) { + auto currentTraj = traj[i]; + auto nextTraj = traj[i + 1]; + auto secondsDiff = nextTraj.relativeTime.tv_sec - currentTraj.relativeTime.tv_sec; + auto microsecondsDiff = nextTraj.relativeTime.tv_usec - currentTraj.relativeTime.tv_usec; + auto timeDiff = secondsDiff * 1000000 + microsecondsDiff; + + obj.setMonr(currentTraj.pos.xCoord_m, + currentTraj.pos.yCoord_m, + currentTraj.pos.zCoord_m, + currentTraj.pos.heading_rad, + currentTraj.spd.lateral_m_s, + currentTraj.spd.longitudinal_m_s); + std::this_thread::sleep_for(std::chrono::microseconds(timeDiff)); + } + finishedRunning = true; + } + } } - /** - * @brief ISO-object that can be used with dynamic trajectories. The ISO-object works in the same way as runFollowTrajectory, but it will get - * a new trajectory at runtime, instead of the full trajectory when connecting. - * + * @brief ISO-object that can be used with dynamic trajectories. The ISO-object works in the same way as + * runFollowTrajectory, but it will get a new trajectory at runtime, instead of the full trajectory when connecting. + * */ void runDynamic(myObject& obj) { - std::vector traj; - double startX; - double endX; - double startY; - double endY; - double startZ; - double endZ; - double startYaw; - double endYaw; - - auto finishedRunning = false; - while(1) { - auto state = obj.getCurrentStateName(); - if (state == "Disarmed") { - // sleep for a while to get all trajectory points - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - traj = obj.getTrajectory(); - startX = traj[0].pos.xCoord_m; - endX = traj.back().pos.xCoord_m; - startY = traj[0].pos.yCoord_m; - endY = traj.back().pos.yCoord_m; - startZ = traj[0].pos.zCoord_m; - endZ = traj.back().pos.zCoord_m; - startYaw = traj[0].pos.heading_rad; - endYaw = traj.back().pos.heading_rad; - obj.setMonr(endX, endY, endZ, endYaw, 0.0, 0.0); - finishedRunning = false; - } - else if (state == "Armed") { - obj.setMonr(startX, startY, startZ, startYaw, 0.0, 0.0); - } - else if (finishedRunning) { - obj.setMonr(endX, endY, endZ, endYaw, 0.0, 0.0); - } - else if (state == "Running") { - int i = 0; - TrajectoryWaypointType currentTraj; - TrajectoryWaypointType nextTraj; - while (i < traj.size()) { - currentTraj = traj[i]; - nextTraj = traj[i+1]; - auto secondsDiff = nextTraj.relativeTime.tv_sec - currentTraj.relativeTime.tv_sec; - auto microsecondsDiff = nextTraj.relativeTime.tv_usec - currentTraj.relativeTime.tv_usec; - auto timeDiff = secondsDiff * 1000000 + microsecondsDiff; - - obj.setMonr(currentTraj.pos.xCoord_m, currentTraj.pos.yCoord_m, currentTraj.pos.zCoord_m, currentTraj.pos.heading_rad, currentTraj.spd.lateral_m_s, currentTraj.spd.longitudinal_m_s); - std::this_thread::sleep_for(std::chrono::microseconds(timeDiff)); - ++i; - traj = obj.trajectory; - } - endX = currentTraj.pos.xCoord_m; - endY = currentTraj.pos.yCoord_m; - endZ = currentTraj.pos.zCoord_m; - endYaw = currentTraj.pos.heading_rad; - finishedRunning = true; - } - } + std::vector traj; + double startX; + double endX; + double startY; + double endY; + double startZ; + double endZ; + double startYaw; + double endYaw; + + auto finishedRunning = false; + while (true) { + auto state = obj.getCurrentStateName(); + if (state == "Disarmed") { + // sleep for a while to get all trajectory points + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + traj = obj.getTrajectory(); + startX = traj[0].pos.xCoord_m; + endX = traj.back().pos.xCoord_m; + startY = traj[0].pos.yCoord_m; + endY = traj.back().pos.yCoord_m; + startZ = traj[0].pos.zCoord_m; + endZ = traj.back().pos.zCoord_m; + startYaw = traj[0].pos.heading_rad; + endYaw = traj.back().pos.heading_rad; + obj.setMonr(endX, endY, endZ, endYaw, 0.0, 0.0); + finishedRunning = false; + } else if (state == "Armed") { + obj.setMonr(startX, startY, startZ, startYaw, 0.0, 0.0); + } else if (finishedRunning) { + obj.setMonr(endX, endY, endZ, endYaw, 0.0, 0.0); + } else if (state == "Running") { + int i = 0; + TrajectoryWaypointType currentTraj; + TrajectoryWaypointType nextTraj; + while (i < traj.size()) { + currentTraj = traj[i]; + nextTraj = traj[i + 1]; + auto secondsDiff = nextTraj.relativeTime.tv_sec - currentTraj.relativeTime.tv_sec; + auto microsecondsDiff = nextTraj.relativeTime.tv_usec - currentTraj.relativeTime.tv_usec; + auto timeDiff = secondsDiff * 1000000 + microsecondsDiff; + + obj.setMonr(currentTraj.pos.xCoord_m, + currentTraj.pos.yCoord_m, + currentTraj.pos.zCoord_m, + currentTraj.pos.heading_rad, + currentTraj.spd.lateral_m_s, + currentTraj.spd.longitudinal_m_s); + std::this_thread::sleep_for(std::chrono::microseconds(timeDiff)); + ++i; + traj = obj.trajectory; + } + endX = currentTraj.pos.xCoord_m; + endY = currentTraj.pos.yCoord_m; + endZ = currentTraj.pos.zCoord_m; + endYaw = currentTraj.pos.heading_rad; + finishedRunning = true; + } + } } /** * @brief ISO-object that moves in a circle when connected. - * + * */ void runCircle(myObject& obj) { - double originX = 0.0; - double originY = 0.0; - double originZ = 0.0; - double radius = 5.0; - double angle = 0.0; - double x = 0.0; - double y = 0.0; - double z = 0.0; - - while (true) { - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - angle += 0.005; - if (angle > 2 * M_PI) { - angle = 0.0; - } - x = originX + radius * cos(angle); - y = originY + radius * sin(angle); - z = originZ + radius/2 * sin(angle); - if (z < 0) { - z = 0; - } - // Todo calculate heading and speed - obj.setMonr(x, y, z, angle + M_PI / 2, 0.0, 0.0); - } + double originX = 0.0; + double originY = 0.0; + double originZ = 0.0; + double radius = 5.0; + double angle = 0.0; + double x = 0.0; + double y = 0.0; + double z = 0.0; + + while (true) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + angle += 0.005; + if (angle > 2 * M_PI) { + angle = 0.0; + } + x = originX + radius * std::cos(angle); + y = originY + radius * std::sin(angle); + z = originZ + radius / 2 * std::sin(angle); + z = std::max(0.0, z); + + // Todo calculate heading and speed + obj.setMonr(x, y, z, angle + M_PI / 2, 0.0, 0.0); + } } /** - * @brief - * - * @param argc - * @param argv - * @return int + * @brief + * + * @param argc + * @param argv + * @return int */ -int main(int argc, char** argv ) { - auto args = parseArguments(argc, argv); - auto ip = resolveIP(args["listen-ip"].as()); - myObject obj(ip); - std::string behaviour = args["behaviour"].as(); - if (behaviour == "follow-trajectory") { - runFollowTrajectory(obj); - } - else if (behaviour == "dynamic") { - runDynamic(obj); - } - else if (behaviour == "circle") { - runCircle(obj); - } - else { - std::invalid_argument("Unknown behaviour"); - } +int main(int argc, char** argv) { + auto args = parseArguments(argc, argv); + auto ip = resolveIP(args["listen-ip"].as()); + myObject obj(ip); + std::string behaviour = args["behaviour"].as(); + if (behaviour == "follow-trajectory") { + runFollowTrajectory(obj); + } else if (behaviour == "dynamic") { + runDynamic(obj); + } else if (behaviour == "circle") { + runCircle(obj); + } else { + std::invalid_argument("Unknown behaviour"); + } } - diff --git a/inc/iso22133object.hpp b/inc/iso22133object.hpp index d30f6be..31d0f87 100644 --- a/inc/iso22133object.hpp +++ b/inc/iso22133object.hpp @@ -1,17 +1,17 @@ #pragma once -#include +#include #include -#include #include -#include +#include +#include #include "header.h" #include "iso22133.h" #include "iso22133state.hpp" -#include "trajDecoder.hpp" #include "signal.hpp" #include "tcpServer.hpp" +#include "trajDecoder.hpp" #include "udpServer.hpp" namespace ISO22133 { @@ -28,7 +28,6 @@ class Aborting; class PreArming; class PreRunning; - #define DEFAULT_CONTROL_CENTER_ID 0 // 0 is reserved for the control center /** * @brief The TestObject class is an abstract form of a @@ -58,85 +57,163 @@ class TestObject { void disconnect(); void shutdown(); - void setPosition(const CartesianPosition& pos) { position = pos; } - void setSpeed(const SpeedType& spd) { speed = spd; } - void setObjectSettings(const ObjectSettingsType& osem) { objectSettings = osem; } - void setAcceleration(const AccelerationType& acc) { acceleration = acc; } - void setDriveDirection(const DriveDirectionType& drd) { driveDirection = drd; } - void setObjectState(const ObjectStateID& ost) { objectState = ost; } - void setName(const std::string nm) { name = nm; } - void setReadyToArm(const int& rdy) { readyToArm = rdy; } - void setErrorState(const char err) { errorState = err; } - - std::string getCurrentStateName() const { return state->getName(); } - int getCurrentStateID() const { return state->getStateID(); } - std::string getName() const { return name; } - CartesianPosition getPosition() const { return position; } - SpeedType getSpeed() const { return speed; } - AccelerationType getAcceleration() const { return acceleration; } - DriveDirectionType getDriveDirection() const { return driveDirection; } - TrajectoryHeaderType getTrajectoryHeader() const { return trajDecoder.getTrajHeader(); } - std::vector getTrajectory() const { return trajDecoder.getTraj(); } - GeographicPositionType getOrigin() const { return origin; } - std::string getLocalIP() const { return localIP; } - uint32_t getTransmitterID() const { return transmitterID; } - uint32_t getReceiverID() const { return receiverID; } - ObjectSettingsType getObjectSettings() const { return objectSettings; } + void setPosition(const CartesianPosition& pos) { + position = pos; + } + void setSpeed(const SpeedType& spd) { + speed = spd; + } + void setObjectSettings(const ObjectSettingsType& osem) { + objectSettings = osem; + } + void setAcceleration(const AccelerationType& acc) { + acceleration = acc; + } + void setDriveDirection(const DriveDirectionType& drd) { + driveDirection = drd; + } + void setObjectState(const ObjectStateID& ost) { + objectState = ost; + } + void setName(const std::string nm) { + name = nm; + } + void setReadyToArm(const int& rdy) { + readyToArm = rdy; + } + void setErrorState(const char err) { + errorState = err; + } + std::string getCurrentStateName() const { + return state->getName(); + } + int getCurrentStateID() const { + return state->getStateID(); + } + std::string getName() const { + return name; + } + CartesianPosition getPosition() const { + return position; + } + SpeedType getSpeed() const { + return speed; + } + AccelerationType getAcceleration() const { + return acceleration; + } + DriveDirectionType getDriveDirection() const { + return driveDirection; + } + TrajectoryHeaderType getTrajectoryHeader() const { + return trajDecoder.getTrajHeader(); + } + std::vector getTrajectory() const { + return trajDecoder.getTraj(); + } + GeographicPositionType getOrigin() const { + return origin; + } + std::string getLocalIP() const { + return localIP; + } + uint32_t getTransmitterID() const { + return transmitterID; + } + uint32_t getReceiverID() const { + return receiverID; + } + ObjectSettingsType getObjectSettings() const { + return objectSettings; + } /** SWIG Wrappers **/ //! Wrapper for handling tcp messages when using an iso connector for simulation - int handleTCPMessage(char *buffer, int bufferLen); + int handleTCPMessage(char* buffer, int bufferLen); //! Wrapper for handling udp message when using an iso connector for simulation - int handleUDPMessage(char *buffer, int bufferLen, int udpSocket, char *addr, const uint32_t port); - + int handleUDPMessage(char* buffer, int bufferLen, int udpSocket, char* addr, const uint32_t port); //! Used to start the threads - void startHandleTCP() { tcpReceiveThread = std::thread(&TestObject::receiveTCP, this); } - void startHandleUDP() { udpReceiveThread = std::thread(&TestObject::receiveUDP, this); } - void startHEABCheck() { heabTimeoutThread = std::thread(&TestObject::checkHeabLoop, this); } - void startSendMonr() { monrThread = std::thread(&TestObject::sendMonrLoop, this); } + void startHandleTCP() { + tcpReceiveThread = std::thread(&TestObject::receiveTCP, this); + } + void startHandleUDP() { + udpReceiveThread = std::thread(&TestObject::receiveUDP, this); + } + void startHEABCheck() { + heabTimeoutThread = std::thread(&TestObject::checkHeabLoop, this); + } + void startSendMonr() { + monrThread = std::thread(&TestObject::sendMonrLoop, this); + } protected: //! Wrapper for handling function that converts to char vector - int handleMessage(char *buffer, int bufferLen); + int handleMessage(char* buffer, int bufferLen); //! Fill message header with receiver/transmitter id and messageCounter. Returns pointer to input header. - MessageHeaderType *populateMessageHeader(MessageHeaderType *header); + MessageHeaderType* populateMessageHeader(MessageHeaderType* header); //! Pure virtual safety function that must be implemented by the user. - virtual void handleAbort() { throw std::logic_error("Use of unimplemented abort handler"); } + virtual void handleAbort() { + throw std::logic_error("Use of unimplemented abort handler"); + } //! Virtual function for adding handling of vendor specific messages if needed. //! Expected to return the number of handled bytes or <= 0 if message was not recognized //! or decoding failed. - virtual int handleVendorSpecificMessage(const int msgType, const std::vector& data) { return -1; } + virtual int handleVendorSpecificMessage(const int msgType, const std::vector& data) { + return -1; + } // These should be overridden if extending one of the states // Example of override: // return dynamic_cast(new myPreArming); //! Must be overridden if modifying the Unknown state - virtual Unknown* createUnknown() const { return new Unknown; } + virtual Unknown* createUnknown() const { + return new Unknown; + } //! Must be overridden if modifying the Off state - virtual Off* createOff() const { return new Off; } + virtual Off* createOff() const { + return new Off; + } //! Must be overridden if modifying the Init state - virtual Init* createInit() const { return new Init; } + virtual Init* createInit() const { + return new Init; + } //! Must be overridden if modifying the Armed state - virtual Armed* createArmed() const { return new Armed; } + virtual Armed* createArmed() const { + return new Armed; + } //! Must be overridden if modifying the Disarmed state - virtual Disarmed* createDisarmed() const { return new Disarmed; } + virtual Disarmed* createDisarmed() const { + return new Disarmed; + } //! Must be overridden if modifying the Running state - virtual Running* createRunning() const { return new Running; } + virtual Running* createRunning() const { + return new Running; + } //! Must be overridden if modifying the Postrun state - virtual PostRun* createPostRun() const { return new PostRun; } + virtual PostRun* createPostRun() const { + return new PostRun; + } //! Must be overridden if modifying the Remote Controlled state - virtual RemoteControlled* createRemoteControlled() const { return new RemoteControlled; } + virtual RemoteControlled* createRemoteControlled() const { + return new RemoteControlled; + } //! Must be overridden if modifying the Abort state - virtual Aborting* createAborting() const { return new Aborting; } + virtual Aborting* createAborting() const { + return new Aborting; + } //! Must be overridden if modifying the Pre-Arm state - virtual PreArming* createPreArming() const { return new PreArming; } + virtual PreArming* createPreArming() const { + return new PreArming; + } //! Must be overridden if modifying the Pre-Running state - virtual PreRunning* createPreRunning() const { return new PreRunning; } + virtual PreRunning* createPreRunning() const { + return new PreRunning; + } //! Signals for events sigslot::signal<> stateChangeSig; @@ -151,22 +228,23 @@ class TestObject { //! overriding and implementing them if needed. //! Preferable using threads as to not slow down //! the main thread. - virtual void onStateChange() {}; - virtual void onOSEM(ObjectSettingsType& osem){}; - virtual void onHEAB(HeabMessageDataType& heab) {}; - virtual void onTRAJ() {}; - virtual void onOSTM(ObjectCommandType& ostm) {}; - virtual void onSTRT(StartMessageType& strt) {}; + virtual void onStateChange() {} + virtual void onOSEM(ObjectSettingsType& osem) {} + virtual void onHEAB(HeabMessageDataType& heab) {} + virtual void onTRAJ() {} + virtual void onOSTM(ObjectCommandType& ostm) {} + virtual void onSTRT(StartMessageType& strt) {} std::chrono::milliseconds expectedHeartbeatPeriod = std::chrono::milliseconds(1000 / HEAB_FREQUENCY_HZ); - std::chrono::milliseconds monrPeriod = std::chrono::milliseconds(1000 / MONR_EXPECTED_FREQUENCY_HZ); - std::chrono::milliseconds heartbeatTimeout = 10*expectedHeartbeatPeriod; - std::chrono::milliseconds maxSafeNetworkDelay = std::chrono::milliseconds(200); + std::chrono::milliseconds monrPeriod = std::chrono::milliseconds(1000 / MONR_EXPECTED_FREQUENCY_HZ); + std::chrono::milliseconds heartbeatTimeout = 10 * expectedHeartbeatPeriod; + std::chrono::milliseconds maxSafeNetworkDelay = std::chrono::milliseconds(200); - //! Used to get estimated network delay + //! Used to get estimated network delay std::chrono::milliseconds getNetworkDelay(); ISO22133::State* state; + private: //! Initializer for commonalities of the constructs void initialize(); @@ -179,7 +257,7 @@ class TestObject { void checkHeabLoop(); //! MONR sending loop that should be run in its own thread. void sendMonrLoop(); - + //! Function for handling received ISO messages. Calls corresponding //! handler in the current state. int handleMessage(std::vector&); @@ -197,17 +275,20 @@ class TestObject { void setNetworkDelay(std::chrono::milliseconds); //! Get the Next message counter to send - char getNextSentMessageCounter() { return sentMessageCounter = (sentMessageCounter + 1) % 256; } + char getNextSentMessageCounter() { + return sentMessageCounter = (sentMessageCounter + 1) % 256; + } //! Check if the received message counter is correct and update regardless to the next expected void checkAndUpdateMessageCounter(const char receivedMessageCounter) { if (receivedMessageCounter != expectedMessageCounter) { - std::cout << "Message counter mismatch. Expected: " << (int)expectedMessageCounter << " Received: " << (int)receivedMessageCounter << std::endl; + std::cout << "Message counter mismatch. Expected: " << (int)expectedMessageCounter + << " Received: " << (int)receivedMessageCounter << std::endl; } expectedMessageCounter = (expectedMessageCounter + 1) % 256; } - sigslot::signal<>heabTimeout; + sigslot::signal<> heabTimeout; std::mutex recvMutex; std::mutex heabMutex; std::mutex netwrkDelayMutex; @@ -229,38 +310,37 @@ class TestObject { std::atomic speed; std::atomic objectSettings; std::atomic acceleration; - std::atomic driveDirection { OBJECT_DRIVE_DIRECTION_UNAVAILABLE }; - std::atomic objectState { ISO_OBJECT_STATE_UNKNOWN }; - std::atomic readyToArm { OBJECT_READY_TO_ARM_UNAVAILABLE }; + std::atomic driveDirection{OBJECT_DRIVE_DIRECTION_UNAVAILABLE}; + std::atomic objectState{ISO_OBJECT_STATE_UNKNOWN}; + std::atomic readyToArm{OBJECT_READY_TO_ARM_UNAVAILABLE}; std::atomic transmitterID; std::atomic receiverID; std::atomic expectedMessageCounter; std::atomic sentMessageCounter; - std::atomic socketsReceivedFromController { false }; - std::atomic errorState { 0 }; - std::atomic awaitingFirstHeab { true }; - std::atomic osemReceived { false }; - std::atomic on { true }; + std::atomic socketsReceivedFromController{false}; + std::atomic errorState{0}; + std::atomic awaitingFirstHeab{true}; + std::atomic osemReceived{false}; + std::atomic on{true}; std::chrono::milliseconds estimatedNetworkDelay = std::chrono::milliseconds(0); }; } // namespace ISO22133 namespace std::chrono { -template +template struct timeval to_timeval(Duration&& d) { std::chrono::seconds const sec = std::chrono::duration_cast(d); struct timeval tv; - tv.tv_sec = sec.count(); + tv.tv_sec = sec.count(); tv.tv_usec = std::chrono::duration_cast(d - sec).count(); return tv; } -} // namespace std::chrono +} // namespace std::chrono -inline bool operator< (const timeval &lhs, const timeval &rhs) { - return (lhs.tv_sec + lhs.tv_usec/1e6) < (rhs.tv_sec + rhs.tv_usec/1e6); +inline bool operator<(const timeval& lhs, const timeval& rhs) { + return (lhs.tv_sec < rhs.tv_sec) || ((lhs.tv_sec == rhs.tv_sec) && (lhs.tv_usec < rhs.tv_usec)); } -inline bool operator> (const timeval &lhs, const timeval &rhs) { - return (lhs.tv_sec + lhs.tv_usec/1e6) > (rhs.tv_sec + rhs.tv_usec/1e6); +inline bool operator>(const timeval& lhs, const timeval& rhs) { + return rhs < lhs; } - diff --git a/inc/iso22133state.hpp b/inc/iso22133state.hpp index 61be2ea..7b1c0a8 100644 --- a/inc/iso22133state.hpp +++ b/inc/iso22133state.hpp @@ -1,37 +1,36 @@ #pragma once -#include +#include +#include +#include #include #include -#include -#include -#include -extern "C"{ -#include "iso22133.h" +#include +extern "C" { #include "header.h" +#include "iso22133.h" } namespace ISO22133 { class TestObject; - // TODO this should be in iso22133.h // typedef ObjectStateType ObjectStateID; -typedef enum { - ISO_OBJECT_STATE_UNKNOWN = -1, - ISO_OBJECT_STATE_OFF = 0, - ISO_OBJECT_STATE_INIT = 1, - ISO_OBJECT_STATE_ARMED = 2, - ISO_OBJECT_STATE_DISARMED = 3, - ISO_OBJECT_STATE_RUNNING = 4, - ISO_OBJECT_STATE_POSTRUN = 5, +enum ObjectStateID { + ISO_OBJECT_STATE_UNKNOWN = -1, + ISO_OBJECT_STATE_OFF = 0, + ISO_OBJECT_STATE_INIT = 1, + ISO_OBJECT_STATE_ARMED = 2, + ISO_OBJECT_STATE_DISARMED = 3, + ISO_OBJECT_STATE_RUNNING = 4, + ISO_OBJECT_STATE_POSTRUN = 5, ISO_OBJECT_STATE_REMOTE_CONTROLLED = 6, - ISO_OBJECT_STATE_ABORTING = 7, - ISO_OBJECT_STATE_PRE_ARMING = 8, - ISO_OBJECT_STATE_PRE_RUNNING = 9 -} ObjectStateID; + ISO_OBJECT_STATE_ABORTING = 7, + ISO_OBJECT_STATE_PRE_ARMING = 8, + ISO_OBJECT_STATE_PRE_RUNNING = 9 +}; namespace Events { -typedef enum { +enum EventType { D, //!< system started L, //!< control center connection lost B, //!< control center connection established @@ -47,44 +46,40 @@ typedef enum { E, //!< internal error detected F, //!< disarm requested X //!< emergency stop reset -} EventType; - -static const std::map descriptions = { - {D, "system started"}, - {L, "control center connection lost"}, - {B, "control center connection established"}, - {H, "remote control requested"}, - {M, "arm requested"}, - {N, "arm completed"}, - {S, "start requested"}, - {T, "start completed"}, - {U, "soft stop requested"}, - {Y, "scenario completed"}, - {R, "object stopped"}, - {W, "emergency stop requested"}, - {E, "internal error detected"}, - {F, "disarm requested"}, - {X, "emergency stop reset"} }; -} +static const std::map descriptions = {{D, "system started"}, + {L, "control center connection lost"}, + {B, "control center connection established"}, + {H, "remote control requested"}, + {M, "arm requested"}, + {N, "arm completed"}, + {S, "start requested"}, + {T, "start completed"}, + {U, "soft stop requested"}, + {Y, "scenario completed"}, + {R, "object stopped"}, + {W, "emergency stop requested"}, + {E, "internal error detected"}, + {F, "disarm requested"}, + {X, "emergency stop reset"}}; + +} // namespace Events // TODO move to iso22133.h // and get rid of the std::map :( static const std::map stateNames = { - {ISO_OBJECT_STATE_UNKNOWN, "Unknown"}, - {ISO_OBJECT_STATE_OFF, "Off"}, - {ISO_OBJECT_STATE_INIT, "Init"}, - {ISO_OBJECT_STATE_ARMED, "Armed"}, - {ISO_OBJECT_STATE_DISARMED, "Disarmed"}, - {ISO_OBJECT_STATE_RUNNING, "Running"}, - {ISO_OBJECT_STATE_POSTRUN, "NormalStop"}, - {ISO_OBJECT_STATE_REMOTE_CONTROLLED, "RemoteControlled"}, - {ISO_OBJECT_STATE_ABORTING, "EmergencyStop"}, - {ISO_OBJECT_STATE_PRE_ARMING, "PreArming"}, - {ISO_OBJECT_STATE_PRE_RUNNING, "PreRunning"} -}; - + {ISO_OBJECT_STATE_UNKNOWN, "Unknown"}, + {ISO_OBJECT_STATE_OFF, "Off"}, + {ISO_OBJECT_STATE_INIT, "Init"}, + {ISO_OBJECT_STATE_ARMED, "Armed"}, + {ISO_OBJECT_STATE_DISARMED, "Disarmed"}, + {ISO_OBJECT_STATE_RUNNING, "Running"}, + {ISO_OBJECT_STATE_POSTRUN, "NormalStop"}, + {ISO_OBJECT_STATE_REMOTE_CONTROLLED, "RemoteControlled"}, + {ISO_OBJECT_STATE_ABORTING, "EmergencyStop"}, + {ISO_OBJECT_STATE_PRE_ARMING, "PreArming"}, + {ISO_OBJECT_STATE_PRE_RUNNING, "PreRunning"}}; /*! * \brief The State class is an abstract form of the states used in @@ -93,29 +88,33 @@ static const std::map stateNames = { */ class State { friend class TestObject; + public: State() {} virtual ~State() {} virtual ObjectStateID getStateID() const = 0; - std::string getName() const { return stateNames.at(getStateID()); } + std::string getName() const { + return stateNames.at(getStateID()); + } //! Handle events according to ISO22133 state change description void handleEvent(TestObject&, const Events::EventType); + protected: //! Will be called on transition, indended to be //! overridden by the inheriting class if necessary virtual void onEnter(TestObject&) {} virtual void onExit(TestObject&) {} - std::mutex eventMutex; + std::mutex eventMutex; //! When a message arrives, these methods are //! the 'front line' handlers. //! Handles ISO22133 required actions from contorl center - virtual void handleTRAJ(TestObject&,std::atomic&); - virtual void handleOSEM(TestObject&,ObjectSettingsType&); - virtual void handleOSTM(TestObject&,ObjectCommandType&); - virtual void handleSTRT(TestObject&,StartMessageType&); + virtual void handleTRAJ(TestObject&, std::atomic&); + virtual void handleOSEM(TestObject&, ObjectSettingsType&); + virtual void handleOSTM(TestObject&, ObjectCommandType&); + virtual void handleSTRT(TestObject&, StartMessageType&); #ifndef SWIG [[noreturn]] @@ -130,120 +129,171 @@ class State { class Unknown : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_UNKNOWN; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_UNKNOWN; + } + private: - void handleTRAJ(TestObject&, std::atomic&) final override { unexpectedMessageWarning("TRAJ"); } - void handleOSEM(TestObject&, ObjectSettingsType&) final override { unexpectedMessageWarning("OSEM"); } - void handleOSTM(TestObject&, ObjectCommandType&) final override { unexpectedMessageWarning("OSTM"); } - void handleSTRT(TestObject&, StartMessageType&) final override { unexpectedMessageWarning("STRT"); } + void handleTRAJ(TestObject&, std::atomic&) final override { + unexpectedMessageWarning("TRAJ"); + } + void handleOSEM(TestObject&, ObjectSettingsType&) final override { + unexpectedMessageWarning("OSEM"); + } + void handleOSTM(TestObject&, ObjectCommandType&) final override { + unexpectedMessageWarning("OSTM"); + } + void handleSTRT(TestObject&, StartMessageType&) final override { + unexpectedMessageWarning("STRT"); + } }; class Off : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_OFF; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_OFF; + } + private: - void handleTRAJ(TestObject&, std::atomic&) final override { unexpectedMessageWarning("TRAJ"); } - void handleOSEM(TestObject&, ObjectSettingsType&) final override { unexpectedMessageWarning("OSEM"); } - void handleOSTM(TestObject&, ObjectCommandType&) final override { unexpectedMessageWarning("OSTM"); } - void handleSTRT(TestObject&, StartMessageType&) final override { unexpectedMessageWarning("STRT"); } + void handleTRAJ(TestObject&, std::atomic&) final override { + unexpectedMessageWarning("TRAJ"); + } + void handleOSEM(TestObject&, ObjectSettingsType&) final override { + unexpectedMessageWarning("OSEM"); + } + void handleOSTM(TestObject&, ObjectCommandType&) final override { + unexpectedMessageWarning("OSTM"); + } + void handleSTRT(TestObject&, StartMessageType&) final override { + unexpectedMessageWarning("STRT"); + } }; class Init : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_INIT; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_INIT; + } + private: - void handleTRAJ(TestObject&, std::atomic&) final override { unexpectedMessageWarning("TRAJ"); } - void handleOSEM(TestObject&, ObjectSettingsType&) final override { unexpectedMessageWarning("OSEM"); } - void handleOSTM(TestObject&, ObjectCommandType&) final override { unexpectedMessageWarning("OSTM"); } - void handleSTRT(TestObject&, StartMessageType&) final override { unexpectedMessageWarning("STRT"); } + void handleTRAJ(TestObject&, std::atomic&) final override { + unexpectedMessageWarning("TRAJ"); + } + void handleOSEM(TestObject&, ObjectSettingsType&) final override { + unexpectedMessageWarning("OSEM"); + } + void handleOSTM(TestObject&, ObjectCommandType&) final override { + unexpectedMessageWarning("OSTM"); + } + void handleSTRT(TestObject&, StartMessageType&) final override { + unexpectedMessageWarning("STRT"); + } }; class Armed : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_ARMED; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_ARMED; + } }; class Disarmed : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_DISARMED; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_DISARMED; + } virtual void onEnter(TestObject& obj); virtual void onExit(TestObject& obj); }; class Running : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_RUNNING; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_RUNNING; + } }; class PostRun : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_POSTRUN; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_POSTRUN; + } }; class RemoteControlled : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_REMOTE_CONTROLLED; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_REMOTE_CONTROLLED; + } }; class Aborting : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_ABORTING; } + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_ABORTING; + } }; class PreArming : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_PRE_ARMING; } - virtual void onEnter(TestObject& obj) {this->handleEvent(obj, Events::N);} + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_PRE_ARMING; + } + virtual void onEnter(TestObject& obj) { + this->handleEvent(obj, Events::N); + } }; class PreRunning : public State { public: - virtual ObjectStateID getStateID() const final override { return ISO_OBJECT_STATE_PRE_RUNNING; } - virtual void onEnter(TestObject& obj) {this->handleEvent(obj, Events::T);} + virtual ObjectStateID getStateID() const final override { + return ISO_OBJECT_STATE_PRE_RUNNING; + } + virtual void onEnter(TestObject& obj) { + this->handleEvent(obj, Events::T); + } }; - -typedef struct { +struct Transition { ObjectStateID source; Events::EventType event; ObjectStateID target; -} Transition; +}; -inline bool operator< (const Transition &lhs, const Transition &rhs){ +inline bool operator<(const Transition& lhs, const Transition& rhs) { return std::tie(lhs.source, lhs.event, lhs.target) < std::tie(rhs.source, rhs.event, rhs.target); } static const std::set language = { - {ISO_OBJECT_STATE_OFF, Events::D, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_INIT, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_INIT, Events::B, ISO_OBJECT_STATE_DISARMED}, - {ISO_OBJECT_STATE_ARMED, Events::E, ISO_OBJECT_STATE_DISARMED}, - {ISO_OBJECT_STATE_ARMED, Events::F, ISO_OBJECT_STATE_DISARMED}, - {ISO_OBJECT_STATE_ARMED, Events::W, ISO_OBJECT_STATE_ABORTING}, - {ISO_OBJECT_STATE_ARMED, Events::S, ISO_OBJECT_STATE_PRE_RUNNING}, - {ISO_OBJECT_STATE_ARMED, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_DISARMED, Events::M, ISO_OBJECT_STATE_PRE_ARMING}, - {ISO_OBJECT_STATE_DISARMED, Events::H, ISO_OBJECT_STATE_REMOTE_CONTROLLED}, - {ISO_OBJECT_STATE_DISARMED, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_DISARMED, Events::W, ISO_OBJECT_STATE_ABORTING}, - {ISO_OBJECT_STATE_RUNNING, Events::U, ISO_OBJECT_STATE_POSTRUN}, - {ISO_OBJECT_STATE_RUNNING, Events::Y, ISO_OBJECT_STATE_POSTRUN}, - {ISO_OBJECT_STATE_RUNNING, Events::W, ISO_OBJECT_STATE_ABORTING}, - {ISO_OBJECT_STATE_RUNNING, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_POSTRUN, Events::R, ISO_OBJECT_STATE_DISARMED}, - {ISO_OBJECT_STATE_POSTRUN, Events::W, ISO_OBJECT_STATE_ABORTING}, - {ISO_OBJECT_STATE_POSTRUN, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_REMOTE_CONTROLLED, Events::F, ISO_OBJECT_STATE_DISARMED}, - {ISO_OBJECT_STATE_REMOTE_CONTROLLED, Events::W, ISO_OBJECT_STATE_ABORTING}, - {ISO_OBJECT_STATE_REMOTE_CONTROLLED, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_ABORTING, Events::W, ISO_OBJECT_STATE_ABORTING}, - {ISO_OBJECT_STATE_ABORTING, Events::X, ISO_OBJECT_STATE_DISARMED}, - {ISO_OBJECT_STATE_ABORTING, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_PRE_ARMING, Events::N, ISO_OBJECT_STATE_ARMED}, - {ISO_OBJECT_STATE_PRE_ARMING, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_PRE_ARMING, Events::W, ISO_OBJECT_STATE_ABORTING}, - {ISO_OBJECT_STATE_PRE_RUNNING, Events::T, ISO_OBJECT_STATE_RUNNING}, - {ISO_OBJECT_STATE_PRE_RUNNING, Events::L, ISO_OBJECT_STATE_INIT}, - {ISO_OBJECT_STATE_PRE_RUNNING, Events::W, ISO_OBJECT_STATE_ABORTING} -}; + {ISO_OBJECT_STATE_OFF, Events::D, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_INIT, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_INIT, Events::B, ISO_OBJECT_STATE_DISARMED}, + {ISO_OBJECT_STATE_ARMED, Events::E, ISO_OBJECT_STATE_DISARMED}, + {ISO_OBJECT_STATE_ARMED, Events::F, ISO_OBJECT_STATE_DISARMED}, + {ISO_OBJECT_STATE_ARMED, Events::W, ISO_OBJECT_STATE_ABORTING}, + {ISO_OBJECT_STATE_ARMED, Events::S, ISO_OBJECT_STATE_PRE_RUNNING}, + {ISO_OBJECT_STATE_ARMED, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_DISARMED, Events::M, ISO_OBJECT_STATE_PRE_ARMING}, + {ISO_OBJECT_STATE_DISARMED, Events::H, ISO_OBJECT_STATE_REMOTE_CONTROLLED}, + {ISO_OBJECT_STATE_DISARMED, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_DISARMED, Events::W, ISO_OBJECT_STATE_ABORTING}, + {ISO_OBJECT_STATE_RUNNING, Events::U, ISO_OBJECT_STATE_POSTRUN}, + {ISO_OBJECT_STATE_RUNNING, Events::Y, ISO_OBJECT_STATE_POSTRUN}, + {ISO_OBJECT_STATE_RUNNING, Events::W, ISO_OBJECT_STATE_ABORTING}, + {ISO_OBJECT_STATE_RUNNING, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_POSTRUN, Events::R, ISO_OBJECT_STATE_DISARMED}, + {ISO_OBJECT_STATE_POSTRUN, Events::W, ISO_OBJECT_STATE_ABORTING}, + {ISO_OBJECT_STATE_POSTRUN, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_REMOTE_CONTROLLED, Events::F, ISO_OBJECT_STATE_DISARMED}, + {ISO_OBJECT_STATE_REMOTE_CONTROLLED, Events::W, ISO_OBJECT_STATE_ABORTING}, + {ISO_OBJECT_STATE_REMOTE_CONTROLLED, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_ABORTING, Events::W, ISO_OBJECT_STATE_ABORTING}, + {ISO_OBJECT_STATE_ABORTING, Events::X, ISO_OBJECT_STATE_DISARMED}, + {ISO_OBJECT_STATE_ABORTING, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_PRE_ARMING, Events::N, ISO_OBJECT_STATE_ARMED}, + {ISO_OBJECT_STATE_PRE_ARMING, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_PRE_ARMING, Events::W, ISO_OBJECT_STATE_ABORTING}, + {ISO_OBJECT_STATE_PRE_RUNNING, Events::T, ISO_OBJECT_STATE_RUNNING}, + {ISO_OBJECT_STATE_PRE_RUNNING, Events::L, ISO_OBJECT_STATE_INIT}, + {ISO_OBJECT_STATE_PRE_RUNNING, Events::W, ISO_OBJECT_STATE_ABORTING}}; } // end namespace ISO22133 diff --git a/inc/tcpServer.hpp b/inc/tcpServer.hpp index b8db4d2..173d60a 100644 --- a/inc/tcpServer.hpp +++ b/inc/tcpServer.hpp @@ -4,7 +4,7 @@ #include #include -// These namespace declarations does not work in SWIG :( +// These namespace declarations does not work in SWIG :( // using namespace boost::asio; // using boost::asio::ip::tcp; @@ -13,21 +13,21 @@ * */ class TcpServer { - public: +public: TcpServer(std::string ip, uint32_t port) : - acceptor(context, boost::asio::ip::tcp::endpoint(boost::asio::ip::address_v4::from_string(ip), port)), - socket(context), - acceptIncoming(true) { + acceptor(context, boost::asio::ip::tcp::endpoint(boost::asio::ip::address_v4::from_string(ip), port)), + socket(context), + acceptIncoming(true) { setBufferSize(defaultBufferSize); - }; + } - TcpServer(int sock) : - socket(context), - acceptor(context), - acceptIncoming(false) { + TcpServer(int sock) : + socket(context), + acceptor(context), + acceptIncoming(false) { setBufferSize(defaultBufferSize); socket.assign(boost::asio::ip::tcp::v4(), sock); - }; + } virtual ~TcpServer() = default; void disconnect() { @@ -35,7 +35,7 @@ class TcpServer { if (this->acceptIncoming) { acceptor.cancel(); } - if (socket.is_open()){ + if (socket.is_open()) { socket.shutdown(boost::asio::socket_base::shutdown_both); socket.close(); } @@ -43,7 +43,7 @@ class TcpServer { } catch (boost::system::system_error& e) { std::cerr << "Error when closing socket: " << e.what() << std::endl; } - }; + } void acceptConnection() { if (!this->acceptIncoming) { @@ -65,12 +65,20 @@ class TcpServer { context.restart(); } - void setBufferSize(size_t size) { dataBuffer.resize(size); }; - size_t getBuffferSize() const { return dataBuffer.size(); }; + void setBufferSize(size_t size) { + dataBuffer.resize(size); + } + size_t getBuffferSize() const { + return dataBuffer.size(); + } - boost::asio::ip::tcp::endpoint getEndPoint() const { return socket.remote_endpoint(); }; + boost::asio::ip::tcp::endpoint getEndPoint() const { + return socket.remote_endpoint(); + } - bool isOpen() const { return socket.is_open(); }; + bool isOpen() const { + return socket.is_open(); + } std::vector receive() { try { @@ -89,24 +97,23 @@ class TcpServer { throw e; } } - }; + } void send(std::vector data, size_t nbytes) { std::vector sendBuffer(data); sendBuffer.resize(nbytes); - socket.async_send(boost::asio::buffer(sendBuffer, nbytes), - [](const boost::system::error_code& error, std::size_t bytes_transferred) { - if (error) { - // Sending failed, handle the error - // Print the error message for example - std::cerr << "Send error: " << error.message() << std::endl; - throw boost::system::system_error(boost::asio::error::eof); - } - } - ); - }; + socket.async_send(boost::asio::buffer(sendBuffer, nbytes), + [](const boost::system::error_code& error, std::size_t bytes_transferred) { + if (error) { + // Sending failed, handle the error + // Print the error message for example + std::cerr << "Send error: " << error.message() << std::endl; + throw boost::system::system_error(boost::asio::error::eof); + } + }); + } - private: +private: std::vector dataBuffer; size_t defaultBufferSize = 4096; bool acceptIncoming; diff --git a/inc/trajDecoder.hpp b/inc/trajDecoder.hpp index fa5b666..d73fc30 100644 --- a/inc/trajDecoder.hpp +++ b/inc/trajDecoder.hpp @@ -1,35 +1,39 @@ #pragma once +#include +#include #include #include -#include -#include #include "iso22133.h" #include "traj.h" /** * @brief Class for decoding TRAJ messages. Stores TRAJ data and - * keeps track of unhandled bytes. + * keeps track of unhandled bytes. */ class TrajDecoder { -public: - TrajDecoder(bool debug) : debug(debug), expectingTRAJPoints(false) {}; - TrajDecoder() : debug(false), expectingTRAJPoints(false) {}; - ssize_t DecodeTRAJ(std::vector&, bool debug = false); - bool ExpectingTrajPoints() const { return this->expectingTRAJPoints; } - TrajectoryHeaderType getTrajHeader() const; - std::vector getTraj() const; +public: + TrajDecoder(bool debug) : + debug(debug), + expectingTRAJPoints(false) {} + TrajDecoder() : + debug(false), + expectingTRAJPoints(false) {} + ssize_t DecodeTRAJ(std::vector&, bool debug = false); + bool ExpectingTrajPoints() const { + return this->expectingTRAJPoints; + } + TrajectoryHeaderType getTrajHeader() const; + std::vector getTraj() const; private: - mutable std::mutex guard; - bool debug; - std::atomic expectingTRAJPoints; - int nPointsHandled = 0; - std::vector unhandledBytes; - std::vector copiedData; - std::vector trajectoryWaypoints; - TrajectoryHeaderType trajecoryHeader; + mutable std::mutex guard; + bool debug; + std::atomic expectingTRAJPoints; + int nPointsHandled = 0; + std::vector unhandledBytes; + std::vector copiedData; + std::vector trajectoryWaypoints; + TrajectoryHeaderType trajecoryHeader; }; - - diff --git a/inc/udpServer.hpp b/inc/udpServer.hpp index d98a72e..a9e4812 100644 --- a/inc/udpServer.hpp +++ b/inc/udpServer.hpp @@ -5,7 +5,7 @@ #include #include -// These namespace declarations does not work in SWIG :( +// These namespace declarations does not work in SWIG :( // using namespace boost::asio; // using ip::udp; @@ -14,29 +14,31 @@ * */ class UdpServer { - public: - UdpServer(const std::string &ip, uint32_t port) : - socket(context, boost::asio::ip::udp::endpoint(boost::asio::ip::address_v4::from_string(ip), port)) { +public: + UdpServer(const std::string& ip, uint32_t port) : + socket(context, boost::asio::ip::udp::endpoint(boost::asio::ip::address_v4::from_string(ip), port)) { setBufferSize(defaultBufferSize); - }; + } UdpServer() : - socket(context) { + socket(context) { setBufferSize(defaultBufferSize); - }; + } - void setEndpoint(int native_socket, boost::asio::ip::udp::endpoint &ep) { + void setEndpoint(int native_socket, boost::asio::ip::udp::endpoint& ep) { socket.assign(boost::asio::ip::udp::v4(), native_socket); senderEndpoint = ep; - }; + } - void setBufferSize(size_t size) { dataBuffer.resize(size); }; + void setBufferSize(size_t size) { + dataBuffer.resize(size); + } void disconnect() { // This call may throw but shutdown is still successful. try { socket.shutdown(boost::asio::socket_base::shutdown_receive); - } catch (const boost::system::system_error& e) {} - }; + } catch (const boost::system::system_error& e) {} + } std::vector receive() { try { @@ -51,7 +53,7 @@ class UdpServer { std::cerr << ss.str(); throw e; } - }; + } size_t send(std::vector data, size_t nbytes) { try { @@ -65,9 +67,9 @@ class UdpServer { std::cerr << ss.str(); throw e; } - }; + } - private: +private: std::vector dataBuffer; size_t defaultBufferSize = 4096; diff --git a/src/iso22133object.cpp b/src/iso22133object.cpp index 5a62441..94c5e9b 100644 --- a/src/iso22133object.cpp +++ b/src/iso22133object.cpp @@ -1,26 +1,25 @@ #include #include +#include "defines.h" // From ISO22133 lib #include "iso22133object.hpp" #include "iso22133state.hpp" -#include "defines.h" // From ISO22133 lib #define TCP_BUFFER_SIZE 1024 #define UDP_BUFFER_SIZE 1024 - namespace ISO22133 { -TestObject::TestObject(const std::string& listenIP) - : name("myTestObject"), - trajDecoder(), - ctrlChannel(listenIP, ISO_22133_DEFAULT_OBJECT_TCP_PORT), - processChannel(listenIP, ISO_22133_OBJECT_UDP_PORT), - transmitterID(TRANSMITTER_ID_UNAVAILABLE_VALUE), - receiverID(DEFAULT_CONTROL_CENTER_ID), - expectedMessageCounter(0), - sentMessageCounter(0), - socketsReceivedFromController(false), - on(true) { +TestObject::TestObject(const std::string& listenIP) : + name("myTestObject"), + trajDecoder(), + ctrlChannel(listenIP, ISO_22133_DEFAULT_OBJECT_TCP_PORT), + processChannel(listenIP, ISO_22133_OBJECT_UDP_PORT), + transmitterID(TRANSMITTER_ID_UNAVAILABLE_VALUE), + receiverID(DEFAULT_CONTROL_CENTER_ID), + expectedMessageCounter(0), + sentMessageCounter(0), + socketsReceivedFromController(false), + on(true) { this->initialize(); this->startHandleTCP(); this->startHEABCheck(); @@ -28,17 +27,17 @@ TestObject::TestObject(const std::string& listenIP) this->startSendMonr(); } -TestObject::TestObject(int tcpSocketNativeHandle) - : name("myTestObject"), - trajDecoder(), - ctrlChannel(tcpSocketNativeHandle), - processChannel(), - transmitterID(TRANSMITTER_ID_UNAVAILABLE_VALUE), - receiverID(DEFAULT_CONTROL_CENTER_ID), - expectedMessageCounter(0), - sentMessageCounter(0), - socketsReceivedFromController(true), - on(true) { +TestObject::TestObject(int tcpSocketNativeHandle) : + name("myTestObject"), + trajDecoder(), + ctrlChannel(tcpSocketNativeHandle), + processChannel(), + transmitterID(TRANSMITTER_ID_UNAVAILABLE_VALUE), + receiverID(DEFAULT_CONTROL_CENTER_ID), + expectedMessageCounter(0), + sentMessageCounter(0), + socketsReceivedFromController(true), + on(true) { this->initialize(); this->startHEABCheck(); } @@ -48,14 +47,14 @@ void TestObject::initialize() { SpeedType initSpd; AccelerationType initAcc; TestModeType initTm; - localIP = "0.0.0.0"; - initPos.isHeadingValid = false; - initPos.isPositionValid = false; - initSpd.isLateralValid = false; + localIP = "0.0.0.0"; + initPos.isHeadingValid = false; + initPos.isPositionValid = false; + initSpd.isLateralValid = false; initSpd.isLongitudinalValid = false; - initAcc.isLateralValid = false; + initAcc.isLateralValid = false; initAcc.isLongitudinalValid = false; - initTm = TEST_MODE_UNAVAILABLE; + initTm = TEST_MODE_UNAVAILABLE; this->setPosition(initPos); this->setSpeed(initSpd); this->setAcceleration(initAcc); @@ -94,24 +93,23 @@ void TestObject::shutdown() { void TestObject::disconnect() { try { - ctrlChannel.disconnect(); // Close TCP socket + ctrlChannel.disconnect(); // Close TCP socket } catch (boost::system::system_error& e) { std::cerr << "TCP socket close error: " << e.what() << '\n'; } if (!this->socketsReceivedFromController) { - processChannel.disconnect(); // Close UDP socket - } - else { + processChannel.disconnect(); // Close UDP socket + } else { this->on = false; } - awaitingFirstHeab = true; // Reset HEAB timeout check + awaitingFirstHeab = true; // Reset HEAB timeout check } -MessageHeaderType *TestObject::populateMessageHeader(MessageHeaderType *header) { - memset(header, 0, sizeof(MessageHeaderType)); - header->transmitterID = this->transmitterID; - header->receiverID = this->receiverID; +MessageHeaderType* TestObject::populateMessageHeader(MessageHeaderType* header) { + std::memset(header, 0, sizeof(MessageHeaderType)); + header->transmitterID = this->transmitterID; + header->receiverID = this->receiverID; header->messageCounter = this->sentMessageCounter++; return header; } @@ -120,8 +118,8 @@ void TestObject::receiveTCP() { std::stringstream ss; while (this->on) { - // socketsReceivedFromController means that the TCP socket was - // created by a controlling system that provides an already + // socketsReceivedFromController means that the TCP socket was + // created by a controlling system that provides an already // open and ready to communicate socket if (!this->socketsReceivedFromController) { ss.str(std::string()); @@ -138,7 +136,7 @@ void TestObject::receiveTCP() { } ss.str(std::string()); ss << "TCP connection to ATOS running at " << ctrlChannel.getEndPoint().address().to_string() - << " established." << std::endl; + << " established." << std::endl; std::cout << ss.str(); state->handleEvent(*this, ISO22133::Events::B); } @@ -169,16 +167,28 @@ void TestObject::receiveTCP() { void TestObject::sendMONR(bool debug) { std::vector buffer(UDP_BUFFER_SIZE); struct timeval time; - auto nanos = std::chrono::system_clock::now().time_since_epoch().count(); - time.tv_sec = nanos / 1e9; + auto nanos = std::chrono::system_clock::now().time_since_epoch().count(); + time.tv_sec = nanos / 1e9; time.tv_usec = nanos / 1e3 - time.tv_sec * 1e6; MessageHeaderType header; - auto nBytesWritten = encodeMONRMessage(this->populateMessageHeader(&header), &time, this->position, this->speed, this->acceleration, - this->driveDirection, this->state->getStateID(), this->readyToArm, - this->errorState, 0x0000, buffer.data(), buffer.size(), debug); - - if (nBytesWritten < 0) { throw(std::invalid_argument("Failed to encode MONR data"));} + auto nBytesWritten = encodeMONRMessage(this->populateMessageHeader(&header), + &time, + this->position, + this->speed, + this->acceleration, + this->driveDirection, + this->state->getStateID(), + this->readyToArm, + this->errorState, + 0x0000, + buffer.data(), + buffer.size(), + debug); + + if (nBytesWritten < 0) { + throw(std::invalid_argument("Failed to encode MONR data")); + } processChannel.send(buffer, static_cast(nBytesWritten)); } @@ -186,24 +196,25 @@ void TestObject::sendGREM(HeaderType msgHeader, GeneralResponseStatus responseCo std::vector buffer(TCP_BUFFER_SIZE); GeneralResponseMessageType grem; - grem.receivedHeaderTransmitterID = msgHeader.transmitterID; - grem.receivedHeaderMessageID = msgHeader.messageID; + grem.receivedHeaderTransmitterID = msgHeader.transmitterID; + grem.receivedHeaderMessageID = msgHeader.messageID; grem.receivedHeaderMessageCounter = msgHeader.messageCounter; - grem.responseCode = responseCode; + grem.responseCode = responseCode; MessageHeaderType header; - auto nBytesWritten = encodeGREMMessage(this->populateMessageHeader(&header), &grem, buffer.data(), buffer.size(), debug); + auto nBytesWritten = + encodeGREMMessage(this->populateMessageHeader(&header), &grem, buffer.data(), buffer.size(), debug); - if (nBytesWritten < 0) { throw(std::invalid_argument("Failed to encode GREM data"));} + if (nBytesWritten < 0) { + throw(std::invalid_argument("Failed to encode GREM data")); + } ctrlChannel.send(buffer, static_cast(nBytesWritten)); } void TestObject::sendMonrLoop() { while (this->on) { // Only send monr connection has been established if transmitterID has been set by an OSEM message - if (ctrlChannel.isOpen() && - !awaitingFirstHeab && - this->transmitterID != TRANSMITTER_ID_UNAVAILABLE_VALUE) { + if (ctrlChannel.isOpen() && !awaitingFirstHeab && this->transmitterID != TRANSMITTER_ID_UNAVAILABLE_VALUE) { sendMONR(); } auto t = std::chrono::steady_clock::now(); @@ -269,40 +280,38 @@ void TestObject::onHeabTimeout() { this->state->handleEvent(*this, Events::L); } -int TestObject::handleTCPMessage(char *buffer, int bufferLen) { +int TestObject::handleTCPMessage(char* buffer, int bufferLen) { state->handleEvent(*this, ISO22133::Events::B); int num_bytes_handled = this->handleMessage(buffer, bufferLen); this->startHandleTCP(); return num_bytes_handled; } -int TestObject::handleUDPMessage(char *buffer, int bufferLen, int udpSocket, char *addr, const uint32_t port) { +int TestObject::handleUDPMessage(char* buffer, int bufferLen, int udpSocket, char* addr, const uint32_t port) { if (awaitingFirstHeab) { - boost::asio::ip::udp::endpoint udpEp = boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(addr), port); - processChannel.setEndpoint(udpSocket, udpEp);; + boost::asio::ip::udp::endpoint udpEp = + boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(addr), port); + processChannel.setEndpoint(udpSocket, udpEp); + ; this->startSendMonr(); } return this->handleMessage(buffer, bufferLen); } -int TestObject::handleMessage(char *buffer, int bufferLen) { - std::vector data; - for (int i = 0; i < bufferLen; i++) { - data.push_back(buffer[i]); - } +int TestObject::handleMessage(char* buffer, int bufferLen) { + std::vector data(buffer, buffer + bufferLen); return handleMessage(data); } int TestObject::handleMessage(std::vector& dataBuffer) { - std::lock_guard lock(this->recvMutex); // Both TCP and UDP threads end up in here + std::lock_guard lock(this->recvMutex); // Both TCP and UDP threads end up in here int bytesHandled = 0; - int debug = 0; + int debug = 0; struct timeval currentTime; currentTime = std::chrono::to_timeval(std::chrono::system_clock::now().time_since_epoch()); - HeaderType msgHeader; enum ISOMessageReturnValue retval = decodeISOHeader(dataBuffer.data(), dataBuffer.size(), &msgHeader, debug); if (retval == MESSAGE_OK) { @@ -322,59 +331,58 @@ int TestObject::handleMessage(std::vector& dataBuffer) { expectedMessageCounter = msgHeader.messageCounter + 1; switch (msgHeader.messageID) { - case MESSAGE_ID_TRAJ: - bytesHandled = this->trajDecoder.DecodeTRAJ(dataBuffer, false); - if (bytesHandled < 0) { - throw std::invalid_argument("Error decoding TRAJ"); - } - if (!this->trajDecoder.ExpectingTrajPoints()) { - this->state->handleTRAJ(*this, lastReceivedMsgHeader); - } - break; - case MESSAGE_ID_OSEM: - ObjectSettingsType OSEMstruct; - bytesHandled = decodeOSEMMessage(&OSEMstruct, dataBuffer.data(), dataBuffer.size(), debug); - if (bytesHandled < 0) { - throw std::invalid_argument("Error decoding OSEM"); - } - this->state->handleOSEM(*this, OSEMstruct); - break; - - case MESSAGE_ID_OSTM: - ObjectCommandType OSTMdata; - bytesHandled = decodeOSTMMessage(dataBuffer.data(), dataBuffer.size(), &OSTMdata, debug); - if (bytesHandled < 0) { - throw std::invalid_argument("Error decoding OSTM"); - } - this->state->handleOSTM(*this, OSTMdata); - break; - - case MESSAGE_ID_STRT: - StartMessageType STRTdata; - bytesHandled - = decodeSTRTMessage(dataBuffer.data(), dataBuffer.size(), ¤tTime, &STRTdata, debug); - if (bytesHandled < 0) { - throw std::invalid_argument("Error decoding STRT"); - } - this->state->handleSTRT(*this, STRTdata); - break; - - case MESSAGE_ID_HEAB: - HeabMessageDataType HEABdata; - bytesHandled = decodeHEABMessage(dataBuffer.data(), dataBuffer.size(), currentTime, &HEABdata, debug); - if (bytesHandled < 0) { - throw std::invalid_argument("Error decoding HEAB"); - } - this->handleHEAB(HEABdata); - break; - default: - bytesHandled = handleVendorSpecificMessage(msgHeader.messageID, dataBuffer); - if (bytesHandled < 0) { - throw std::invalid_argument(std::string("Unable to decode ISO-22133 message with MsgID ") - + std::to_string(msgHeader.messageID)); - } - bytesHandled = static_cast(dataBuffer.size()); - break; + case MESSAGE_ID_TRAJ: + bytesHandled = this->trajDecoder.DecodeTRAJ(dataBuffer, false); + if (bytesHandled < 0) { + throw std::invalid_argument("Error decoding TRAJ"); + } + if (!this->trajDecoder.ExpectingTrajPoints()) { + this->state->handleTRAJ(*this, lastReceivedMsgHeader); + } + break; + case MESSAGE_ID_OSEM: + ObjectSettingsType OSEMstruct; + bytesHandled = decodeOSEMMessage(&OSEMstruct, dataBuffer.data(), dataBuffer.size(), debug); + if (bytesHandled < 0) { + throw std::invalid_argument("Error decoding OSEM"); + } + this->state->handleOSEM(*this, OSEMstruct); + break; + + case MESSAGE_ID_OSTM: + ObjectCommandType OSTMdata; + bytesHandled = decodeOSTMMessage(dataBuffer.data(), dataBuffer.size(), &OSTMdata, debug); + if (bytesHandled < 0) { + throw std::invalid_argument("Error decoding OSTM"); + } + this->state->handleOSTM(*this, OSTMdata); + break; + + case MESSAGE_ID_STRT: + StartMessageType STRTdata; + bytesHandled = decodeSTRTMessage(dataBuffer.data(), dataBuffer.size(), ¤tTime, &STRTdata, debug); + if (bytesHandled < 0) { + throw std::invalid_argument("Error decoding STRT"); + } + this->state->handleSTRT(*this, STRTdata); + break; + + case MESSAGE_ID_HEAB: + HeabMessageDataType HEABdata; + bytesHandled = decodeHEABMessage(dataBuffer.data(), dataBuffer.size(), currentTime, &HEABdata, debug); + if (bytesHandled < 0) { + throw std::invalid_argument("Error decoding HEAB"); + } + this->handleHEAB(HEABdata); + break; + default: + bytesHandled = handleVendorSpecificMessage(msgHeader.messageID, dataBuffer); + if (bytesHandled < 0) { + throw std::invalid_argument(std::string("Unable to decode ISO-22133 message with MsgID ") + + std::to_string(msgHeader.messageID)); + } + bytesHandled = static_cast(dataBuffer.size()); + break; } return bytesHandled; } @@ -392,35 +400,35 @@ void TestObject::handleHEAB(HeabMessageDataType& heab) { // Check network delay: difference between // timestamp in HEAB and local time - // Requires the system clocks of ATOS + // Requires the system clocks of ATOS // and object to be synced!! - auto heabTime = seconds(heab.dataTimestamp.tv_sec) + microseconds(heab.dataTimestamp.tv_usec); + auto heabTime = seconds(heab.dataTimestamp.tv_sec) + microseconds(heab.dataTimestamp.tv_usec); auto networkDelay = system_clock::now().time_since_epoch() - heabTime; setNetworkDelay(duration_cast(networkDelay)); if (networkDelay > maxSafeNetworkDelay) { std::stringstream ss; - ss << "Network delay of " << duration_cast(networkDelay).count() - << " ms exceeds safe limit of " << maxSafeNetworkDelay.count() << " ms." << std::endl; + ss << "Network delay of " << duration_cast(networkDelay).count() << " ms exceeds safe limit of " + << maxSafeNetworkDelay.count() << " ms." << std::endl; std::cerr << ss.str(); // TODO: do something } std::scoped_lock lock(heabMutex); - lastHeabTime = steady_clock::now(); + lastHeabTime = steady_clock::now(); awaitingFirstHeab = false; switch (heab.controlCenterStatus) { - case CONTROL_CENTER_STATUS_NORMAL_STOP: - this->state->handleEvent(*this, ISO22133::Events::U); - break; - case CONTROL_CENTER_STATUS_ABORT: - this->state->handleEvent(*this, ISO22133::Events::W); - break; - case CONTROL_CENTER_STATUS_TEST_DONE: - this->state->handleEvent(*this, ISO22133::Events::Y); - break; - default: - break; + case CONTROL_CENTER_STATUS_NORMAL_STOP: + this->state->handleEvent(*this, ISO22133::Events::U); + break; + case CONTROL_CENTER_STATUS_ABORT: + this->state->handleEvent(*this, ISO22133::Events::W); + break; + case CONTROL_CENTER_STATUS_TEST_DONE: + this->state->handleEvent(*this, ISO22133::Events::Y); + break; + default: + break; } ccStatus = heab.controlCenterStatus; return; @@ -439,4 +447,4 @@ void TestObject::setNetworkDelay(std::chrono::milliseconds delay) { estimatedNetworkDelay = delay; } -} // namespace ISO22133 +} // namespace ISO22133 diff --git a/src/iso22133state.cpp b/src/iso22133state.cpp index e2a6b56..986e9d0 100644 --- a/src/iso22133state.cpp +++ b/src/iso22133state.cpp @@ -11,13 +11,12 @@ */ void ISO22133::State::handleEvent(TestObject& obj, const ISO22133::Events::EventType event) { std::scoped_lock lock(eventMutex); - auto transition - = std::find_if(language.begin(), language.end(), [&event, this](const ISO22133::Transition& tr) { - return event == tr.event && this->getStateID() == tr.source; - }); + auto transition = std::find_if(language.begin(), language.end(), [&event, this](const ISO22133::Transition& tr) { + return event == tr.event && this->getStateID() == tr.source; + }); if (transition == language.end()) { - throw std::runtime_error(std::string("Unexpected event '") + Events::descriptions.at(event) - + "' in state " + this->getName()); + throw std::runtime_error(std::string("Unexpected event '") + Events::descriptions.at(event) + "' in state " + + this->getName()); } if (transition->source == transition->target) { return; @@ -28,41 +27,41 @@ void ISO22133::State::handleEvent(TestObject& obj, const ISO22133::Events::Event State* temp = obj.state; switch (transition->target) { - case ISO_OBJECT_STATE_OFF: - obj.state = obj.createOff(); - break; - case ISO_OBJECT_STATE_INIT: - obj.state = obj.createInit(); - break; - case ISO_OBJECT_STATE_ARMED: - obj.state = obj.createArmed(); - break; - case ISO_OBJECT_STATE_DISARMED: - obj.state = obj.createDisarmed(); - break; - case ISO_OBJECT_STATE_RUNNING: - obj.state = obj.createRunning(); - break; - case ISO_OBJECT_STATE_POSTRUN: - obj.state = obj.createPostRun(); - break; - case ISO_OBJECT_STATE_REMOTE_CONTROLLED: - obj.state = obj.createRemoteControlled(); - break; - case ISO_OBJECT_STATE_ABORTING: - obj.state = obj.createAborting(); - obj.handleAbort(); - break; - case ISO_OBJECT_STATE_PRE_ARMING: - obj.state = obj.createPreArming(); - break; - case ISO_OBJECT_STATE_PRE_RUNNING: - obj.state = obj.createPreRunning(); - break; - case ISO_OBJECT_STATE_UNKNOWN: - default: - obj.state = obj.createUnknown(); - break; + case ISO_OBJECT_STATE_OFF: + obj.state = obj.createOff(); + break; + case ISO_OBJECT_STATE_INIT: + obj.state = obj.createInit(); + break; + case ISO_OBJECT_STATE_ARMED: + obj.state = obj.createArmed(); + break; + case ISO_OBJECT_STATE_DISARMED: + obj.state = obj.createDisarmed(); + break; + case ISO_OBJECT_STATE_RUNNING: + obj.state = obj.createRunning(); + break; + case ISO_OBJECT_STATE_POSTRUN: + obj.state = obj.createPostRun(); + break; + case ISO_OBJECT_STATE_REMOTE_CONTROLLED: + obj.state = obj.createRemoteControlled(); + break; + case ISO_OBJECT_STATE_ABORTING: + obj.state = obj.createAborting(); + obj.handleAbort(); + break; + case ISO_OBJECT_STATE_PRE_ARMING: + obj.state = obj.createPreArming(); + break; + case ISO_OBJECT_STATE_PRE_RUNNING: + obj.state = obj.createPreRunning(); + break; + case ISO_OBJECT_STATE_UNKNOWN: + default: + obj.state = obj.createUnknown(); + break; } delete temp; @@ -83,20 +82,20 @@ void ISO22133::State::handleOSTM(TestObject& obj, ObjectCommandType& ostm) { // after the handleEvent() calls obj.ostmSig(ostm); switch (ostm) { - case OBJECT_COMMAND_ARM: - this->handleEvent(obj, ISO22133::Events::M); - break; - case OBJECT_COMMAND_DISARM: - this->handleEvent(obj, ISO22133::Events::F); - break; - case OBJECT_COMMAND_REMOTE_CONTROL: - this->handleEvent(obj, ISO22133::Events::H); - break; - case OBJECT_COMMAND_ALL_CLEAR: - this->handleEvent(obj, ISO22133::Events::X); - break; - default: - break; + case OBJECT_COMMAND_ARM: + this->handleEvent(obj, ISO22133::Events::M); + break; + case OBJECT_COMMAND_DISARM: + this->handleEvent(obj, ISO22133::Events::F); + break; + case OBJECT_COMMAND_REMOTE_CONTROL: + this->handleEvent(obj, ISO22133::Events::H); + break; + case OBJECT_COMMAND_ALL_CLEAR: + this->handleEvent(obj, ISO22133::Events::X); + break; + default: + break; } } @@ -107,11 +106,11 @@ void ISO22133::State::handleOSTM(TestObject& obj, ObjectCommandType& ostm) { * @param osem struct ObjectSettingsType */ void ISO22133::State::handleOSEM(TestObject& obj, ObjectSettingsType& osem) { - obj.origin = osem.coordinateSystemOrigin; + obj.origin = osem.coordinateSystemOrigin; obj.transmitterID = osem.desiredID.transmitter; - obj.receiverID = osem.desiredID.controlCentre; + obj.receiverID = osem.desiredID.controlCentre; - std::stringstream msg; // Remove risk of not printing the whole message due to threading + std::stringstream msg; // Remove risk of not printing the whole message due to threading msg << "Got OSEM - set transmitter ID to " << obj.transmitterID << std::endl; std::cout << msg.str(); @@ -121,16 +120,16 @@ void ISO22133::State::handleOSEM(TestObject& obj, ObjectSettingsType& osem) { << 1000 / obj.expectedHeartbeatPeriod.count() << " Hz) " << std::endl; std::cout << msg.str(); - obj.heartbeatTimeout = 10*obj.expectedHeartbeatPeriod; + obj.heartbeatTimeout = 10 * obj.expectedHeartbeatPeriod; msg.str(std::string()); - msg << "Set HEAB timeout to " << obj.heartbeatTimeout.count() << " ms. (" - << 1000 / obj.heartbeatTimeout.count() << " Hz) " << std::endl; + msg << "Set HEAB timeout to " << obj.heartbeatTimeout.count() << " ms. (" << 1000 / obj.heartbeatTimeout.count() + << " Hz) " << std::endl; std::cout << msg.str(); obj.monrPeriod = std::chrono::milliseconds(1000 / (uint)osem.rate.monr); msg.str(std::string()); - msg << "Setting MONR period to " << obj.monrPeriod.count() << " ms. (" - << 1000 / obj.monrPeriod.count() << " Hz) " << std::endl; + msg << "Setting MONR period to " << obj.monrPeriod.count() << " ms. (" << 1000 / obj.monrPeriod.count() << " Hz) " + << std::endl; std::cout << msg.str(); obj.osemSig(osem); @@ -146,7 +145,7 @@ void ISO22133::State::handleOSEM(TestObject& obj, ObjectSettingsType& osem) { void ISO22133::State::handleSTRT(TestObject& obj, StartMessageType& strt) { // No delayed start, start immediately - if(!strt.isTimestampValid) { + if (!strt.isTimestampValid) { // Order matters here, below changes state // causing the signal to not be triggered if placed // after the handleEvent() calls @@ -154,23 +153,22 @@ void ISO22133::State::handleSTRT(TestObject& obj, StartMessageType& strt) { this->handleEvent(obj, ISO22133::Events::S); return; } - + // Current time with compensation for network delay - auto currentTime = std::chrono::to_timeval( - std::chrono::system_clock::now().time_since_epoch() - - obj.getNetworkDelay() - ); + auto currentTime = + std::chrono::to_timeval(std::chrono::system_clock::now().time_since_epoch() - obj.getNetworkDelay()); struct timeval diff; timersub(&strt.startTime, ¤tTime, &diff); - uint32_t diffmySec = diff.tv_sec*1e6 + diff.tv_usec; - int diffint = diff.tv_sec*1e6 + diff.tv_usec; - + uint32_t diffmySec = diff.tv_sec * 1e6 + diff.tv_usec; + int diffint = diff.tv_sec * 1e6 + diff.tv_usec; + // Start time already passed. Request abort from Control Center // resolution is 0,25ms (250 microseconds) in ISO spec. - if(diffint > -250) { + if (diffint > -250) { std::stringstream ss; - ss << "Got STRT message with start time in " << diff.tv_sec << " seconds, " << diff.tv_usec << " mySecs. Waiting" << std::endl; + ss << "Got STRT message with start time in " << diff.tv_sec << " seconds, " << diff.tv_usec + << " mySecs. Waiting" << std::endl; std::cout << ss.str(); obj.delayedStrtThread = std::thread([&, diffmySec]() { @@ -180,17 +178,17 @@ void ISO22133::State::handleSTRT(TestObject& obj, StartMessageType& strt) { // after the handleEvent() calls obj.strtSig(strt); this->handleEvent(obj, ISO22133::Events::S); - }); - } - else { + }); + } else { std::stringstream ss; ss << "Got STRT message with start time in the past. Requesting abort." << std::endl; - ss << "Requested time: " << strt.startTime.tv_sec << " seconds, " << strt.startTime.tv_usec << " mySecs." << std::endl; + ss << "Requested time: " << strt.startTime.tv_sec << " seconds, " << strt.startTime.tv_usec << " mySecs." + << std::endl; ss << "Current time: " << currentTime.tv_sec << " seconds, " << currentTime.tv_usec << " mySecs." << std::endl; ss << "Estimated network delay: " << obj.getNetworkDelay().count() << " mySecs." << std::endl; std::cout << ss.str(); uint8_t error = 0; - error |= 1 << 7; // Abort request is MSB of error mask + error |= 1 << 7; // Abort request is MSB of error mask obj.setErrorState(error); return; } @@ -199,7 +197,7 @@ void ISO22133::State::handleSTRT(TestObject& obj, StartMessageType& strt) { /** * @brief Signals that a new TRAJ is available and sends GREM if in online planned mode - * and object is receiving TRAJ chunks. + * and object is receiving TRAJ chunks. * @param obj * @param msgHeader */ diff --git a/src/trajDecoder.cpp b/src/trajDecoder.cpp index 2d519b2..6cbcb12 100644 --- a/src/trajDecoder.cpp +++ b/src/trajDecoder.cpp @@ -1,79 +1,73 @@ #include #include -#include "trajDecoder.hpp" #include "iso22133.h" +#include "trajDecoder.hpp" ssize_t TrajDecoder::DecodeTRAJ(std::vector& dataBuffer, bool debug) { - std::lock_guard lock(this->guard); - copiedData = dataBuffer; - int tmpByteCounter; - // Decode TRAJ Header - if(!expectingTRAJPoints) { - std::cout << "Receiving TRAJ" << std::endl; - tmpByteCounter = decodeTRAJMessageHeader(&this->trajecoryHeader, - copiedData.data(), copiedData.size(), debug); - if(tmpByteCounter < 0) { - throw std::invalid_argument("Error decoding TRAJ Header"); - } - // Remove header bytes - copiedData.erase(copiedData.begin(), copiedData.begin()+tmpByteCounter); - // The rest will be TRAJ waypoints - expectingTRAJPoints = true; - trajectoryWaypoints.clear(); - trajectoryWaypoints.reserve(trajecoryHeader.nWaypoints); - } - else { - // Insert previously not treated bytes - copiedData.insert(copiedData.begin(), unhandledBytes.begin(), - unhandledBytes.end()); - } + std::lock_guard lock(this->guard); + copiedData = dataBuffer; + int tmpByteCounter; + // Decode TRAJ Header + if (!expectingTRAJPoints) { + std::cout << "Receiving TRAJ" << std::endl; + tmpByteCounter = decodeTRAJMessageHeader(&this->trajecoryHeader, copiedData.data(), copiedData.size(), debug); + if (tmpByteCounter < 0) { + throw std::invalid_argument("Error decoding TRAJ Header"); + } + // Remove header bytes + copiedData.erase(copiedData.begin(), copiedData.begin() + tmpByteCounter); + // The rest will be TRAJ waypoints + expectingTRAJPoints = true; + trajectoryWaypoints.clear(); + trajectoryWaypoints.reserve(trajecoryHeader.nWaypoints); + } else { + // Insert previously not treated bytes + copiedData.insert(copiedData.begin(), unhandledBytes.begin(), unhandledBytes.end()); + } - // Decode TRAJ waypoints - int tmpSize; - TrajectoryWaypointType waypoint; + // Decode TRAJ waypoints + int const tmpSize{trajecoryHeader.nWaypoints - nPointsHandled}; + TrajectoryWaypointType waypoint; - tmpSize = trajecoryHeader.nWaypoints - nPointsHandled; - for(int i = 0; i < tmpSize; i++) { - // Save the bytes remaining and return - if(copiedData.size() < sizeof(TRAJPointType)) { - unhandledBytes.resize(copiedData.size()); - unhandledBytes = copiedData; - break; - } + for (int i = 0; i < tmpSize; i++) { + // Save the bytes remaining and return + if (copiedData.size() < sizeof(TRAJPointType)) { + unhandledBytes.resize(copiedData.size()); + unhandledBytes = copiedData; + break; + } - // We have enough bytes, go ahead and decode waypoint - tmpByteCounter = - decodeTRAJMessagePoint(&waypoint, copiedData.data(), debug); - if(tmpByteCounter < 0) { - throw std::invalid_argument("Error decoding TRAJ Waypoint"); - } - // Remove the decoded bytes - copiedData.erase(copiedData.begin(), copiedData.begin()+tmpByteCounter); - trajectoryWaypoints.push_back(waypoint); - nPointsHandled += 1; - } - std::cout << "Handling TRAJ point, ignore decoding errors" << std::endl; + // We have enough bytes, go ahead and decode waypoint + tmpByteCounter = decodeTRAJMessagePoint(&waypoint, copiedData.data(), debug); + if (tmpByteCounter < 0) { + throw std::invalid_argument("Error decoding TRAJ Waypoint"); + } + // Remove the decoded bytes + copiedData.erase(copiedData.begin(), copiedData.begin() + tmpByteCounter); + trajectoryWaypoints.push_back(waypoint); + nPointsHandled += 1; + } + std::cout << "Handling TRAJ point, ignore decoding errors" << std::endl; - if(nPointsHandled == trajecoryHeader.nWaypoints) { - std::cout << "TRAJ received; " << - trajecoryHeader.nWaypoints << " points." << std::endl; - expectingTRAJPoints = false; // Complete TRAJ received - nPointsHandled = 0; // reset - unhandledBytes.clear(); - } + if (nPointsHandled == trajecoryHeader.nWaypoints) { + std::cout << "TRAJ received; " << trajecoryHeader.nWaypoints << " points." << std::endl; + expectingTRAJPoints = false; // Complete TRAJ received + nPointsHandled = 0; // reset + unhandledBytes.clear(); + } - // Always return the complete buffer size since we don't want - // the same bytes in here again + // Always return the complete buffer size since we don't want + // the same bytes in here again return static_cast(dataBuffer.size()); } TrajectoryHeaderType TrajDecoder::getTrajHeader() const { - std::lock_guard lock(this->guard); - return this->trajecoryHeader; + std::lock_guard lock(this->guard); + return this->trajecoryHeader; } std::vector TrajDecoder::getTraj() const { - std::lock_guard lock(this->guard); - return this->trajectoryWaypoints; + std::lock_guard lock(this->guard); + return this->trajectoryWaypoints; } diff --git a/tests/isoObject.cpp b/tests/isoObject.cpp index 31aae6f..2fdb99b 100644 --- a/tests/isoObject.cpp +++ b/tests/isoObject.cpp @@ -1,153 +1,154 @@ -#include -#include +#include "iso22133object.hpp" #include #include -#include "iso22133object.hpp" +#include +#include extern "C" { - #include "iso22133.h" +#include "iso22133.h" } - using namespace ISO22133; using namespace boost::asio; using namespace std::chrono_literals; -struct timeval * TimeSetToCurrentSystemTime(struct timeval *time) -{ +struct timeval* TimeSetToCurrentSystemTime(struct timeval* time) { // Get system time struct timespec currentTime; clock_gettime(CLOCK_REALTIME, ¤tTime); - time->tv_sec = currentTime.tv_sec; + time->tv_sec = currentTime.tv_sec; time->tv_usec = currentTime.tv_nsec / 1000; return time; } class SimulatedTestObject : public TestObject { - public: - SimulatedTestObject(int tcpSocket) : TestObject(tcpSocket) { +public: + SimulatedTestObject(int tcpSocket) : + TestObject(tcpSocket) { this->startHandleTCP(); state->handleEvent(*this, ISO22133::Events::B); } }; -class ControlCenterEmulator -{ - public: - ControlCenterEmulator(const std::string& ip = "0.0.0.0", int id = 1, int transmitterID = -1, char messCnt = 0) : - listenIP(ip), - tcpSocket(context), - udpSocket(context, ip::udp::v4()), - receiverID(id), - messageCounter(messCnt), - transmitterID(transmitterID) { - } - ~ControlCenterEmulator() { - - } - void connect() { - try { - tcpSocket.connect(ip::tcp::endpoint(ip::address_v4::from_string(listenIP), ISO_22133_DEFAULT_OBJECT_TCP_PORT)); - } catch (boost::system::system_error err) { - EXPECT_EQ(1, 2) << "TCP Connection failed"; - } - } - void sendTCP(const std::vector& data) { - tcpSocket.send(buffer(data)); - } - void sendUDP(const std::vector& data) { - ip::udp::endpoint receiverEP(ip::address_v4::from_string(listenIP), ISO_22133_OBJECT_UDP_PORT); - udpSocket.send_to(buffer(data), receiverEP); +class ControlCenterEmulator { +public: + ControlCenterEmulator(const std::string& ip = "0.0.0.0", int id = 1, int transmitterID = -1, char messCnt = 0) : + listenIP(ip), + tcpSocket(context), + udpSocket(context, ip::udp::v4()), + receiverID(id), + messageCounter(messCnt), + transmitterID(transmitterID) {} + ~ControlCenterEmulator() {} + void connect() { + try { + tcpSocket.connect( + ip::tcp::endpoint(ip::address_v4::from_string(listenIP), ISO_22133_DEFAULT_OBJECT_TCP_PORT)); + } catch (boost::system::system_error err) { + EXPECT_EQ(1, 2) << "TCP Connection failed"; } - void disconnect() { - try { - tcpSocket.close(); - } catch (boost::system::system_error err) { - std::cout << "Error closing TCP socket" << std::endl; - } - try { - udpSocket.close(); - } catch (boost::system::system_error err) { - std::cout << "Error closing UDP socket" << std::endl; - } + } + void sendTCP(const std::vector& data) { + tcpSocket.send(buffer(data)); + } + void sendUDP(const std::vector& data) { + ip::udp::endpoint receiverEP(ip::address_v4::from_string(listenIP), ISO_22133_OBJECT_UDP_PORT); + udpSocket.send_to(buffer(data), receiverEP); + } + void disconnect() { + try { + tcpSocket.close(); + } catch (boost::system::system_error err) { + std::cout << "Error closing TCP socket" << std::endl; } - std::size_t receiveUDP(std::vector& data, ip::udp::endpoint& ep) { - return udpSocket.receive_from(buffer(data), ep); + try { + udpSocket.close(); + } catch (boost::system::system_error err) { + std::cout << "Error closing UDP socket" << std::endl; } + } + std::size_t receiveUDP(std::vector& data, ip::udp::endpoint& ep) { + return udpSocket.receive_from(buffer(data), ep); + } - void receiveTCP(std::vector& data) { - tcpSocket.receive(buffer(data)); - } + void receiveTCP(std::vector& data) { + tcpSocket.receive(buffer(data)); + } - void sendHeartbeat(const ControlCenterStatusType ccStatus) { - HeabMessageDataType heartbeat; - TimeSetToCurrentSystemTime(&heartbeat.dataTimestamp); - heartbeat.controlCenterStatus = ccStatus; - std::vector transmitBuffer(1024); - MessageHeaderType header; - header.receiverID = this->receiverID; - header.messageCounter = this->messageCounter; - header.transmitterID = this->transmitterID; - auto nBytesWritten = encodeHEABMessage(&header, &heartbeat.dataTimestamp, heartbeat.controlCenterStatus, - transmitBuffer.data(), transmitBuffer.size(), false); - transmitBuffer.resize(nBytesWritten); - sendUDP(transmitBuffer); - } + void sendHeartbeat(const ControlCenterStatusType ccStatus) { + HeabMessageDataType heartbeat; + TimeSetToCurrentSystemTime(&heartbeat.dataTimestamp); + heartbeat.controlCenterStatus = ccStatus; + std::vector transmitBuffer(1024); + MessageHeaderType header; + header.receiverID = this->receiverID; + header.messageCounter = this->messageCounter; + header.transmitterID = this->transmitterID; + auto nBytesWritten = encodeHEABMessage(&header, + &heartbeat.dataTimestamp, + heartbeat.controlCenterStatus, + transmitBuffer.data(), + transmitBuffer.size(), + false); + transmitBuffer.resize(nBytesWritten); + sendUDP(transmitBuffer); + } - void buildOSEM(ObjectSettingsType &objSettings) { - - objSettings.desiredID.transmitter = receiverID; - objSettings.desiredID.controlCentre = transmitterID; - objSettings.desiredID.subTransmitter = this->transmitterID; + void buildOSEM(ObjectSettingsType& objSettings) { - memset(&objSettings.coordinateSystemOrigin, 0, sizeof(objSettings.coordinateSystemOrigin)); - objSettings.coordinateSystemType = COORDINATE_SYSTEM_WGS84; - objSettings.coordinateSystemRotation_rad = 0.0; + objSettings.desiredID.transmitter = receiverID; + objSettings.desiredID.controlCentre = transmitterID; + objSettings.desiredID.subTransmitter = this->transmitterID; - TimeSetToCurrentSystemTime(&objSettings.currentTime); - - objSettings.heabTimeout.tv_usec = 20000; - objSettings.heabTimeout.tv_sec = 0; + std::memset(&objSettings.coordinateSystemOrigin, 0, sizeof(objSettings.coordinateSystemOrigin)); + objSettings.coordinateSystemType = COORDINATE_SYSTEM_WGS84; + objSettings.coordinateSystemRotation_rad = 0.0; - objSettings.rate.heab = 10; - objSettings.rate.monr = 100; - objSettings.rate.monr2 = 1; + TimeSetToCurrentSystemTime(&objSettings.currentTime); - objSettings.maxDeviation.lateral_m = 5.0; - objSettings.maxDeviation.position_m = 5.0; - objSettings.maxDeviation.yaw_rad = 15.0 * M_PI/180.0; + objSettings.heabTimeout.tv_usec = 20000; + objSettings.heabTimeout.tv_sec = 0; - objSettings.minRequiredPositioningAccuracy_m = 1.0; + objSettings.rate.heab = 10; + objSettings.rate.monr = 100; + objSettings.rate.monr2 = 1; - objSettings.timeServer.ip = 0; - objSettings.timeServer.port = 0; - } + objSettings.maxDeviation.lateral_m = 5.0; + objSettings.maxDeviation.position_m = 5.0; + objSettings.maxDeviation.yaw_rad = 15.0 * M_PI / 180.0; - void sendOSEM() { - ObjectSettingsType objSettings; - buildOSEM(objSettings); - std::vector transmitBuffer(1024); - MessageHeaderType header; - header.receiverID = this->receiverID; - header.messageCounter = this->messageCounter; - header.transmitterID = this->transmitterID; - auto nBytesWritten = encodeOSEMMessage(&header, &objSettings, transmitBuffer.data(), transmitBuffer.size(), false); - transmitBuffer.resize(nBytesWritten); - sendTCP(transmitBuffer); - } + objSettings.minRequiredPositioningAccuracy_m = 1.0; - ip::tcp::socket *getTCPSocket() { - return &tcpSocket; - } - ip::udp::socket *getUDPSocket() { - return &udpSocket; - } + objSettings.timeServer.ip = 0; + objSettings.timeServer.port = 0; + } - void sendUDPNoop() { - std::vector noopSend(1); - sendUDP(noopSend); - } + void sendOSEM() { + ObjectSettingsType objSettings; + buildOSEM(objSettings); + std::vector transmitBuffer(1024); + MessageHeaderType header; + header.receiverID = this->receiverID; + header.messageCounter = this->messageCounter; + header.transmitterID = this->transmitterID; + auto nBytesWritten = + encodeOSEMMessage(&header, &objSettings, transmitBuffer.data(), transmitBuffer.size(), false); + transmitBuffer.resize(nBytesWritten); + sendTCP(transmitBuffer); + } - private: + ip::tcp::socket* getTCPSocket() { + return &tcpSocket; + } + ip::udp::socket* getUDPSocket() { + return &udpSocket; + } + + void sendUDPNoop() { + std::vector noopSend(1); + sendUDP(noopSend); + } + +private: const std::string listenIP; io_context context; ip::tcp::socket tcpSocket; @@ -157,57 +158,54 @@ class ControlCenterEmulator const int transmitterID; }; -class ControllerEmulator -{ - public: - ControllerEmulator(const std::string& listenIP = "127.0.0.1") : - acceptor(context, ip::tcp::endpoint(ip::address_v4::from_string(listenIP), ISO_22133_DEFAULT_OBJECT_TCP_PORT)), - udpSocket(context, ip::udp::endpoint(ip::address_v4::from_string(listenIP), ISO_22133_OBJECT_UDP_PORT)) {} - virtual ~ControllerEmulator() = default; - /* - * Remember to delete the socket after!! - * Not done here! - */ - ip::tcp::socket *acceptConnection() { - io_context cont; - ip::tcp::socket *sock = new ip::tcp::socket(cont); - acceptor.accept(*sock); - return sock; - } +class ControllerEmulator { +public: + ControllerEmulator(const std::string& listenIP = "127.0.0.1") : + acceptor(context, ip::tcp::endpoint(ip::address_v4::from_string(listenIP), ISO_22133_DEFAULT_OBJECT_TCP_PORT)), + udpSocket(context, ip::udp::endpoint(ip::address_v4::from_string(listenIP), ISO_22133_OBJECT_UDP_PORT)) {} + virtual ~ControllerEmulator() = default; + /* + * Remember to delete the socket after!! + * Not done here! + */ + ip::tcp::socket* acceptConnection() { + io_context cont; + ip::tcp::socket* sock = new ip::tcp::socket(cont); + acceptor.accept(*sock); + return sock; + } - ip::udp::socket *getUDPSocket() { - return &udpSocket; - } + ip::udp::socket* getUDPSocket() { + return &udpSocket; + } - void disconnect() { - acceptor.close(); - try { - udpSocket.shutdown(boost::asio::socket_base::shutdown_both); - } catch (boost::system::system_error err) { } - } + void disconnect() { + acceptor.close(); + try { + udpSocket.shutdown(boost::asio::socket_base::shutdown_both); + } catch (boost::system::system_error err) {} + } - std::size_t receiveUDP(std::vector& data, ip::udp::endpoint& ep) { - return udpSocket.receive_from(buffer(data), ep); - } + std::size_t receiveUDP(std::vector& data, ip::udp::endpoint& ep) { + return udpSocket.receive_from(buffer(data), ep); + } - void sendUDPNoop(ip::udp::endpoint &ep) { - udpSocket.send_to(buffer(std::vector(1)), ep); - } - - private: - io_context context; - ip::tcp::acceptor acceptor; - ip::udp::socket udpSocket; - int timeout; + void sendUDPNoop(ip::udp::endpoint& ep) { + udpSocket.send_to(buffer(std::vector(1)), ep); + } + +private: + io_context context; + ip::tcp::acceptor acceptor; + ip::udp::socket udpSocket; + int timeout; }; -class test_multipleSimulatedISOObjects : public ::testing::Test -{ +class test_multipleSimulatedISOObjects : public ::testing::Test { protected: - test_multipleSimulatedISOObjects(): - obj1Conn("127.0.0.1", 1, 0xF00F), - obj2Conn("127.0.0.1", 2, 0xF00F) - { + test_multipleSimulatedISOObjects() : + obj1Conn("127.0.0.1", 1, 0xF00F), + obj2Conn("127.0.0.1", 2, 0xF00F) { threadListener = std::thread(&test_multipleSimulatedISOObjects::tcpListen, this); obj1Conn.connect(); threadListener.join(); @@ -217,25 +215,24 @@ class test_multipleSimulatedISOObjects : public ::testing::Test obj1 = new SimulatedTestObject(sockets[0]->native_handle()); obj2 = new SimulatedTestObject(sockets[1]->native_handle()); CartesianPosition pos; - pos.xCoord_m = 0.0; - pos.yCoord_m = 0.0; - pos.zCoord_m = 0.0; - pos.isXcoordValid = true; - pos.isYcoordValid = true; - pos.isZcoordValid = true; + pos.xCoord_m = 0.0; + pos.yCoord_m = 0.0; + pos.zCoord_m = 0.0; + pos.isXcoordValid = true; + pos.isYcoordValid = true; + pos.isZcoordValid = true; pos.isPositionValid = true; - pos.isHeadingValid = true; + pos.isHeadingValid = true; obj1->setPosition(pos); obj2->setPosition(pos); SpeedType spd; - spd.lateral_m_s = 0.0; - spd.longitudinal_m_s = 0.0; - spd.isLateralValid = true; + spd.lateral_m_s = 0.0; + spd.longitudinal_m_s = 0.0; + spd.isLateralValid = true; spd.isLongitudinalValid = true; obj1->setSpeed(spd); obj2->setSpeed(spd); threadListener = std::thread(&test_multipleSimulatedISOObjects::udpReceive, this); - } void SetUp() override {} @@ -263,7 +260,6 @@ class test_multipleSimulatedISOObjects : public ::testing::Test } } - void sendUDPNoopToClient(int id) { listener.sendUDPNoop(udpEndpoints[id]); } @@ -288,22 +284,25 @@ class test_multipleSimulatedISOObjects : public ::testing::Test for (int i = 0; i < num_received; i++) { printf("0x%02X ", data.at(i)); } - SimulatedTestObject *obj = nullptr; + SimulatedTestObject* obj = nullptr; udpEndpoints[HeaderData.receiverID] = ep; if (HeaderData.receiverID == 1) { obj = obj1; - } - else if (HeaderData.receiverID == 2) { + } else if (HeaderData.receiverID == 2) { obj = obj2; } if (obj != nullptr) { - char address[ep.address().to_string().length() +1]; - memset(address, 0, sizeof(address)); - memcpy(address, ep.address().to_string().c_str(), ep.address().to_string().length()); - int handled = obj->handleUDPMessage(data.data(), data.size(), listener.getUDPSocket()->native_handle(), address, ep.port()); + char address[ep.address().to_string().length() + 1]; + std::memset(address, 0, sizeof(address)); + std::memcpy( + address, ep.address().to_string().c_str(), ep.address().to_string().length()); + int handled = obj->handleUDPMessage(data.data(), + data.size(), + listener.getUDPSocket()->native_handle(), + address, + ep.port()); nBytesHandled += handled; - } - else { + } else { EXPECT_NE(obj, nullptr) << "Unknown receiver ID"; break; } @@ -311,10 +310,10 @@ class test_multipleSimulatedISOObjects : public ::testing::Test break; } data.erase(data.begin(), data.begin() + nBytesHandled); - } + } } } - } catch(const std::exception& e) {} + } catch (const std::exception& e) {} } std::vector sockets; @@ -322,8 +321,8 @@ class test_multipleSimulatedISOObjects : public ::testing::Test ControlCenterEmulator obj1Conn; ControlCenterEmulator obj2Conn; ControllerEmulator listener; - SimulatedTestObject *obj1; - SimulatedTestObject *obj2; + SimulatedTestObject* obj1; + SimulatedTestObject* obj2; std::thread threadListener; std::thread threadObj1; std::thread threadObj2; @@ -334,31 +333,29 @@ TEST_F(test_multipleSimulatedISOObjects, HEAB_Sent_And_MONR_Not_received_due_to_ ip::udp::endpoint ep1, ep2; std::vector receivedData1(4096); std::vector receivedData2(4096); - obj1Conn.sendHeartbeat(ControlCenterStatusType::CONTROL_CENTER_STATUS_INIT); bool sent1 = false; - std::thread timeoutThread1([this, sent1](){ + std::thread timeoutThread1([this, sent1]() { std::this_thread::sleep_for(10ms); if (!sent1) { this->sendUDPNoopToClient(1); } }); std::size_t received = obj1Conn.receiveUDP(receivedData1, ep1); - sent1 = true; + sent1 = true; EXPECT_EQ(received, 1) << "Received data on test-object1"; - obj2Conn.sendHeartbeat(ControlCenterStatusType::CONTROL_CENTER_STATUS_INIT); bool sent2 = false; - std::thread timeoutThread2([this, sent2](){ + std::thread timeoutThread2([this, sent2]() { std::this_thread::sleep_for(10ms); if (!sent2) { this->sendUDPNoopToClient(2); } }); received = obj2Conn.receiveUDP(receivedData2, ep2); - sent2 = true; + sent2 = true; EXPECT_EQ(received, 1) << "Received data on test-object1"; timeoutThread1.join(); timeoutThread2.join(); @@ -370,30 +367,30 @@ TEST_F(test_multipleSimulatedISOObjects, OSEM_Sent_MONR_Received_as_READY) { std::vector receivedData2(4096); obj1Conn.sendOSEM(); bool sent1 = false; - std::thread timeoutThread1([&, sent1](){ + std::thread timeoutThread1([&, sent1]() { std::this_thread::sleep_for(100ms); if (!sent1) { this->sendUDPNoopToClient(1); } - }); + }); obj1Conn.sendHeartbeat(ControlCenterStatusType::CONTROL_CENTER_STATUS_INIT); std::size_t received = obj1Conn.receiveUDP(receivedData1, ep1); - sent1 = true; + sent1 = true; EXPECT_EQ(received, 60); obj2Conn.sendOSEM(); obj2Conn.sendHeartbeat(ControlCenterStatusType::CONTROL_CENTER_STATUS_INIT); bool sent2 = false; - std::thread timeoutThread2([&, sent2](){ + std::thread timeoutThread2([&, sent2]() { std::this_thread::sleep_for(100ms); if (!sent2) { this->sendUDPNoopToClient(2); } }); received = obj2Conn.receiveUDP(receivedData2, ep2); - sent2 = true; + sent2 = true; EXPECT_EQ(received, 60); timeoutThread1.join(); timeoutThread2.join(); -} \ No newline at end of file +} diff --git a/tests/test_tcpServer.cpp b/tests/test_tcpServer.cpp index 627d2c4..6f0de05 100644 --- a/tests/test_tcpServer.cpp +++ b/tests/test_tcpServer.cpp @@ -1,130 +1,123 @@ -#include +#include +#include #include -#include #include #include -#include -#include +#include +#include #include "tcpServer.hpp" extern "C" { - #include "iso22133.h" +#include "iso22133.h" } using namespace boost::asio; -class tcpServer_connect_new_socket : public ::testing::Test -{ +class tcpServer_connect_new_socket : public ::testing::Test { protected: - void SetUp() override - - - { - // Start server - const std::string ip = "127.0.0.1"; - const uint32_t port = 1234; - server = new TcpServer(ip, port); - acceptThread = new std::thread(&TcpServer::acceptConnection, server); - } - - void TearDown() override - { - // Stop server - server->disconnect(); - acceptThread->join(); - delete server; - } - std::thread *acceptThread; - TcpServer *server; + void SetUp() override + + { + // Start server + const std::string ip = "127.0.0.1"; + const uint32_t port = 1234; + server = new TcpServer(ip, port); + acceptThread = new std::thread(&TcpServer::acceptConnection, server); + } + + void TearDown() override { + // Stop server + server->disconnect(); + acceptThread->join(); + delete server; + } + std::thread* acceptThread; + TcpServer* server; }; -TEST_F(tcpServer_connect_new_socket, connect) -{ - // Connect to server - - io_service io_service; - ip::tcp::socket socket(io_service); - ip::tcp::endpoint endpoint(ip::address::from_string("127.0.0.1"), 1234); - try { - socket.connect(endpoint); - } catch (boost::system::system_error &e) { - FAIL() << "Failed to connect to server: " << e.what(); - } +TEST_F(tcpServer_connect_new_socket, connect) { + // Connect to server + + io_service io_service; + ip::tcp::socket socket(io_service); + ip::tcp::endpoint endpoint(ip::address::from_string("127.0.0.1"), 1234); + try { + socket.connect(endpoint); + } catch (boost::system::system_error& e) { + FAIL() << "Failed to connect to server: " << e.what(); + } } - -class tcpServer_on_existing_socket : public ::testing::Test -{ +class tcpServer_on_existing_socket : public ::testing::Test { protected: - tcpServer_on_existing_socket() : connecterSocket(io_service), listener(io_service) {} - void SetUp() override - { - server = NULL; - std::shared_future readyFuture(readyPromise.get_future()); - // Start server - ip::tcp::endpoint ep(ip::address_v4::from_string("127.0.0.1"), 1234); - listener.open(ep.protocol()); - listener.set_option(ip::tcp::acceptor::reuse_address(true)); - listener.bind(ep); - listener.listen(1); - listener.async_accept([this](const boost::system::error_code& error, ip::tcp::socket peer) { - sockets.push_back(std::move(peer)); - server = new TcpServer(sockets.back().native_handle()); - readyPromise.set_value(); - }); - // Connect to server - - try { - connecterSocket.connect(ep); - } catch (boost::system::system_error &e) { - FAIL() << "Failed to connect to server: " << e.what(); - } - io_service.run_one(); - readyFuture.wait(); - } - - void TearDown() override - { - try { - listener.cancel(); - listener.close(); - } catch (boost::system::system_error &e) { - std::cerr << "Failed to close listener: " << e.what() << std::endl; - } - // Close connection - for (auto &socket : sockets) { - try { - socket.shutdown(ip::tcp::socket::shutdown_both); - } catch (boost::system::system_error &e) { - std::cerr << "Failed to close listener: " << e.what() << std::endl; - } - } - - // Stop server - if (server != NULL) { - server->disconnect(); - delete server; - } - } - - io_context io_service; - std::promise readyPromise; - ip::tcp::socket connecterSocket; - ip::tcp::acceptor listener; - std::thread *acceptThread; - TcpServer *server; - std::vector sockets; + tcpServer_on_existing_socket() : + connecterSocket(io_service), + listener(io_service) {} + void SetUp() override { + server = NULL; + std::shared_future readyFuture(readyPromise.get_future()); + // Start server + ip::tcp::endpoint ep(ip::address_v4::from_string("127.0.0.1"), 1234); + listener.open(ep.protocol()); + listener.set_option(ip::tcp::acceptor::reuse_address(true)); + listener.bind(ep); + listener.listen(1); + listener.async_accept([this](const boost::system::error_code& error, ip::tcp::socket peer) { + sockets.push_back(std::move(peer)); + server = new TcpServer(sockets.back().native_handle()); + readyPromise.set_value(); + }); + // Connect to server + + try { + connecterSocket.connect(ep); + } catch (boost::system::system_error& e) { + FAIL() << "Failed to connect to server: " << e.what(); + } + io_service.run_one(); + readyFuture.wait(); + } + + void TearDown() override { + try { + listener.cancel(); + listener.close(); + } catch (boost::system::system_error& e) { + std::cerr << "Failed to close listener: " << e.what() << std::endl; + } + // Close connection + for (auto& socket : sockets) { + try { + socket.shutdown(ip::tcp::socket::shutdown_both); + } catch (boost::system::system_error& e) { + std::cerr << "Failed to close listener: " << e.what() << std::endl; + } + } + + // Stop server + if (server != NULL) { + server->disconnect(); + delete server; + } + } + + io_context io_service; + std::promise readyPromise; + ip::tcp::socket connecterSocket; + ip::tcp::acceptor listener; + std::thread* acceptThread; + TcpServer* server; + std::vector sockets; }; -TEST_F(tcpServer_on_existing_socket, acceptConnection_should_throw_error) -{ - // Connect to server - try { - server->acceptConnection(); - FAIL() << "Should throw error"; - } catch (std::runtime_error &e) { - SUCCEED() << "Failed to accept connection: " << e.what(); - } -} \ No newline at end of file +TEST_F(tcpServer_on_existing_socket, acceptConnection_should_throw_error) { + // Connect to server + try { + server->acceptConnection(); + FAIL() << "Should throw error"; + } catch (std::runtime_error& e) { + SUCCEED() << "Failed to accept connection: " << e.what(); + } +} diff --git a/tests/test_trajDecoder.cpp b/tests/test_trajDecoder.cpp index 5cfed5f..d23abb2 100644 --- a/tests/test_trajDecoder.cpp +++ b/tests/test_trajDecoder.cpp @@ -1,87 +1,78 @@ #include "trajDecoder.hpp" + +#include + #include class DecodeTraj : public ::testing::Test { - protected: - DecodeTraj() {} - void SetUp() override { - auto trajectoryVersion = TRAJECTORY_INFO_RELATIVE_TO_OBJECT; - auto trajectoryName = "some description"; - auto nameLength = strlen(trajectoryName)-1; +protected: + DecodeTraj() {} + void SetUp() override { + auto trajectoryVersion = TRAJECTORY_INFO_RELATIVE_TO_OBJECT; + auto trajectoryName = "some description"; + auto nameLength = strlen(trajectoryName) - 1; auto numberOfPointsInTraj = 3; - memset(encodeBuffer, 0, sizeof(encodeBuffer)); - auto points = encodeBuffer; + std::memset(encodeBuffer, 0, sizeof(encodeBuffer)); + auto points = encodeBuffer; auto bufferLength = sizeof(encodeBuffer); - bool debug = false; + bool debug = false; MessageHeaderType header = {0}; - auto offset = encodeTRAJMessageHeader( - &header, - trajectoryID, - TRAJECTORY_INFO_RELATIVE_TO_OBJECT, - trajectoryName, - nameLength, - nTrajPoints, - points, - sizeof(encodeBuffer), - debug); + auto offset = encodeTRAJMessageHeader(&header, + trajectoryID, + TRAJECTORY_INFO_RELATIVE_TO_OBJECT, + trajectoryName, + nameLength, + nTrajPoints, + points, + sizeof(encodeBuffer), + debug); ASSERT_GT(offset, 0); points += offset; for (double i = 0; i < nTrajPoints; i++) { // Using float as loop counter to avoid ugly explicit casts - struct timeval tv = {1,2}; - CartesianPosition pos = {1+i,2+i,3+i,4,true,true,true,true,true}; - SpeedType spd = {1+i,2+i,true,true}; - AccelerationType acc = {1+i*2,2+i*2,true,true}; - float curvature = 12.34+i; - offset = encodeTRAJMessagePoint( - &tv, - pos, - spd, - acc, - curvature, - points, - sizeof(encodeBuffer) - (points-encodeBuffer), - debug); + struct timeval tv = {1, 2}; + CartesianPosition pos = {1 + i, 2 + i, 3 + i, 4, true, true, true, true, true}; + SpeedType spd = {1 + i, 2 + i, true, true}; + AccelerationType acc = {1 + i * 2, 2 + i * 2, true, true}; + float curvature = 12.34 + i; + offset = encodeTRAJMessagePoint( + &tv, pos, spd, acc, curvature, points, sizeof(encodeBuffer) - (points - encodeBuffer), debug); ASSERT_GT(offset, 0); points += offset; } - offset = encodeTRAJMessageFooter( - points, - sizeof(encodeBuffer) - (points - encodeBuffer), - debug - ); + offset = encodeTRAJMessageFooter(points, sizeof(encodeBuffer) - (points - encodeBuffer), debug); ASSERT_GT(offset, 0); points += offset; - } - - char encodeBuffer[1024]; + } + + char encodeBuffer[1024]; int const nTrajPoints = 10; - int trajectoryID = 666; - TrajDecoder decoder; + int trajectoryID = 666; + TrajDecoder decoder; }; TEST_F(DecodeTraj, CheckThatAllPointsAreDecoded) { - std::vector data(encodeBuffer, encodeBuffer + sizeof(encodeBuffer)); - decoder.DecodeTRAJ(data, true); - ASSERT_EQ(decoder.getTrajHeader().trajectoryID, 666); - for (int i=0; i < nTrajPoints; i++){ - ASSERT_EQ(decoder.getTraj()[i].pos.xCoord_m, 1+i); - ASSERT_EQ(decoder.getTraj()[i].pos.yCoord_m, 2+i); - ASSERT_EQ(decoder.getTraj()[i].pos.zCoord_m, 3+i); + std::vector data(encodeBuffer, encodeBuffer + sizeof(encodeBuffer)); + decoder.DecodeTRAJ(data, true); + ASSERT_EQ(decoder.getTrajHeader().trajectoryID, 666); + for (int i = 0; i < nTrajPoints; i++) { + ASSERT_EQ(decoder.getTraj()[i].pos.xCoord_m, 1 + i); + ASSERT_EQ(decoder.getTraj()[i].pos.yCoord_m, 2 + i); + ASSERT_EQ(decoder.getTraj()[i].pos.zCoord_m, 3 + i); ASSERT_EQ(decoder.getTraj()[i].pos.isXcoordValid, true); ASSERT_EQ(decoder.getTraj()[i].pos.isYcoordValid, true); ASSERT_EQ(decoder.getTraj()[i].pos.isZcoordValid, true); - ASSERT_EQ(decoder.getTraj()[i].spd.longitudinal_m_s, 1+i); - ASSERT_EQ(decoder.getTraj()[i].spd.lateral_m_s, 2+i); + ASSERT_EQ(decoder.getTraj()[i].spd.longitudinal_m_s, 1 + i); + ASSERT_EQ(decoder.getTraj()[i].spd.lateral_m_s, 2 + i); ASSERT_EQ(decoder.getTraj()[i].spd.isLateralValid, true); ASSERT_EQ(decoder.getTraj()[i].spd.isLongitudinalValid, true); - ASSERT_EQ(decoder.getTraj()[i].acc.longitudinal_m_s2, 1+i*2); - ASSERT_EQ(decoder.getTraj()[i].acc.lateral_m_s2, 2+i*2); + ASSERT_EQ(decoder.getTraj()[i].acc.longitudinal_m_s2, 1 + i * 2); + ASSERT_EQ(decoder.getTraj()[i].acc.lateral_m_s2, 2 + i * 2); ASSERT_EQ(decoder.getTraj()[i].acc.isLateralValid, true); ASSERT_EQ(decoder.getTraj()[i].acc.isLongitudinalValid, true); - EXPECT_NEAR(decoder.getTraj()[i].curvature, 12.34+i, 0.0001); + EXPECT_NEAR(decoder.getTraj()[i].curvature, 12.34 + i, 0.0001); } -} \ No newline at end of file +}