From 399f4179a5ea24b0ac47a1004b136cec17a4edb1 Mon Sep 17 00:00:00 2001 From: Sebastian Lindholm Date: Thu, 2 Oct 2025 14:03:35 +0200 Subject: [PATCH] IsoObject using OSEM v3 --- demoIsoObject.cpp | 2 +- inc/iso22133object.hpp | 12 ++++++------ iso22133 | 2 +- src/iso22133object.cpp | 21 ++++++++++++--------- src/iso22133state.cpp | 1 + tests/isoObject.cpp | 4 ++-- 6 files changed, 23 insertions(+), 19 deletions(-) diff --git a/demoIsoObject.cpp b/demoIsoObject.cpp index db50c7f..8c9489c 100644 --- a/demoIsoObject.cpp +++ b/demoIsoObject.cpp @@ -104,7 +104,7 @@ class myObject : public ISO22133::TestObject { ISO22133::TestObject(ip), dummyMember(0) { ObjectSettingsType osem; - osem.testMode = TEST_MODE_UNAVAILABLE; + osem.testMode = TEST_MODE_PREPLANNED; setMonr(1,2,3,0.4,5,6); setObjectSettings(osem); } diff --git a/inc/iso22133object.hpp b/inc/iso22133object.hpp index d30f6be..eb2226a 100644 --- a/inc/iso22133object.hpp +++ b/inc/iso22133object.hpp @@ -94,7 +94,7 @@ class TestObject { //! 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 startHEABCheck() { communicationTimeoutThread = std::thread(&TestObject::checkHeabLoop, this); } void startSendMonr() { monrThread = std::thread(&TestObject::sendMonrLoop, this); } protected: @@ -161,7 +161,7 @@ class TestObject { 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 maxSafeNetworkDelay = std::chrono::milliseconds(10000); //! Used to get estimated network delay std::chrono::milliseconds getNetworkDelay(); @@ -189,9 +189,9 @@ class TestObject { //! Sends GREM message on control channel void sendGREM(HeaderType header, GeneralResponseStatus responseCode, bool debug = false); //! Called if HEAB messages do not arrive on time - void onHeabTimeout(); + void onCommunicationTimeout(); //! Function that checks if HEABs arrive on time - void checkHeabTimeout(); + void checkCommunicationTimeout(); //! Set estimated network delay from HEAB times void setNetworkDelay(std::chrono::milliseconds); @@ -207,7 +207,7 @@ class TestObject { expectedMessageCounter = (expectedMessageCounter + 1) % 256; } - sigslot::signal<>heabTimeout; + sigslot::signal<>communicationTimeout; std::mutex recvMutex; std::mutex heabMutex; std::mutex netwrkDelayMutex; @@ -215,7 +215,7 @@ class TestObject { std::thread tcpReceiveThread; std::thread udpReceiveThread; std::thread monrThread; - std::thread heabTimeoutThread; + std::thread communicationTimeoutThread; std::thread delayedStrtThread; std::string name = "unnamed"; TcpServer ctrlChannel; diff --git a/iso22133 b/iso22133 index 1b6c206..3f1a446 160000 --- a/iso22133 +++ b/iso22133 @@ -1 +1 @@ -Subproject commit 1b6c206b974c200725760befd4b9e745f43ff1d4 +Subproject commit 3f1a446136bb19b6591638239ceec01571494033 diff --git a/src/iso22133object.cpp b/src/iso22133object.cpp index 5a62441..83aad13 100644 --- a/src/iso22133object.cpp +++ b/src/iso22133object.cpp @@ -48,6 +48,9 @@ void TestObject::initialize() { SpeedType initSpd; AccelerationType initAcc; TestModeType initTm; + EmergencyBehaviorType emergencyBehavior; + ComLostType comLost; + xyzTrajPointResolutionType xyzTrajPointResolution; localIP = "0.0.0.0"; initPos.isHeadingValid = false; initPos.isPositionValid = false; @@ -55,7 +58,7 @@ void TestObject::initialize() { initSpd.isLongitudinalValid = false; initAcc.isLateralValid = false; initAcc.isLongitudinalValid = false; - initTm = TEST_MODE_UNAVAILABLE; + initTm = TEST_MODE_PREPLANNED; this->setPosition(initPos); this->setSpeed(initSpd); this->setAcceleration(initAcc); @@ -67,7 +70,7 @@ void TestObject::initialize() { this->ostmSig.connect(&TestObject::onOSTM, this); this->trajSig.connect(&TestObject::onTRAJ, this); this->strtSig.connect(&TestObject::onSTRT, this); - this->heabTimeout.connect(&TestObject::onHeabTimeout, this); + this->communicationTimeout.connect(&TestObject::onCommunicationTimeout, this); } TestObject::~TestObject() { @@ -78,7 +81,7 @@ TestObject::~TestObject() { } monrThread.join(); tcpReceiveThread.join(); - heabTimeoutThread.join(); + communicationTimeoutThread.join(); if (delayedStrtThread.joinable()) { delayedStrtThread.join(); } @@ -241,7 +244,7 @@ void TestObject::receiveUDP() { } } -void TestObject::checkHeabTimeout() { +void TestObject::checkCommunicationTimeout() { using namespace std::chrono; std::scoped_lock lock(heabMutex); // Check time difference of received HEAB and last HEAB @@ -251,20 +254,20 @@ void TestObject::checkHeabTimeout() { ss << "Heartbeat timeout: " << duration_cast(timeSinceHeab).count() << " ms since last heartbeat exceeds limit of " << heartbeatTimeout.count() << " ms." << std::endl; std::cerr << ss.str(); - heabTimeout(); + communicationTimeout(); } } void TestObject::checkHeabLoop() { while (this->on) { auto t = std::chrono::steady_clock::now(); - checkHeabTimeout(); + checkCommunicationTimeout(); // Don't lock the mutex all the time std::this_thread::sleep_until(t + expectedHeartbeatPeriod); } } -void TestObject::onHeabTimeout() { +void TestObject::onCommunicationTimeout() { disconnect(); this->state->handleEvent(*this, Events::L); } @@ -333,7 +336,7 @@ int TestObject::handleMessage(std::vector& dataBuffer) { break; case MESSAGE_ID_OSEM: ObjectSettingsType OSEMstruct; - bytesHandled = decodeOSEMMessage(&OSEMstruct, dataBuffer.data(), dataBuffer.size(), debug); + bytesHandled = decodeOSEMMessage(&OSEMstruct, dataBuffer.data(), dataBuffer.size(), true); if (bytesHandled < 0) { throw std::invalid_argument("Error decoding OSEM"); } @@ -361,7 +364,7 @@ int TestObject::handleMessage(std::vector& dataBuffer) { case MESSAGE_ID_HEAB: HeabMessageDataType HEABdata; - bytesHandled = decodeHEABMessage(dataBuffer.data(), dataBuffer.size(), currentTime, &HEABdata, debug); + bytesHandled = decodeHEABMessage(dataBuffer.data(), dataBuffer.size(), currentTime, &HEABdata, true); if (bytesHandled < 0) { throw std::invalid_argument("Error decoding HEAB"); } diff --git a/src/iso22133state.cpp b/src/iso22133state.cpp index e2a6b56..13f9a48 100644 --- a/src/iso22133state.cpp +++ b/src/iso22133state.cpp @@ -69,6 +69,7 @@ void ISO22133::State::handleEvent(TestObject& obj, const ISO22133::Events::Event std::cout << "Entering state: " << obj.state->getName() << std::endl; obj.stateChangeSig(); obj.state->onEnter(obj); + } /** diff --git a/tests/isoObject.cpp b/tests/isoObject.cpp index 31aae6f..8fab6dc 100644 --- a/tests/isoObject.cpp +++ b/tests/isoObject.cpp @@ -105,8 +105,8 @@ class ControlCenterEmulator TimeSetToCurrentSystemTime(&objSettings.currentTime); - objSettings.heabTimeout.tv_usec = 20000; - objSettings.heabTimeout.tv_sec = 0; + objSettings.communicationTimeout.tv_usec = 20000; + objSettings.communicationTimeout.tv_sec = 0; objSettings.rate.heab = 10; objSettings.rate.monr = 100;