From 1004233e20aa5d4db0fc70ac9c15035729035052 Mon Sep 17 00:00:00 2001 From: zjk <1553836110@qq.com> Date: Mon, 20 Nov 2023 09:58:12 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BF=81=E7=A7=BBpi=E4=B8=8A=E7=9A=84feture-mc?= =?UTF-8?q?=E5=88=86=E6=94=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- PlanConfigure.json | 65 + launch/launch.moos | 34 +- setting/ControlParam.json | 43 + setting/Origin.json | 6 + setting/PlanConfigure.json | 195 +++ src/CMakeLists.txt | 1 + src/pBoardSupportComm/BoardSupportComm.cpp | 150 +- src/pBoardSupportComm/BoardSupportComm.h | 19 +- src/pClientViewer/ClientViewer.cpp | 331 ++-- src/pDataManagement/DataManagement.cpp | 415 ++++- src/pDataManagement/DataManagement.h | 16 +- src/pEmulator/Emulator.cpp | 2 +- src/pEmulator/Emulator.hpp | 2 +- src/pEmulator/_150server.hpp | 1 + src/pFaultHandle/.vscode/settings.json | 12 + src/pFaultHandle/CMakeLists.txt | 32 + src/pFaultHandle/FaultHandle.cpp | 1366 +++++++++++++++++ src/pFaultHandle/FaultHandle.h | 170 ++ src/pFaultHandle/FaultHandle_Info.cpp | 115 ++ src/pFaultHandle/FaultHandle_Info.h | 18 + src/pFaultHandle/main.cpp | 52 + src/pFaultHandle/pFaultHandle.moos | 9 + .../SurfaceSupportComm.cpp | 674 ++++---- src/pSurfaceSupportComm/SurfaceSupportComm.h | 7 +- src/pTaskManger/TaskManger.cpp | 45 +- src/pTaskManger/TaskManger.h | 11 +- 26 files changed, 3173 insertions(+), 618 deletions(-) create mode 100644 setting/ControlParam.json create mode 100644 setting/Origin.json create mode 100644 setting/PlanConfigure.json create mode 100644 src/pFaultHandle/.vscode/settings.json create mode 100644 src/pFaultHandle/CMakeLists.txt create mode 100644 src/pFaultHandle/FaultHandle.cpp create mode 100644 src/pFaultHandle/FaultHandle.h create mode 100644 src/pFaultHandle/FaultHandle_Info.cpp create mode 100644 src/pFaultHandle/FaultHandle_Info.h create mode 100644 src/pFaultHandle/main.cpp create mode 100644 src/pFaultHandle/pFaultHandle.moos diff --git a/PlanConfigure.json b/PlanConfigure.json index fdc25d4..861e03d 100644 --- a/PlanConfigure.json +++ b/PlanConfigure.json @@ -45,6 +45,71 @@ "taskId" : "1", "taskName" : "east_waypt_survey" }, + "plan1_toMoos" : + { + "boardStamp" : 1699602762.2845099, + "clientStamp" : 1699602762.7520001, + "closedLoop" : false, + "duration" : -1.0, + "maxDepth" : -1.0, + "minDepth" : -1.0, + "origin" : + { + "altitude" : 0.0, + "lat" : 43.825299999999999, + "lon" : -70.330399999999997 + }, + "perpetual" : false, + "points" : + [ + { + "depth" : 2.0, + "east" : 117.83291847226671, + "lat" : 43.825713999999998, + "lon" : -70.32893, + "name" : "Goto1", + "north" : 46.200319317940647, + "speed" : 2.0, + "type" : "point" + }, + { + "depth" : 2.0, + "east" : -17.18366087421261, + "lat" : 43.826782000000001, + "lon" : -70.330609999999993, + "name" : "Goto2", + "north" : 164.87635389378988, + "speed" : 2.0, + "type" : "point" + }, + { + "depth" : 2.0, + "east" : -241.19025325837993, + "lat" : 43.825465999999999, + "lon" : -70.333399999999997, + "name" : "Goto3", + "north" : 18.653618776002617, + "speed" : 2.0, + "type" : "point" + }, + { + "depth" : 2.0, + "east" : -203.76118848802312, + "lat" : 43.823234999999997, + "lon" : -70.332930000000005, + "name" : "Goto4", + "north" : -229.29782627916489, + "speed" : 2.0, + "type" : "point" + } + ], + "priority" : 10, + "repeat" : 1, + "sourceAddress" : "10.25.0.163", + "sourceName" : "CCU JHL 0_163", + "taskId" : "0,106,3,-96,8,-103,13,6,9,32,50,-13,47,-71,61,1", + "taskName" : "plan1_toMoos" + }, "west_waypt_survey" : { "boardStamp" : 1698135268.3958621, diff --git a/launch/launch.moos b/launch/launch.moos index 2ff2039..c0ed762 100644 --- a/launch/launch.moos +++ b/launch/launch.moos @@ -15,6 +15,19 @@ LatOrigin = 43.825300 LongOrigin = -70.330400 AltOrigin = 0 +VehicleName = lauv-150 + +LogEnable = false +LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/ + +AuvDataLog = auvData.txt +MissionHistoryLog = missionHistory.txt +ClientCommandLog = clientCommand.txt +FaultLog = faultLog.txt + +planConfigPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/PlanConfigure.json +llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json + //------------------------------------------ // Antler configuration block ProcessConfig = ANTLER @@ -27,18 +40,18 @@ ProcessConfig = ANTLER Run = uProcessWatch @ NewConsole = false Run = pRealm @ NewConsole = false Run = pShare @ NewConsole = false - //Run = pMarineViewer @ NewConsole = false - //Run = pLogger @ NewConsole = false + Run = pMarineViewer @ NewConsole = false //===========Our define process==================== Run = pBoardSupportComm @ NewConsole = false Run = pTaskManger @ NewConsole = false Run = pMotionControler @ NewConsole = false - //Run = pSurfaceSupportComm @ NewConsole = false + Run = pSurfaceSupportComm @ NewConsole = false Run = pDataManagement @ NewConsole = false //===============For test process=================== Run = pEmulator @ NewConsole = false //Run = uSimMarine @ NewConsole = false //Run = pMarinePID @ NewConsole = false + Run = pFaultHandle @ NewConsole = false } ProcessConfig = pMarineViewer @@ -195,9 +208,6 @@ ProcessConfig = pTaskManger { AppTick = 8 CommsTick = 8 - - planConfigPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/PlanConfigure.json - //planConfigPath = /home/ben/project/moos-ivp-pi/PlanConfigure.json } ProcessConfig = pBoardSupportComm @@ -259,13 +269,13 @@ ProcessConfig = pMotionControler cheak_stalensee = true delta_freqency = 5 - config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/src/pMotionControler/ControlParam.json - + //config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/src/pMotionControler/ControlParam.json + config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/ControlParam.json } ProcessConfig = pEmulator { - AppTick = 100 + AppTick = 5 CommsTick = 5 matlab_host = 10.25.0.137 matlab_port = 8085 @@ -304,3 +314,9 @@ ProcessConfig = pLogger WildCardOmitPattern = DB_RWSUMMARY WildCardExclusionLog = true } +ProcessConfig = pFaultHandle +{ + AppTick = 4 + CommsTick = 4 +} + diff --git a/setting/ControlParam.json b/setting/ControlParam.json new file mode 100644 index 0000000..9204381 --- /dev/null +++ b/setting/ControlParam.json @@ -0,0 +1,43 @@ +{ + "speed" : + { + "Kp" : 10.0, + "Ki" : 5.0, + "Kd" : 0.0, + "LimitDelta" : 50, + "MaxOut" : 100, + "MinOut" : 0 + }, + "heading" : + { + "Kp" : 0.8, + "Ki" : 0.05, + "Kd" : 2.2, + "LimitDelta" : 5, + "MaxOut" : 30, + "MinOut" : -30 + }, + "depth" : + { + "Kp" : 10.0, + "Ki" : 0.3, + "Kd" : 2.0, + "LimitDelta" : 5, + "MaxOut" : 10, + "MinOut" : -10 + }, + "pitch" : + { + "Kp" : 0.6, + "Ki" : 0.03, + "Kd" : 1.5, + "LimitDelta" : 5, + "MaxOut" : 30, + "MinOut" : -30 + }, + "const_thrust" : 0, + "dead_zone" : 10, + "speedCol" : true, + "depthCol" : true, + "HeadingCol" : true +} diff --git a/setting/Origin.json b/setting/Origin.json new file mode 100644 index 0000000..5822f5c --- /dev/null +++ b/setting/Origin.json @@ -0,0 +1,6 @@ +{ + "AltOrigin" : 0, + "LatOrigin" : 50.825298309326172, + "LongOrigin" : -90.330398559570312, + "TaskName" : "east_waypt_survey" +} diff --git a/setting/PlanConfigure.json b/setting/PlanConfigure.json new file mode 100644 index 0000000..9bad925 --- /dev/null +++ b/setting/PlanConfigure.json @@ -0,0 +1,195 @@ +{ + "east_waypt_survey" : + { + "boardStamp" : 1699945816.369447, + "clientStamp" : 1699945815.876014, + "closedLoop" : true, + "constSpeed" : -1.0, + "duration" : -1.0, + "maxDepth" : -1.0, + "minDepth" : -1.0, + "origin" : + { + "altitude" : 0.0, + "lat" : 50.825299999999999, + "lon" : -90.330399999999997 + }, + "perpetual" : false, + "points" : + [ + { + "depth" : 10.0, + "east" : 121.51780491942635, + "lat" : 43.824428558349609, + "lon" : -70.328887939453125, + "name" : "station_1", + "north" : -96.635898081838207, + "speed" : 3.0, + "type" : "point" + }, + { + "depth" : 8.0, + "east" : 201.91511278899367, + "lat" : 43.824676513671875, + "lon" : -70.327888488769531, + "name" : "station_2", + "north" : -69.083922179977435, + "speed" : 5.0, + "type" : "point" + }, + { + "depth" : 6.0, + "east" : 203.75986507534614, + "lat" : 43.823623657226562, + "lon" : -70.327865600585938, + "name" : "station_3", + "north" : -186.06549181810368, + "speed" : 7.0, + "type" : "point" + }, + { + "depth" : 4.0, + "east" : 131.33919016412341, + "lat" : 43.823623657226562, + "lon" : -70.328765869140625, + "name" : "station_4", + "north" : -186.06731482554795, + "speed" : 9.0, + "type" : "point" + } + ], + "priority" : 10, + "repeat" : 1, + "sourceAddress" : "10.25.0.160", + "sourceName" : "CCU Neptus 0_163", + "taskId" : "1", + "taskName" : "east_waypt_survey" + }, + "plan1_toMoos" : + { + "boardStamp" : 1699602762.2845099, + "clientStamp" : 1699602762.7520001, + "closedLoop" : false, + "duration" : -1.0, + "maxDepth" : -1.0, + "minDepth" : -1.0, + "origin" : + { + "altitude" : 0.0, + "lat" : 50.825299999999999, + "lon" : -90.330399999999997 + }, + "perpetual" : false, + "points" : + [ + { + "depth" : 2.0, + "east" : 117.83291847226671, + "lat" : 43.825713999999998, + "lon" : -70.32893, + "name" : "Goto1", + "north" : 46.200319317940647, + "speed" : 2.0, + "type" : "point" + }, + { + "depth" : 2.0, + "east" : -17.18366087421261, + "lat" : 43.826782000000001, + "lon" : -70.330609999999993, + "name" : "Goto2", + "north" : 164.87635389378988, + "speed" : 2.0, + "type" : "point" + }, + { + "depth" : 2.0, + "east" : -241.19025325837993, + "lat" : 43.825465999999999, + "lon" : -70.333399999999997, + "name" : "Goto3", + "north" : 18.653618776002617, + "speed" : 2.0, + "type" : "point" + }, + { + "depth" : 2.0, + "east" : -203.76118848802312, + "lat" : 43.823234999999997, + "lon" : -70.332930000000005, + "name" : "Goto4", + "north" : -229.29782627916489, + "speed" : 2.0, + "type" : "point" + } + ], + "priority" : 10, + "repeat" : 1, + "sourceAddress" : "10.25.0.163", + "sourceName" : "CCU JHL 0_163", + "taskId" : "0,106,3,-96,8,-103,13,6,9,32,50,-13,47,-71,61,1", + "taskName" : "plan1_toMoos" + }, + "west_waypt_survey" : + { + "boardStamp" : 1698135268.3958621, + "clientStamp" : 1698135268.2057669, + "closedLoop" : true, + "constSpeed" : -1.0, + "duration" : -1.0, + "maxDepth" : -1.0, + "minDepth" : -1.0, + "origin" : + { + "altitude" : 0.0, + "lat" : 43.825298309326172, + "lon" : -70.330398559570312 + }, + "perpetual" : true, + "points" : + [ + { + "depth" : 9.0, + "east" : -91.445572043530944, + "lat" : 43.824195861816406, + "lon" : -70.331535339355469, + "name" : "station_1", + "north" : -122.49101460421512, + "speed" : 4.0 + }, + { + "depth" : 7.0, + "east" : 5.5235485468483718, + "lat" : 43.824298858642578, + "lon" : -70.330329895019531, + "name" : "station_2", + "north" : -111.04778559533926, + "speed" : 6.0 + }, + { + "depth" : 5.0, + "east" : 4.2961493948725868, + "lat" : 43.823516845703125, + "lon" : -70.330345153808594, + "name" : "station_3", + "north" : -197.93630920628678, + "speed" : 8.0 + }, + { + "depth" : 3.0, + "east" : -81.013520711457318, + "lat" : 43.823207855224609, + "lon" : -70.331405639648438, + "name" : "station_4", + "north" : -232.26737690334403, + "speed" : 10.0 + } + ], + "priority" : 10, + "repeat" : -1, + "sourceAddress" : "10.25.0.160", + "sourceName" : "CCU Neptus 0_163", + "taskId" : "2", + "taskName" : "west_waypt_survey" + } +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 81bb93f..dd5a525 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -23,6 +23,7 @@ ADD_SUBDIRECTORY(pTaskManger) ADD_SUBDIRECTORY(pTaskSend) ADD_SUBDIRECTORY(pMotionControler) ADD_SUBDIRECTORY(pEmulator) +ADD_SUBDIRECTORY(pFaultHandle) ############################################################################## # END of CMakeLists.txt ############################################################################## diff --git a/src/pBoardSupportComm/BoardSupportComm.cpp b/src/pBoardSupportComm/BoardSupportComm.cpp index 44ee2d0..8a781c5 100644 --- a/src/pBoardSupportComm/BoardSupportComm.cpp +++ b/src/pBoardSupportComm/BoardSupportComm.cpp @@ -13,28 +13,14 @@ #define TCP_RECEIVE_PORT 8001 #define TCP_SERVER_ADDRESS "127.0.0.1" -// #define MOOS_AUV_SIM -#define MATLAB_AUV_SIM +#define MOOS_AUV_SIM +// #define MATLAB_AUV_SIM // #ifdef TRUE_AUV using namespace std; BoardSupportComm::BoardSupportComm() { - // uMotion_posX_fb = 0; - // uMotion_posY_fb = 0; - // uMotion_posZ_fb = 0; - // uMotion_roll_fb = 0; - // uMotion_pitch_fb = 0; - // uMotion_yaw_fb = 0; - // uMotion_longitude_fb = 0; - // uMotion_latitude_fb = 0; - // uMotion_velocityX_fb = 0; - // uMotion_velocityY_fb = 0; - // uMotion_velocityZ_fb = 0; - // uMotion_deep_fb = 0; - - estimatedState.info = "lauv-150"; estimatedState.currentLon = 0; estimatedState.currentLat = 0; estimatedState.currentAltitude = 0; @@ -108,12 +94,12 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) if(key == "NAV_X") { //E->N - estimatedState.offsetNorth = msg.GetDouble(); + estimatedState.offsetEast = msg.GetDouble(); } if(key == "NAV_Y") { //N->E - estimatedState.offsetEast = msg.GetDouble(); + estimatedState.offsetNorth = msg.GetDouble(); } if(key == "NAV_Z") { @@ -122,7 +108,18 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "NAV_YAW") { - estimatedState.yaw = -msg.GetDouble(); + double yawTemp = msg.GetDouble(); + if (std::abs(yawTemp) <= M_PI) + { + if (yawTemp <= 0) + { + estimatedState.yaw = -yawTemp * 180 / M_PI; + } + else + { + estimatedState.yaw = (2 * M_PI - yawTemp) * 180 / M_PI; + } + } } if(key == "NAV_PITCH") { @@ -224,10 +221,6 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) { return false; } - // if (executeCommand.drive_mode != 0x02) - // { - // return false; - // } std::string err; Json::Value recvCommand; std::istringstream iss(msg.GetString()); @@ -286,12 +279,45 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) std::cerr << "Failed to parse JSON string." << std::endl; return false; } - estimatedState.referenceAltitude = recvCommand["altitude"].asFloat(); + estimatedState.referenceAltitude = recvCommand["alt"].asFloat(); estimatedState.referenceLat = recvCommand["lat"].asFloat(); estimatedState.referenceLon = recvCommand["lon"].asFloat(); + + struct stat info; + if (stat(llaOriginPath.c_str(), &info) == 0) + { + std::ifstream ifs; + ifs.open(llaOriginPath, std::ios::in); + + Json::Reader reader; + Json::Value originJsonValue; + reader.parse(ifs, originJsonValue); + ifs.close(); + + originJsonValue["LongOrigin"] = recvCommand["lon"].asFloat(); + originJsonValue["LatOrigin"] = recvCommand["lat"].asFloat(); + originJsonValue["AltOrigin"] = recvCommand["alt"].asFloat(); + originJsonValue["TaskName"] = recvCommand["task"].asString(); + Json::StreamWriterBuilder builder; + std::ofstream ofs; + ofs.open(llaOriginPath, std::ios::out); + ofs << Json::writeString(builder, originJsonValue) << std::endl; + ofs.close(); + } + else + { + Json::Value originJsonValue; + originJsonValue["LongOrigin"] = recvCommand["lon"].asFloat(); + originJsonValue["LatOrigin"] = recvCommand["lat"].asFloat(); + originJsonValue["AltOrigin"] = recvCommand["alt"].asFloat(); + originJsonValue["TaskName"] = recvCommand["task"].asString(); + Json::StreamWriterBuilder builder; + std::ofstream ofs; + ofs.open(llaOriginPath, std::ios::out); + ofs << Json::writeString(builder, originJsonValue) << std::endl; + ofs.close(); + } } - - } return(true); @@ -346,8 +372,8 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) embeddedState["insVX"] = estimatedState.linearVelocityNorth * 0.01; embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01; embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01; - embeddedState["currentLon"] = estimatedState.currentLon * 0.000001; - embeddedState["currentLat"] = estimatedState.currentLat * 0.000001; + embeddedState["currentLon"] = estimatedState.currentLon; + embeddedState["currentLat"] = estimatedState.currentLat; embeddedState["currentAltitude"] = 0; embeddedState["dvlVX"] = 0; embeddedState["dvlVY"] = 0; @@ -435,7 +461,9 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) ConvertLLAToNED(reference, current, ned); embeddedState["north"] = ned.at(0); embeddedState["east"]= ned.at(1); - + embeddedState["referenceLon"]= estimatedState.referenceLon; + embeddedState["referenceLat"]= estimatedState.referenceLat; + embeddedState["referenceAltitude"]= estimatedState.referenceAltitude; Json::StreamWriterBuilder builder; std::string embeddedStateString = Json::writeString(builder, embeddedState); @@ -447,19 +475,16 @@ void BoardSupportComm::tcpProcessThread() { while(1) { - //bzero(tcpReceiveBuffer, 0); - //int lens = read(tcpSockFD, (unsigned char* )tcpReceiveBuffer, sizeof(tcpReceiveBuffer)); + bzero(tcpReceiveBuffer, 0); int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer)); - // std::cout << MOOS::Time() << " recv:" << lens << std::endl; if(lens>0) { parseMessage((unsigned char* )tcpReceiveBuffer, lens); - // parseMessage(tcpReceiveBuffer, lens); Json::Value embeddedState; std::string embeddedStateString = convertEmbeddedFormat(embeddedState); - // std::cout << std::fixed << std::setprecision(6) << MOOS::Time() << embeddedStateString << std::endl; Notify("uDevice_monitor_fb", embeddedStateString); +#ifndef MOOS_AUV_SIM Notify("NAV_X", embeddedState["north"].asDouble()); Notify("NAV_Y", embeddedState["east"].asDouble()); Notify("NAV_Z", embeddedState["depth"].asDouble()); @@ -474,6 +499,7 @@ void BoardSupportComm::tcpProcessThread() Notify("NAV_ROLL", embeddedState["roll"].asDouble() * 180 / M_PI); Notify("NAV_PITCH", embeddedState["pitch"].asDouble() * 180 / M_PI); Notify("NAV_YAW", embeddedState["yaw"].asDouble() * 180 / M_PI); +#endif } } } @@ -510,6 +536,7 @@ bool BoardSupportComm::Iterate() return false; } } + // std::cout << "tcpSockFD: " << tcpSockFD << ", tcpConnectRet: " << tcpConnectRet << std::endl; if ((tcpSockFD != -1) && (tcpConnectRet != -1)) { std::thread t1(&BoardSupportComm::tcpProcessThread, this); @@ -552,7 +579,7 @@ bool BoardSupportComm::Iterate() printf("footer: %u\n", embeddedInfo.footer); #endif -#if 0 +#ifdef MOOS_AUV_SIM executeCommand.header = 0xEBA2; //1:[0,1] executeCommand.count = 16; //2:[2,3] executeCommand.size = 21; //3:[4] @@ -576,7 +603,6 @@ bool BoardSupportComm::Iterate() try { write(tcpSockFD, tcpSendBuffer, executeCommand.size); - // sock_tcp_send.write(tcpSendBuffer, executeCommand.size); } catch(const std::exception& e) { @@ -590,18 +616,6 @@ bool BoardSupportComm::Iterate() return true; } -uint16_t BoardSupportComm::imcPollTCP(char* buffer, int bufferLength) -{ - if (m_poll_1.poll(0)) - { - bzero(buffer, 0); - uint16_t rv = sock_tcp_send.read(buffer, bufferLength); - return rv; - } - - return 0; -} - template inline uint16_t BoardSupportComm::serialize(const Type t, uint8_t* bfr, uint16_t& sumSize) { @@ -696,10 +710,44 @@ int BoardSupportComm::parseMessage(unsigned char* buffer, int size) bool BoardSupportComm::OnStartUp() { // AppCastingMOOSApp::OnStartUp(); + m_MissionReader.GetValue("llaOriginPath", llaOriginPath); + try + { + struct stat info; + if (stat(llaOriginPath.c_str(), &info) == 0) + { + std::ifstream ifs; + ifs.open(llaOriginPath, std::ios::in); - m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon); - m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat); - m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude); + Json::Reader reader; + Json::Value originJsonValue; + reader.parse(ifs, originJsonValue); + ifs.close(); + + if (originJsonValue.isMember("LongOrigin") && + originJsonValue.isMember("LatOrigin") && + originJsonValue.isMember("AltOrigin")) + { + estimatedState.referenceLon = originJsonValue["LongOrigin"].asFloat(); + estimatedState.referenceLat = originJsonValue["LatOrigin"].asFloat(); + estimatedState.referenceAltitude = originJsonValue["AltOrigin"].asFloat(); + } + else + { + throw -1; + } + } + else + { + throw -1; + } + } + catch(...) + { + m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon); + m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat); + m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude); + } std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", " << estimatedState.referenceLat << ", " diff --git a/src/pBoardSupportComm/BoardSupportComm.h b/src/pBoardSupportComm/BoardSupportComm.h index 8cba0f9..974524b 100644 --- a/src/pBoardSupportComm/BoardSupportComm.h +++ b/src/pBoardSupportComm/BoardSupportComm.h @@ -77,7 +77,6 @@ struct AUVExecuteCommand }; struct EstimatedState { - std::string info; float referenceLon; float referenceLat; float referenceAltitude; @@ -113,18 +112,6 @@ protected: // Standard MOOSApp functions to overload // bool buildReport(); private: - // double uMotion_posX_fb; - // double uMotion_posY_fb; - // double uMotion_posZ_fb; - // double uMotion_roll_fb; - // double uMotion_pitch_fb; - // double uMotion_yaw_fb; - // double uMotion_longitude_fb; - // double uMotion_latitude_fb; - // double uMotion_velocityX_fb; - // double uMotion_velocityY_fb; - // double uMotion_velocityZ_fb; - // double uMotion_deep_fb; struct EstimatedState estimatedState; @@ -146,12 +133,8 @@ private: std::string convertEmbeddedFormat(Json::Value &embeddedState); int tcpSockFD; int tcpConnectRet; - // unsigned char tcpReceiveBuffer[65535]; char tcpReceiveBuffer[65535]; unsigned char tcpSendBuffer[65535]; - DUNE::IO::Poll m_poll_1; - DUNE::Network::TCPSocket sock_tcp_send; - uint16_t imcPollTCP(char* buffer, int bufferLength); int parseMessage(unsigned char* buffer, int size); int serializeMessage(unsigned char* buffer); @@ -165,6 +148,8 @@ private: bool testCount = false; int sockfd = -1; int connectFlg = -1; + std::string llaOriginPath; + }; diff --git a/src/pClientViewer/ClientViewer.cpp b/src/pClientViewer/ClientViewer.cpp index db6f14b..9b6ad27 100644 --- a/src/pClientViewer/ClientViewer.cpp +++ b/src/pClientViewer/ClientViewer.cpp @@ -71,7 +71,7 @@ bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail) double mtime = msg.GetTime(); bool mdbl = msg.IsDouble(); bool mstr = msg.IsString(); - std::cout << key << " : " << sval << std::endl; + std::cout << key << " : " << sval << std::endl; if(key == "Command") { if (sval == "SetPlan1") //PlanDB @@ -204,7 +204,55 @@ bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail) msg.type = DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST; msg.command = 1; tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT); - } + } + if (sval == "GetLogBook") //PlanControl + { + DUNE::IMC::LogBookControl msg; + msg.setTimeStamp(); + msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET; + DUNE::IMC::LogBookEntry msgLogBookEntry; + msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO; + msgLogBookEntry.context = "LIST"; + msg.msg.push_back(msgLogBookEntry); + tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT); + } + if (sval == "RetrLogBook") //PlanControl + { + DUNE::IMC::LogBookControl msg; + msg.setTimeStamp(); + msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET; + DUNE::IMC::LogBookEntry msgLogBookEntry; + msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO; + msgLogBookEntry.context = "RETR"; + + Json::Value retrFile; + retrFile["file"].append("lauv-150/2023-11-16/103058/auvData.txt"); + retrFile["file"].append("lauv-150/2023-11-16/103058/clientCommand.txt"); + retrFile["file"].append("lauv-150/2023-11-16/105131/missionHistory.txt"); + Json::StreamWriterBuilder builder; + msgLogBookEntry.text = Json::writeString(builder, retrFile); + msg.msg.push_back(msgLogBookEntry); + tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT); + } + if (sval == "DeleLogBook") //PlanControl + { + DUNE::IMC::LogBookControl msg; + msg.setTimeStamp(); + msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_CLEAR; + DUNE::IMC::LogBookEntry msgLogBookEntry; + msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO; + msgLogBookEntry.context = "DELE"; + + Json::Value retrFile; + retrFile["file"].append("lauv-150/2023-11-16/103058/auvData.txt"); + // retrFile["file"].append("lauv-150/2023-11-17/195131/missionHistory.txt"); + retrFile["file"].append("lauv-150/2023-11-16/103058/clientCommand.txt"); + retrFile["file"].append("lauv-150/2023-11-16/105131/missionHistory.txt"); + Json::StreamWriterBuilder builder; + msgLogBookEntry.text = Json::writeString(builder, retrFile); + msg.msg.push_back(msgLogBookEntry); + tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT); + } } } @@ -222,7 +270,6 @@ bool ClientViewer::OnConnectToServer() bool ClientViewer::Iterate() { -#if 1 DUNE::IMC::Message * receiveUDPMessage; while ((receiveUDPMessage = imcPollUDP()) != NULL) @@ -230,90 +277,91 @@ bool ClientViewer::Iterate() processMessage(receiveUDPMessage); free(receiveUDPMessage); } -#endif - -# if 1 + DUNE::IMC::Message * receiveTCPMessage; while ((receiveTCPMessage = imcPollTCP()) != NULL) { processMessage(receiveTCPMessage); free(receiveTCPMessage); } -#endif return(true); } -bool processMessageCallback(DUNE::IMC::Message * message) -{ - int type = message->getId(); -#if 1 - if (type == DUNE::IMC::Announce::getIdStatic()) - { - DUNE::IMC::Announce * msg = dynamic_cast(message); - // printf("server receive %s: %lf, %lf, %lf, %lf\n", \ - // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height); - printf("server receive %s: %lf, %lf, %lf, %lf\n", \ - msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height); - } -#endif - if (type == DUNE::IMC::PlanDB::getIdStatic()) - { - DUNE::IMC::PlanDB * msg = dynamic_cast(message); - printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op); - printf("%s\n", msg->info.c_str()); - } -#if 1 - if (type == DUNE::IMC::PlanControlState::getIdStatic()) - { - DUNE::IMC::PlanControlState * msg = dynamic_cast(message); +// bool processMessageCallback(DUNE::IMC::Message * message) +// { +// int type = message->getId(); +// if (type == DUNE::IMC::Announce::getIdStatic()) +// { +// DUNE::IMC::Announce * msg = dynamic_cast(message); +// // printf("server receive %s: %lf, %lf, %lf, %lf\n", \ +// // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height); +// printf("server receive %s: %lf, %lf, %lf, %lf\n", \ +// msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height); +// } +// if (type == DUNE::IMC::PlanDB::getIdStatic()) +// { +// DUNE::IMC::PlanDB * msg = dynamic_cast(message); +// printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op); +// printf("%s\n", msg->info.c_str()); +// } +// if (type == DUNE::IMC::PlanControlState::getIdStatic()) +// { +// DUNE::IMC::PlanControlState * msg = dynamic_cast(message); - printf("server receive %s: %lf, %s, %s, %u\n", \ - msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state); - } - if (type == DUNE::IMC::MsgList::getIdStatic()) - { - DUNE::IMC::MsgList * msgList = dynamic_cast(message); - printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); - DUNE::IMC::MessageList::const_iterator iter1 = msgList->msgs.begin(); - for (; iter1 != msgList->msgs.end(); ++iter1) - { - DUNE::IMC::EntityParameters *entityParameter = static_cast(*iter1); - DUNE::IMC::MessageList::const_iterator iter2 = entityParameter->params.begin(); - for (; iter2 != entityParameter->params.end(); ++iter2) - { - DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); - std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; - } - } - } - if (type == DUNE::IMC::EstimatedState::getIdStatic()) - { - DUNE::IMC::EstimatedState * msg = dynamic_cast(message); - // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", - // msg->getName(), msg->getTimeStamp(), - // msg->lat, msg->lon, msg->depth, - // msg->phi, msg->theta, msg->psi); - printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", - msg->getName(), msg->getTimeStamp(), - msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI, - msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI); - } - if (type == DUNE::IMC::VehicleState::getIdStatic()) - { - DUNE::IMC::VehicleState * msg = dynamic_cast(message); - printf("server receive %s: %lf, %u\n", - msg->getName(), msg->getTimeStamp(), msg->op_mode); - } - if (type == DUNE::IMC::PlanControl::getIdStatic()) - { - DUNE::IMC::PlanControl * msg = dynamic_cast(message); - printf("server receive %s: %lf, %u,\n", - msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str()); - } -#endif - return true; -} +// printf("server receive %s: %lf, %s, %s, %u\n", \ +// msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state); +// } +// if (type == DUNE::IMC::MsgList::getIdStatic()) +// { +// DUNE::IMC::MsgList * msgList = dynamic_cast(message); +// printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); +// DUNE::IMC::MessageList::const_iterator iter1 = msgList->msgs.begin(); +// for (; iter1 != msgList->msgs.end(); ++iter1) +// { +// DUNE::IMC::EntityParameters *entityParameter = static_cast(*iter1); +// DUNE::IMC::MessageList::const_iterator iter2 = entityParameter->params.begin(); +// for (; iter2 != entityParameter->params.end(); ++iter2) +// { +// DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); +// std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; +// } +// } +// } +// if (type == DUNE::IMC::EstimatedState::getIdStatic()) +// { +// DUNE::IMC::EstimatedState * msg = dynamic_cast(message); +// // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", +// // msg->getName(), msg->getTimeStamp(), +// // msg->lat, msg->lon, msg->depth, +// // msg->phi, msg->theta, msg->psi); +// printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", +// msg->getName(), msg->getTimeStamp(), +// msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI, +// msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI); +// } +// if (type == DUNE::IMC::VehicleState::getIdStatic()) +// { +// DUNE::IMC::VehicleState * msg = dynamic_cast(message); +// printf("server receive %s: %lf, %u\n", +// msg->getName(), msg->getTimeStamp(), msg->op_mode); +// } +// if (type == DUNE::IMC::LogBookControl::getIdStatic()) +// { +// DUNE::IMC::LogBookControl * msg = dynamic_cast(message); +// printf("server receive %s: %lf, %u\n", +// msg->getName(), msg->getTimeStamp(), msg->command); + +// DUNE::IMC::MessageList::const_iterator iter = msg->msg.begin(); +// for (; iter != msg->msg.end(); ++iter) +// { +// DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast(*iter); +// std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->text << std::endl; +// } +// } + +// return true; +// } //--------------------------------------------------------- // Procedure: OnStartUp() @@ -333,12 +381,9 @@ bool ClientViewer::OnStartUp() // tcpCommEvent.SetCallback(processMessageCallback,NULL); // tcpCommEvent.Start(); -#if 1 sock_udp_receive.bind(UDP_RECEIVE_PORT, DUNE::Network::Address::Any, false); m_poll_0.add(sock_udp_receive); -#endif -#if 1 try { // sock_tcp_send.setSendTimeout(5); @@ -351,16 +396,11 @@ bool ClientViewer::OnStartUp() { std::cerr << e.what() << '\n'; } -#endif - -#if 1 char local_ip2[INET_ADDRSTRLEN] = {0}; get_local_ip_using_create_socket(local_ip2); ethernetIP = local_ip2; -#endif - RegisterVariables(); return(true); @@ -668,71 +708,74 @@ bool ClientViewer::tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, i void ClientViewer::processMessage(DUNE::IMC::Message * message) { int type = message->getId(); - if (type == DUNE::IMC::Announce::getIdStatic()) - { - DUNE::IMC::Announce * msg = dynamic_cast(message); - // printf("server receive %s: %lf, %lf, %lf, %lf\n", \ - // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height); - printf("server receive %s: %lf, %lf, %lf, %lf\n", \ - msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height); - } - else if (type == DUNE::IMC::PlanDB::getIdStatic()) - { - DUNE::IMC::PlanDB * msg = dynamic_cast(message); - printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op); - printf("%s\n", msg->info.c_str()); - } - else if (type == DUNE::IMC::PlanControlState::getIdStatic()) - { - DUNE::IMC::PlanControlState * msg = dynamic_cast(message); + // if (type == DUNE::IMC::Announce::getIdStatic()) + // { + // DUNE::IMC::Announce * msg = dynamic_cast(message); + // // printf("server receive %s: %lf, %lf, %lf, %lf\n", \ + // // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height); + // printf("server receive %s: %lf, %lf, %lf, %lf\n", \ + // msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height); + // } + // if (type == DUNE::IMC::PlanDB::getIdStatic()) + // { + // DUNE::IMC::PlanDB * msg = dynamic_cast(message); + // printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op); + // printf("%s\n", msg->info.c_str()); + // } + // if (type == DUNE::IMC::PlanControlState::getIdStatic()) + // { + // DUNE::IMC::PlanControlState * msg = dynamic_cast(message); - printf("server receive %s: %lf, %s, %s, %u\n", \ - msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state); - } - else if (type == DUNE::IMC::MsgList::getIdStatic()) + // printf("server receive %s: %lf, %s, %s, %u\n", \ + // msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state); + // } + // if (type == DUNE::IMC::MsgList::getIdStatic()) + // { + // DUNE::IMC::MsgList * msgList = dynamic_cast(message); + // printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); + // DUNE::IMC::MessageList::const_iterator iter1 = msgList->msgs.begin(); + // for (; iter1 != msgList->msgs.end(); ++iter1) + // { + // DUNE::IMC::EntityParameters *entityParameter = static_cast(*iter1); + // DUNE::IMC::MessageList::const_iterator iter2 = entityParameter->params.begin(); + // for (; iter2 != entityParameter->params.end(); ++iter2) + // { + // DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); + // std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; + // } + // } + // } + // if (type == DUNE::IMC::EstimatedState::getIdStatic()) + // { + // DUNE::IMC::EstimatedState * msg = dynamic_cast(message); + // // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", + // // msg->getName(), msg->getTimeStamp(), + // // msg->lat, msg->lon, msg->depth, + // // msg->phi, msg->theta, msg->psi); + // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", + // msg->getName(), msg->getTimeStamp(), + // msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI, + // msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI); + // } + // if (type == DUNE::IMC::VehicleState::getIdStatic()) + // { + // DUNE::IMC::VehicleState * msg = dynamic_cast(message); + // printf("server receive %s: %lf, %u\n", + // msg->getName(), msg->getTimeStamp(), msg->op_mode); + // } + if (type == DUNE::IMC::LogBookControl::getIdStatic()) { - DUNE::IMC::MsgList * msgList = dynamic_cast(message); - printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); - DUNE::IMC::MessageList::const_iterator iter1 = msgList->msgs.begin(); - for (; iter1 != msgList->msgs.end(); ++iter1) + DUNE::IMC::LogBookControl * msg = dynamic_cast(message); + printf("server receive %s: %lf, %u\n", + msg->getName(), msg->getTimeStamp(), msg->command); + + DUNE::IMC::MessageList::const_iterator iter = msg->msg.begin(); + for (; iter != msg->msg.end(); ++iter) { - DUNE::IMC::EntityParameters *entityParameter = static_cast(*iter1); - DUNE::IMC::MessageList::const_iterator iter2 = entityParameter->params.begin(); - for (; iter2 != entityParameter->params.end(); ++iter2) - { - DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); - std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; - } + DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast(*iter); + std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->context << ", " << msgLogBookEntry->text << std::endl; } } - else if (type == DUNE::IMC::EstimatedState::getIdStatic()) - { - DUNE::IMC::EstimatedState * msg = dynamic_cast(message); - // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", - // msg->getName(), msg->getTimeStamp(), - // msg->lat, msg->lon, msg->depth, - // msg->phi, msg->theta, msg->psi); - printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", - msg->getName(), msg->getTimeStamp(), - msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI, - msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI); - } - else if (type == DUNE::IMC::VehicleState::getIdStatic()) - { - DUNE::IMC::VehicleState * msg = dynamic_cast(message); - printf("server receive %s: %lf, %u\n", - msg->getName(), msg->getTimeStamp(), msg->op_mode); - } - else if (type == DUNE::IMC::PlanControl::getIdStatic()) - { - DUNE::IMC::PlanControl * msg = dynamic_cast(message); - printf("server receive %s: %lf, %u,\n", - msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str()); - } - else - { - - } } //--------------------------------------------------------- diff --git a/src/pDataManagement/DataManagement.cpp b/src/pDataManagement/DataManagement.cpp index 5466ea4..48808ee 100644 --- a/src/pDataManagement/DataManagement.cpp +++ b/src/pDataManagement/DataManagement.cpp @@ -6,9 +6,9 @@ /************************************************************/ #include +#include #include "MBUtils.h" #include "DataManagement.h" -//#include "NavigationInfo.pb.h" #include using namespace std; @@ -17,18 +17,50 @@ using namespace std; // Constructor DataManagement::DataManagement() { + + nDoublePrecision = 5; + //AUV状态信息:0~99 + logVarList["uMotion_pose_log"] = 0; + //任务历史信息:100~199 + logVarList["uMission_task_log"] = 100; + //客户端操作记录:200~299 + logVarList["uClient_plandbSet_log"] = 200; + logVarList["uClient_plandbGet_log"] = 201; + logVarList["uClient_parameterSet_log"] = 202; + logVarList["uClient_planControl_log"] = 203; + logVarList["uClient_manualEnable_log"] = 204; + logVarList["uClient_manualDrive_log"] = 205; + //错误日志:300~399 } //--------------------------------------------------------- // Destructor DataManagement::~DataManagement() { + if(auvDataStream.is_open()) + { + auvDataStream.close(); + } + if(missionHistoryStream.is_open()) + { + missionHistoryStream.close(); + } + if(clientCommandStream.is_open()) + { + clientCommandStream.close(); + } + if(faultLogStream.is_open()) + { + faultLogStream.close(); + } } //--------------------------------------------------------- // Procedure: OnNewMail bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) { + DoAsyncLog(NewMail); + MOOSMSG_LIST::iterator p; for(p=NewMail.begin(); p!=NewMail.end(); p++) @@ -44,89 +76,124 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) bool mdbl = msg.IsDouble(); bool mstr = msg.IsString(); - std::string estimatedStateString; - if(key == "uMotion_pose_fb") + if(key == "uDevice_monitor_fb") { - estimatedStateString = msg.GetString(); std::string err; Json::Value estimatedStateData; + std::istringstream iss(msg.GetString()); Json::CharReaderBuilder builder; - const std::unique_ptr reader(builder.newCharReader()); - if (!reader->parse(estimatedStateString.c_str(), - estimatedStateString.c_str() + static_cast(estimatedStateString.length()), - &estimatedStateData, - &err)) - { - std::cout << "error" << std::endl; - return EXIT_FAILURE; - - } - std::string info = estimatedStateData["info"].asString(); - float currentLon = estimatedStateData["currentLon"].asFloat(); - float currentLat = estimatedStateData["currentLat"].asFloat(); - float currentAltitude = estimatedStateData["currentAltitude"].asFloat(); - float referenceLon = estimatedStateData["referenceLon"].asFloat(); - float referenceLat = estimatedStateData["referenceLat"].asFloat(); - float referenceAltitude = estimatedStateData["referenceAltitude"].asFloat(); - float offsetNorth = estimatedStateData["offsetNorth"].asFloat(); - float offsetEast = estimatedStateData["offsetEast"].asFloat(); - float offsetDown = estimatedStateData["offsetDown"].asFloat(); - float roll = estimatedStateData["roll"].asFloat(); - float pitch = estimatedStateData["pitch"].asFloat(); - float yaw = estimatedStateData["yaw"].asFloat(); - float linearVelocityNorth = estimatedStateData["linearVelocityNorth"].asFloat(); - float linearVelocityEast = estimatedStateData["linearVelocityEast"].asFloat(); - float linearVelocityDown = estimatedStateData["linearVelocityDown"].asFloat(); - float angularVelocityNorth = estimatedStateData["angularVelocityNorth"].asFloat(); - float angularVelocityEast = estimatedStateData["angularVelocityEast"].asFloat(); - float angularVelocityDown = estimatedStateData["angularVelocityDown"].asFloat(); - float height = estimatedStateData["height"].asFloat(); - float depth = estimatedStateData["depth"].asFloat(); - - std::cout << "info: " << info << std::endl; - std::cout << "referenceLon: " << referenceLon << std::endl; - std::cout << "referenceLat: " << referenceLat << std::endl; - std::cout << "referenceAltitude: " << referenceAltitude << std::endl; - std::cout << "currentLon: " << currentLon << std::endl; - std::cout << "currentLat: " << currentLat << std::endl; - std::cout << "altitude: " << currentAltitude << std::endl; - std::cout << "offsetNorth: " << offsetNorth << std::endl; - std::cout << "offsetEast: " << offsetEast << std::endl; - std::cout << "offsetDown: " << offsetDown << std::endl; - std::cout << "roll: " << roll << std::endl; - std::cout << "pitch: " << pitch << std::endl; - std::cout << "yaw: " << yaw << std::endl; - std::cout << "linearVelocityNorth: " << linearVelocityNorth << std::endl; - std::cout << "linearVelocityEast: " << linearVelocityEast << std::endl; - std::cout << "linearVelocityDown: " << linearVelocityDown << std::endl; - std::cout << "angularVelocityNorth: " << angularVelocityNorth << std::endl; - std::cout << "angularVelocityEast: " << angularVelocityEast << std::endl; - std::cout << "angularVelocityDown: " << angularVelocityDown << std::endl; - std::cout << "height: " << height << std::endl; - std::cout << "depth: " << depth << std::endl; - + bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err); + if (!parsingResult) + { + std::cerr << "Failed to parse JSON string." << std::endl; + return false; + } std::stringstream ss; - ss << std::fixed << std::setprecision(6) << getTimeStamp() << "," - << info << "," - << referenceLon << "," - << referenceLat << "," - << referenceAltitude << "," - << currentLon << "," - << currentLat << "," - << currentAltitude << "," - << offsetNorth << "," - << offsetEast << "," - << offsetDown << "," - << roll << "," - << pitch << "," - << yaw << "," - << height << "," - << depth << std::endl; + ss << std::fixed << std::setprecision(6) << MOOS::Time() << "," + << estimatedStateData["driveMode"].asUInt() << "," + << estimatedStateData["referenceLon"].asFloat() << "," + << estimatedStateData["referenceLat"].asFloat() << "," + << estimatedStateData["referenceAltitude"].asFloat() << "," + << estimatedStateData["currentLon"].asFloat() << "," + << estimatedStateData["currentLat"].asFloat() << "," + << estimatedStateData["currentAltitude"].asFloat() << "," + << estimatedStateData["north"].asFloat() << "," + << estimatedStateData["east"].asFloat() << "," + << estimatedStateData["depth"].asFloat() << "," + << estimatedStateData["roll"].asFloat() << "," + << estimatedStateData["pitch"].asFloat() << "," + << estimatedStateData["yaw"].asFloat() << "," + << estimatedStateData["insVX"].asFloat() << "," + << estimatedStateData["insVY"].asFloat() << "," + << estimatedStateData["insVZ"].asFloat() << "," + << estimatedStateData["dvlVX"].asFloat() << "," + << estimatedStateData["dvlVY"].asFloat() << "," + << estimatedStateData["dvlVZ"].asFloat() << "," + << estimatedStateData["height"].asFloat() << "," + << estimatedStateData["rpm"].asInt() << "," + << estimatedStateData["lightEnable"].asUInt() << "," + << estimatedStateData["throwingLoadEnable"].asUInt() << "," + << estimatedStateData["dvlStatus"].asUInt() << "," + << estimatedStateData["iridium"].asUInt() << "," + << estimatedStateData["batteryVoltage"].asUInt() << "," + << estimatedStateData["batteryLevel"].asUInt() << "," + << estimatedStateData["batteryTemp"].asFloat(); Notify("uMotion_pose_log", ss.str()); } - } + + if(key == "uMission_task_fb") + { + std::string err; + Json::Value missionStatusObject; + std::istringstream iss(msg.GetString()); + Json::CharReaderBuilder builder; + bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err); + std::stringstream ss; + ss << std::fixed << std::setprecision(6) << MOOS::Time() << "," + << missionStatusObject["state"].asInt() << "," + << missionStatusObject["taskName"].asString() << "," + << missionStatusObject["destName"].asString() << "," + << missionStatusObject["errorCode"].asUInt(); + Notify("uMission_task_log", ss.str()); + } + } - return(true); + return(true); +} + +bool DataManagement::DoAsyncLog(MOOSMSG_LIST &NewMail) +{ + MOOSMSG_LIST::iterator q; + std::stringstream sStream; + int saveFileIndex = -1; + for(q = NewMail.begin();q!=NewMail.end();q++) + { + CMOOSMsg & rMsg = *q; + if(logVarList.find(rMsg.m_sKey)!=logVarList.end()) + { + //ok we have a name at column nVar...(numerical order retained in vector) + std::map::iterator q = logVarList.find(rMsg.m_sKey); + saveFileIndex = q->second; + + std::stringstream sEntry; + sEntry.setf(ios::left); + sEntry.setf(ios::fixed); + if(rMsg.IsDataType(MOOS_STRING) || rMsg.IsDataType(MOOS_DOUBLE)) + { + sEntry << rMsg.GetAsString(12,nDoublePrecision) << ' '; + } + + if ((saveFileIndex >= 0) && (saveFileIndex < 100)) + { + if(auvDataStream.is_open()) + { + auvDataStream << sEntry.str() << std::endl; + } + } + if ((saveFileIndex >= 100) && (saveFileIndex < 200)) + { + if(missionHistoryStream.is_open()) + { + missionHistoryStream << sEntry.str() << std::endl; + } + } + if ((saveFileIndex >= 200) && (saveFileIndex < 300)) + { + if(clientCommandStream.is_open()) + { + clientCommandStream << sEntry.str() << std::endl; + } + } + if ((saveFileIndex >= 300) && (saveFileIndex < 400)) + { + if(faultLogStream.is_open()) + { + faultLogStream << sEntry.str() << std::endl; + } + } + } + } + return true; } //--------------------------------------------------------- @@ -144,6 +211,10 @@ bool DataManagement::OnConnectToServer() bool DataManagement::Iterate() { + auvDataStream.flush(); + missionHistoryStream.flush(); + clientCommandStream.flush(); + faultLogStream.flush(); return(true); } @@ -170,6 +241,67 @@ bool DataManagement::OnStartUp() } } } + std::string saveLogDir; + m_MissionReader.GetValue("LogDir", saveLogDir); + std::string vehicleName; + m_MissionReader.GetValue("VehicleName", vehicleName); + bool logEnable = false; + m_MissionReader.GetValue("LogEnable", logEnable); + + std::string auvDataFile; + m_MissionReader.GetValue("AuvDataLog", auvDataFile); + std::string missionHistoryFile; + m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile); + std::string clientCommandFile; + m_MissionReader.GetValue("ClientCommandLog", clientCommandFile); + std::string faultLogFile; + m_MissionReader.GetValue("FaultLog", faultLogFile); + + if (logEnable) + { + if (access(saveLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveLogDir.c_str(), mode); + } + saveLogDir += "/" + vehicleName; + if (access(saveLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveLogDir.c_str(), mode); + } + + std::string subDir; + std::string localTime; + GenerateFileName(subDir, localTime); + saveLogDir += "/" + subDir; + if (access(saveLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveLogDir.c_str(), mode); + } + saveLogDir += "/" + localTime; + if (access(saveLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveLogDir.c_str(), mode); + } + std::string auvDataSavePath = saveLogDir + "/" + auvDataFile; + std::string missionHistorySavePath = saveLogDir + "/" + missionHistoryFile; + std::string clientCommandSavePath = saveLogDir + "/" + clientCommandFile; + std::string faultLogSavePath = saveLogDir + "/" + faultLogFile; + + if(!OpenFile(auvDataStream, auvDataSavePath)) + return MOOSFail("Failed to Open auvData file"); + if(!OpenFile(missionHistoryStream, missionHistorySavePath)) + return MOOSFail("Failed to Open missionHistory file"); + if(!OpenFile(clientCommandStream, clientCommandSavePath)) + return MOOSFail("Failed to Open clientCommand file"); + if(!OpenFile(faultLogStream, faultLogSavePath)) + return MOOSFail("Failed to Open faultLog file"); + + DoLogBanner(auvDataStream); + } RegisterVariables(); return(true); @@ -180,15 +312,130 @@ bool DataManagement::OnStartUp() void DataManagement::RegisterVariables() { - // Register("FOOBAR", 0); - Register("uMotion_pose_fb", 0); - //Register("uMotion_pose_log", 0); + Register("uDevice_monitor_fb", 0); + Register("uMission_task_fb", 0); + Register("uMission_task_log", 0); + Register("uMotion_pose_log", 0); + Register("uClient_plandbSet_log", 0); + Register("uClient_plandbGet_log", 0); + Register("uClient_parameterSet_log", 0); + Register("uClient_planControl_log", 0); + Register("uClient_manualEnable_log", 0); + Register("uClient_manualDrive_log", 0); } -double DataManagement::getTimeStamp() +// double DataManagement::getTimeStamp() +// { +// struct timeval tv; +// gettimeofday(&tv,NULL); +// double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000; +// return stamp; +// } + +bool DataManagement::DoLogBanner(std::ofstream &os) { - struct timeval tv; - gettimeofday(&tv,NULL); - double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000; - return stamp; + os << "stamp" << ","; + os << "mode" << ","; + os << "refLon" << ","; + os << "refLat" << ","; + os << "refAlt" << ","; + os << "curLon" << ","; + os << "curLat" << ","; + os << "curAlt" << ","; + os << "north" << ","; + os << "east" << ","; + os << "depth" << ","; + os << "roll" << ","; + os << "pitch" << ","; + os << "yaw" << ","; + os << "insVX" << ","; + os << "insVY" << ","; + os << "insVZ" << ","; + os << "dvlVX" << ","; + os << "dvlVY" << ","; + os << "dvlVZ" << ","; + os << "height" << ","; + os << "thrust" << ","; + os << "light" << ","; + os << "load" << ","; + os << "dvl" << ","; + os << "iridium" << ","; + os << "batteryVol" << ","; + os << "batteryLev" << ","; + os << "batteryTemp" << std::endl; + return true; +} + +bool DataManagement::OpenFile(std::ofstream & of,const std::string & sName) +{ + of.open(sName.c_str()); + + if(!of.is_open()) + { + return false; + } + return true; +} + +void DataManagement::GenerateFileName(std::string &fileDir, std::string &fileName) +{ + time_t now = time(0); + tm *gmtm = localtime(&now); + int year = 1900 + gmtm->tm_year; + int monthTemp = gmtm->tm_mon+1; + std::string month; + if (monthTemp < 10) + { + month = "0" + std::to_string(monthTemp); + } + else + { + month = std::to_string(monthTemp); + } + int dayTemp = gmtm->tm_mday; + std::string day; + if (dayTemp < 10) + { + day = "0" + std::to_string(dayTemp); + } + else + { + day = std::to_string(dayTemp); + } + int hourTemp = gmtm->tm_hour; + std::string hour; + if (hourTemp < 10) + { + hour = "0" + std::to_string(hourTemp); + } + else + { + hour = std::to_string(hourTemp); + } + int minuteTemp = gmtm->tm_min; + std::string minute; + if (minuteTemp < 10) + { + minute = "0" + std::to_string(minuteTemp); + } + else + { + minute = std::to_string(minuteTemp); + } + int secondTemp = gmtm->tm_sec; + std::string second; + if (secondTemp < 10) + { + second = "0" + std::to_string(secondTemp); + } + else + { + second = std::to_string(secondTemp); + } + std::stringstream ss; + ss << year << "-" << month << "-" << day; + fileDir = ss.str(); + ss.str(""); + ss << hour << minute << second; + fileName = ss.str(); } \ No newline at end of file diff --git a/src/pDataManagement/DataManagement.h b/src/pDataManagement/DataManagement.h index 80b588e..9c959c6 100644 --- a/src/pDataManagement/DataManagement.h +++ b/src/pDataManagement/DataManagement.h @@ -21,12 +21,22 @@ class DataManagement : public CMOOSApp bool Iterate(); bool OnConnectToServer(); bool OnStartUp(); + bool DoAsyncLog(MOOSMSG_LIST & NewMail); protected: - void RegisterVariables(); + void RegisterVariables(); + // double getTimeStamp(); + bool OpenFile(std::ofstream & of,const std::string & sName); + bool DoLogBanner(std::ofstream &os); + void GenerateFileName(std::string &fileDir, std::string &fileName); + + int nDoublePrecision; + std::ofstream auvDataStream; + std::ofstream missionHistoryStream; + std::ofstream clientCommandStream; + std::ofstream faultLogStream; + std::map logVarList; - private: - double getTimeStamp(); }; #endif diff --git a/src/pEmulator/Emulator.cpp b/src/pEmulator/Emulator.cpp index 34a4830..0c63a73 100644 --- a/src/pEmulator/Emulator.cpp +++ b/src/pEmulator/Emulator.cpp @@ -2,7 +2,7 @@ * @Author: zjk 1553836110@qq.com * @Date: 2023-10-12 09:52:27 * @LastEditors: zjk 1553836110@qq.com - * @LastEditTime: 2023-11-08 17:33:41 + * @LastEditTime: 2023-11-10 08:42:57 * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp * @Description: * diff --git a/src/pEmulator/Emulator.hpp b/src/pEmulator/Emulator.hpp index 87aad10..fbd6b8c 100644 --- a/src/pEmulator/Emulator.hpp +++ b/src/pEmulator/Emulator.hpp @@ -2,7 +2,7 @@ * @Author: zjk 1553836110@qq.com * @Date: 2023-10-12 15:57:27 * @LastEditors: zjk 1553836110@qq.com - * @LastEditTime: 2023-11-08 17:24:35 + * @LastEditTime: 2023-11-10 08:34:14 * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.hpp * @Description: * diff --git a/src/pEmulator/_150server.hpp b/src/pEmulator/_150server.hpp index c12d1c5..f7049c5 100644 --- a/src/pEmulator/_150server.hpp +++ b/src/pEmulator/_150server.hpp @@ -91,5 +91,6 @@ public: AUVCmd _150cmd; unsigned char embeddedBuffer[59]; unsigned char recvbuf[65535] = {0}; + unsigned int _150_recive = 0; }; #endif \ No newline at end of file diff --git a/src/pFaultHandle/.vscode/settings.json b/src/pFaultHandle/.vscode/settings.json new file mode 100644 index 0000000..73912a6 --- /dev/null +++ b/src/pFaultHandle/.vscode/settings.json @@ -0,0 +1,12 @@ +{ + "files.associations": { + "array": "cpp", + "deque": "cpp", + "list": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "string_view": "cpp", + "initializer_list": "cpp" + } +} \ No newline at end of file diff --git a/src/pFaultHandle/CMakeLists.txt b/src/pFaultHandle/CMakeLists.txt new file mode 100644 index 0000000..4ab9f64 --- /dev/null +++ b/src/pFaultHandle/CMakeLists.txt @@ -0,0 +1,32 @@ +#-------------------------------------------------------- +# The CMakeLists.txt for: pFaultHandle +# Author(s): wade +#-------------------------------------------------------- + + + +SET(SRC + FaultHandle.cpp + FaultHandle_Info.cpp + main.cpp + FaultHandle.h +) + +# FIND_LIBRARY(/usr/local/lib) +# FIND_PATH(/usr/local/include) + +include_directories(/usr/local/include/jsoncpp/) +link_directories(/usr/local/lib/) + +ADD_EXECUTABLE(pFaultHandle ${SRC}) + +TARGET_LINK_LIBRARIES(pFaultHandle + ${MOOS_LIBRARIES} + ${CMAKE_DL_LIBS} + ${SYSTEM_LIBS} + mbutil + pthread + jsoncpp +) + + diff --git a/src/pFaultHandle/FaultHandle.cpp b/src/pFaultHandle/FaultHandle.cpp new file mode 100644 index 0000000..e0a3bda --- /dev/null +++ b/src/pFaultHandle/FaultHandle.cpp @@ -0,0 +1,1366 @@ +/************************************************************/ +/* NAME: wade */ +/* ORGN: MIT */ +/* FILE: FaultHandle.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "FaultHandle.h" +#include "ACTable.h" +#include +#include + +using namespace std; + +//--------------------------------------------------------- +// Constructor + +FaultHandle::FaultHandle() +{ + m_watch_all_db = true; + m_watch_all_antler = true; + m_min_wait = -1; + m_noted_gone_wait = 10; + m_allow_retractions = -1; + m_proc_watch_summary_changed = false; + m_last_posting_time = 0; + m_Mission_fault_fb = 0; //任务管理故障状态 + m_Motion_fault_fb = 0; //运动控制故障状态 + + m_faultBattery = 0; + m_faultEmergencyBattery = 0; + m_faultLeakSensor =0; + m_faultThrust = 0; + m_batteryLevel = 80; + m_CPU_temperature = 30.0; //CPU温度初始化 + + + //初始化故障上报列表 + uFH_ErrorMsg_fb["Time"] = "null"; + uFH_ErrorMsg_fb["Fault"] = "trouble-free"; + uFH_ErrorMsg_fb["FaultID"] = 0; + uFH_ErrorMsg_fb["FaultLevel"] = 0; + uFH_ErrorMsg_fb["Source"] = "null"; + + errorMsgInit(); + + +} + +//--------------------------------------------------------- +// Destructor + +FaultHandle::~FaultHandle() +{ +} + +//--------------------------------------------------------- +// Procedure: OnNewMail + +bool FaultHandle::OnNewMail(MOOSMSG_LIST &NewMail) +{ + AppCastingMOOSApp::OnNewMail(NewMail); + MOOSMSG_LIST::iterator p; + + for(p=NewMail.begin(); p!=NewMail.end(); p++) { + CMOOSMsg &msg = *p; + + //------------------------------- + string key = msg.GetKey(); + if(key == "DB_CLIENTS") { + m_db_clients = msg.GetString(); + handleMailNewDBClients(); + } + else if(strEnds(key, "_STATUS")) + { + handleMailStatusUpdate(msg.GetString()); + } + + else if(key == "EXITED_NORMALLY") + { + m_excused_list.push_back(msg.GetString()); + } + else if (key == "uMission_fault_fb") + { + m_Mission_fault_fb = (int)msg.GetDouble(); + } + else if(key == "uMotion_fault_fb") + { + m_Motion_fault_fb = (int)msg.GetDouble(); + } + else if(key == "uDevice_monitor_fb") + { + std::string DeviceStatusString = msg.GetString(); + std::string errors; + + std::istringstream iss(DeviceStatusString); + Json::CharReaderBuilder builder; + + bool parsingSuccessful = Json::parseFromStream(builder, iss, &Device_monitor, &errors); + if (parsingSuccessful) + { + m_batteryLevel = Device_monitor["batteryLevel"].asUInt(); + m_faultBattery = Device_monitor["faultBattery"].asUInt(); + m_faultEmergencyBattery = Device_monitor["faultEmergencyBattery"].asUInt(); + m_faultLeakSensor = Device_monitor["faultLeakSensor"].asUInt(); + m_faultThrust = Device_monitor["faultThrust"].asUInt(); + // 解析成功，可以通过Device_monitor变量访问Json数据了 + } + else + { + std::cerr << "Failed to parse the JSON: " << errors << std::endl; + } + } + else + reportRunWarning("Unhandled Mail: " + key); + } + + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnConnectToServer + +bool FaultHandle::OnConnectToServer() +{ + RegisterVariables(); + return(true); +} + +//--------------------------------------------------------- +// Procedure: Iterate() +// happens AppTick times per second + +bool FaultHandle::Iterate() +{ + AppCastingMOOSApp::Iterate(); + //---------------------------------------- + // bool post_based_on_time = false; + // if(m_min_wait >= 0) { + // double moos_elapsed_time = m_curr_time - m_last_posting_time; + // double real_elapsed_time = moos_elapsed_time; + // if(m_time_warp > 0) + // real_elapsed_time = moos_elapsed_time / m_time_warp; + // if(real_elapsed_time > m_min_wait) + // post_based_on_time = true; + // } + + // if(m_proc_watch_summary_changed || post_based_on_time) { + // //AppCastingMOOSApp::Notify(postVar("PROC_WATCH_SUMMARY"), m_proc_watch_summary); + // m_last_posting_time = m_curr_time; + // // if(m_proc_watch_summary == "All Present") + // // Notify("PROC_WATCH_ALL_OK", "true"); + // // else + // // Notify("PROC_WATCH_ALL_OK", "false"); + // } + + + if(m_proc_watch_summary_changed) { + checkForIndividualUpdates(); + //postFullSummary(); + //m_map_alive_prev = m_map_alive; + } + + m_proc_watch_summary_changed = false; + + CPUTempMonitor();//监控CPU温度并实时反馈 + //---------------------------------------- + AppCastingMOOSApp::PostReport(); + + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnStartUp() +// happens before connection is open + +bool FaultHandle::OnStartUp() +{ + AppCastingMOOSApp::OnStartUp(); + + STRING_LIST sParams; + m_MissionReader.EnableVerbatimQuoting(false); + if(!m_MissionReader.GetConfiguration(GetAppName(), sParams)) + reportConfigWarning("No config block found for " + GetAppName()); + + STRING_LIST::iterator p; + for(p = sParams.begin(); p!=sParams.end(); p++) { + string orig = *p; + string line = *p; + string param = tolower(biteStringX(line, '=')); + string value = line; + + if(param == "watch") + handleConfigWatchList(value); + else if(param == "nowatch") + handleConfigExcludeList(value); + else if(param == "watch_all") { + string lvalue = tolower(value); + if(lvalue == "true") { + m_watch_all_antler = true; + m_watch_all_db = true; + } + else if(lvalue == "false") { + m_watch_all_antler = false; + m_watch_all_db = false; + } + else if(lvalue == "antler") { + m_watch_all_antler = true; + m_watch_all_db = false; + } + else if((lvalue == "dbclients") || (lvalue == "db_clients")) { + m_watch_all_antler = false; + m_watch_all_db = true; + } + else + reportConfigWarning("Invalide WATCH_ALL value:" + value); + } + else if(param == "summary_wait") { + if(isNumber(value)) + m_min_wait = atof(value.c_str()); + else + reportConfigWarning("SUMMARY_WAIT must be numerical value"); + } + else if(param == "allow_retractions") { + if(tolower(value) == "true") + m_allow_retractions = -1; + else if(tolower(value) == "false") + m_allow_retractions = 0; + else if(isNumber(value)) + m_allow_retractions = atof(value.c_str()); + else + reportConfigWarning("Param ALLOW_RETRACTIONS must be Boolean or numerial"); + } + else if(param == "post_mapping") + handlePostMapping(value); + else + reportUnhandledConfigWarning(orig); + } + + if(m_watch_all_antler == true) + populateAntlerList(); + + RegisterVariables(); + return(true); +} + +//buildReport测试信息发送 +//--------------------------------------------------------- +bool FaultHandle::buildReport() +{ + map::iterator p; + for(p=m_map_alive.begin(); p!=m_map_alive.end(); p++) { + string proc_name = p->first; + bool proc_here = p->second; + + if(proc_here) + { + //actab.addCell("OK", "reversegreen");//在FaultHandle可视化界面中显示 + //Notify("FaultHandle_Status" + proc_name, "OK"); + MoosAppStatus[proc_name]["Status"] = "OK"; + } + else { + if(vectorContains(m_excused_list, proc_name)) + { + //If the APP exits with MOOSDB's permission, the status changes to "EXCUSED" + //actab.addCell("EXCUSED", "reverseblue"); + MoosAppStatus[proc_name ]["Status"] = "EXCUSED"; + + } + else + { + //actab.addCell("MISSING", "reversered"); + MoosAppStatus[proc_name ]["Status"] = "MISSING"; + } + } + + //actab << doubleToString(m_map_now_cpuload[proc_name], 2); + MoosAppStatus[proc_name]["CpuLoad"] = doubleToString(m_map_now_cpuload[proc_name], 2); + //actab << doubleToString(m_map_max_cpuload[proc_name], 2); + } + //m_msgs << "TestFlag:" << testflag++ << endl; + + DeviceLeakageMonitor(); + MoosAppStatusMonitor(); + MoosAppOverloadMonitor(); + MissionFaultMonitor(); + BatteryStatusMonitor(); + EmergencyBatteryMonitor(); + ThrustStatusMonitor(); + + uFH_errorMsg1_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg3_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + + //std::string uFH_ErrorMsg_fb_str = uFH_ErrorMsg_fb.toStyledString(); + std::string uFH_errorMsg1_fb_str = uFH_errorMsg1_fb.toStyledString(); + std::string uFH_errorMsg2_fb_str = uFH_errorMsg2_fb.toStyledString(); + std::string uFH_errorMsg3_fb_str = uFH_errorMsg3_fb.toStyledString(); + + std::string MoosAppStatus_str = MoosAppStatus.toStyledString(); + + //Notify("uFH_ErrorMsg_fb", uFH_ErrorMsg_fb_str); + Notify("uFH_errorMsg1_fb", uFH_errorMsg1_fb_str); + Notify("uFH_errorMsg2_fb", uFH_errorMsg2_fb_str); + Notify("uFH_errorMsg3_fb", uFH_errorMsg3_fb_str); + + Notify("MoosAppStatus", MoosAppStatus_str); + + + //m_msgs << "\n"<< MoosAppStatus; + m_msgs << "\n"<< uFH_errorMsg1_fb; + m_msgs << "\n"<< uFH_errorMsg2_fb; + m_msgs << "\n"<< uFH_errorMsg3_fb; + + Notify("m_curr_time", m_curr_time); + //m_msgs << "\n"<< m_curr_time; + return true; +} + +//--------------------------------------------------------- +// Procedure: RegisterVariables + +void FaultHandle::RegisterVariables() +{ + AppCastingMOOSApp::RegisterVariables(); + + + Register("DB_CLIENTS", 0); + Register("EXITED_NORMALLY", 0); + Register("TIME", 0); + Register("uMission_fault_fb", 0); + Register("uDevice_monitor_fb", 0); + Register("uMotion_fault_fb", 0); + + + + #if 0 + Register("漏水信息", 0); + Register("电池信息", 0); + Register("任务超深", 0); + Register("任务超时", 0); + Register("任务超区域", 0); + #endif +} +//------------------------------------------------------------ +// Procedure: handleMailNewDBClients + +void FaultHandle::handleMailNewDBClients() +{ + // Phase 1: Possibly expand watchlist given current DB_CLIENTS + if(m_watch_all_db) { + vector svector = parseString(m_db_clients, ','); + unsigned int i, vsize = svector.size(); + for(i=0; i m_noted_gone_wait) { + if(!vectorContains(m_excused_list, proc)) { + if(m_map_alive[proc]) + procNotedGone(proc); + if(m_proc_watch_summary == "All Present") + m_proc_watch_summary = "AWOL: " + proc; + else + m_proc_watch_summary += "," + proc; + } + else { // proc IS excused and newly gone + if(m_map_alive[proc]) + procNotedExcused(proc); + } + } + } + else { + if(!m_map_alive[proc]) + procNotedHere(proc); + } + } + + if(prev_summary != m_proc_watch_summary) + m_proc_watch_summary_changed = true; +} + +//------------------------------------------------------------ +// Procedure: handleMailStatusUpdate + +void FaultHandle::handleMailStatusUpdate(string status) +{ + string procname = tokStringParse(status, "MOOSName", ',', '='); + string cpuload = tokStringParse(status, "cpuload", ',', '='); + + if(procname == "") { + reportRunWarning("Unhandled STATUS update: Missing MOOSName."); + return; + } + + if(cpuload == "") { + reportRunWarning("Unhandled STATUS update: Missing cpuload for " + procname); + return; + } + + double d_cpuload = atof(cpuload.c_str()); + + m_map_now_cpuload[procname] = d_cpuload; + if(d_cpuload > m_map_max_cpuload[procname]) + m_map_max_cpuload[procname] = d_cpuload; +} + +//----------------------------------------------------------------- +// Procedure: isAlive +// Purpose: Check the given process name against the current list +// of processes from DB_CLIENTS + +bool FaultHandle::isAlive(string procname) +{ + procname = stripBlankEnds(procname); + + vector svector = parseString(m_db_clients, ','); + unsigned int i, vsize = svector.size(); + + for(i=0; i svector = parseString(pnames, ','); + unsigned int i, vsize = svector.size(); + + if(vsize == 0) { + reportConfigWarning("Empty list provided to WATCH config param"); + return(false); + } + + bool handled = true; + for(i=0; i 0) && (procname.at(len-1) == '*')) { + prefix = true; + procname = procname.substr(0, len-1); + } + + // Check to see if this process requests a dedicated posting + // A procname of say "pFooBar:FOO_PRESENT" will request a separate + // MOOS variable FOO_PRESENT report on the presence or absense + // of the pFooBar process. + string proc = biteStringX(procname, ':'); + string post = procname; + procname = proc; + if((!prefix) && (post != "")) + m_map_proc_post[proc] = post; + + // Check to see if the process name is already present + unsigned int i, vsize = m_include_list.size(); + for(i=0; i svector = parseString(pnames, ','); + unsigned int i, vsize = svector.size(); + + if(vsize == 0) { + reportConfigWarning("Empty list provided to NOWATCH config param"); + return(false); + } + + bool handled = true; + for(i=0; i 0) && (procname.at(len-1) == '*')) { + prefix = true; + procname = procname.substr(0, len-1); + } + + // Check to see if the process name is already present + unsigned int i, vsize = m_exclude_list.size(); + for(i=0; i::iterator p; + for(p=m_map_proc_post.begin(); p!=m_map_proc_post.end(); p++) { + string proc = p->first; + string proc_post = p->second; + + bool alive_prev = m_map_alive_prev[proc]; + bool alive_curr = m_map_alive[proc]; + // if(alive_prev != alive_curr) + // Notify(proc_post, boolToString(alive_curr)); + } +} + +//----------------------------------------------------------------- +#if 0 +// Procedure: postFullSummary +// Note: + +void FaultHandle::postFullSummary() +{ + string full_summary; + unsigned int i, vsize = m_watch_list.size(); + + for(i=0; i 0) + full_summary += ","; + full_summary += msg; + } + + if(full_summary == "") + full_summary = "No processes to watch"; + + Notify(postVar("PROC_WATCH_FULL_SUMMARY"), full_summary); +} +#endif + + + +//----------------------------------------------------------------- +// Procedure: procNotedHere + +void FaultHandle::procNotedHere(string procname) +{ + procname = stripBlankEnds(procname); + + m_map_noted_here[procname]++; + m_map_alive[procname] = true; + + string msg; + if(m_map_noted_here[procname] <= 1) + msg += "Noted to be present: [" + procname + "]"; + else { + msg += "Resurrected: [" + procname + "]"; + + bool allow_this_retraction = false; + if(m_allow_retractions < 0) + allow_this_retraction = true; + else { + double moos_elapsed_time = m_curr_time - m_last_posting_time; + double real_elapsed_time = moos_elapsed_time; + if(m_time_warp > 0) + real_elapsed_time = moos_elapsed_time / m_time_warp; + if(real_elapsed_time <= m_allow_retractions) + allow_this_retraction = true; + } + + // Retraction String must match exactly. See XYZ below. + if(allow_this_retraction) + retractRunWarning("Process [" + procname + "] is missing."); + } + + // Notify(postVar("PROC_WATCH_EVENT"), msg); + reportEvent(msg); +} + +//----------------------------------------------------------------- +// Procedure: procNotedGone + +void FaultHandle::procNotedGone(string procname) +{ + procname = stripBlankEnds(procname); + + m_map_noted_gone[procname]++; + m_map_alive[procname] = false; + + // Run Warning must match exactly when/if retracting. See XYZ above. + string msg = "Process [" + procname + "] is missing."; + //Notify(postVar("PROC_WATCH_EVENT"), msg); + + reportEvent("PROC_WATCH_EVENT: " + msg); + reportRunWarning(msg); +} + +//----------------------------------------------------------------- +// Procedure: procNotedExcused + +void FaultHandle::procNotedExcused(string procname) +{ + procname = stripBlankEnds(procname); + + m_map_noted_gone[procname]++; + m_map_alive[procname] = false; + + // Run Warning must match exactly when/if retracting. See XYZ above. + string msg = "Process [" + procname + "] is gone but excused."; + //Notify(postVar("PROC_WATCH_EVENT"), msg); + + reportEvent("PROC_WATCH_EVENT: " + msg); +} + + +//----------------------------------------------------------------- +// Procedure: handlePostMapping + +void FaultHandle::handlePostMapping(string mapping) +{ + string left = biteStringX(mapping, ','); + string right = mapping; + + if(!strContainsWhite(right)) + m_map_chgpost[left] = right; +} + +//----------------------------------------------------------------- +// Procedure: postVar + +string FaultHandle::postVar(string varname) +{ + map::iterator p = m_map_chgpost.find(varname); + if(p == m_map_chgpost.end()) + return(varname); + else + return(p->second); +} + +//----------------------------------------------------------------- +// Procedure: populateAntlerList + +void FaultHandle::populateAntlerList() +{ + STRING_LIST sParams; + m_MissionReader.GetConfiguration("ANTLER", sParams); + + STRING_LIST::iterator p; + for(p = sParams.begin();p!=sParams.end();p++) { + string sLine = *p; + string param = tolower(biteStringX(sLine, '=')); + string value = stripBlankEnds(sLine); + + if(param == "run") { + string app = biteStringX(value, '@'); + + // Handle case where app is launched under an alias name with ~ alias + biteStringX(value, '~'); + if(value != "") + app = value; + + if((app != "") && (app != "uProcessWatch") && !strBegins(app, "MOOSDB")) { + m_set_antler_clients.insert(app); + bool added = addToWatchList(app); + if(added) + procNotedHere(app); + } + } + } +} + +//----------------------------------------------------------------- +// Procedure: processExcluded +// Purpose: Determine if this process is to be excluded based on +// NOWATCH params. Either by an explicit match or by a +// pattern match. + +bool FaultHandle::processExcluded(const string& procname) +{ + bool excluded = false; + unsigned int i, vsize = m_exclude_list.size(); + for(i=0; i 80) + // { + // uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["FaultID"] = "pOdometryTEST"; + // } + if(MoosAppStatus["pBoardSupportComm"]["CpuLoad"] > 100) + { + uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["FaultID"] = 111; + } + else if(MoosAppStatus["pDataManagement"]["CpuLoad"] > 80) + { + uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["FaultID"] = 112; + } + else if(MoosAppStatus["pMotionControl"]["CpuLoad"] > 80) + { + uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["FaultID"] = 113; + } + else if(MoosAppStatus["pSurfaceSupportComm"]["CpuLoad"] > 80) + { + uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["FaultID"] = 114; + } + else if(MoosAppStatus["pTaskManagement"]["CpuLoad"] > 80) + { + uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["FaultID"] = 115; + } + else + { + uFH_errorMsg1_fb["FaultMsgs"]["uSoftWare_appOverload_fault"]["FaultID"] = 999; + } + +} +} + +//--------------------------------------------------------------------------- +//用于监控TaskManagement反馈的任务状态并作出处理 +void FaultHandle::MissionFaultMonitor() +{ + static bool firstRecvFlag = false; + static double FirstRecvTime = 0; + if(m_Mission_fault_fb != 0 && (firstRecvFlag == false)) + { + FirstRecvTime = MOOSTime(); + firstRecvFlag = true; + } + if(m_Mission_fault_fb != 0) + { + //--------------------------------------------------------------- + //test + m_msgs << "m_Mission_fault_fb:" << m_Mission_fault_fb << endl; + m_msgs << "FirstRecvTime:" << FirstRecvTime << endl; + //---------------------------------------------------------------- + + uFH_errorMsg2_fb["FaultLevel"] = 2; + //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["Source"] = "TaskManagement"; + switch (m_Mission_fault_fb) + { + case 1: + uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FaultID"] = 71;//超出规定深度 + break; + case 2: + uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FaultID"] = 72;//超出规定时间 + break; + case 3: + uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FaultID"] = 73;//超出安全区域 + break; + // case 4: + // uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FaultID"] = 74;//超出规定最小高度（暂时不用，保留） + // break; + case 11: + uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FaultID"] = 75; + break; + case 12: + case 13: + case 14: + uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FaultID"] = 76; + break; + //FaultID从77-90为任务管理故障预留位置 + // case 99: + // uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FaultID"] = "77-90"; + // break; + default: + uFH_errorMsg2_fb["FaultMsgs"]["uMission_status_fault"]["FaultID"] = 999; + break; + } + } + +} +//------------------------------------------------------------------------- +//用于监控MotionControl反馈的运动状态并作出处理 +void FaultHandle::MotionControlMonitor()//20231109未完成 +{ + static bool firstRecvFlag = false; + static double FirstRecvTime = 0; + if((m_faultLeakSensor != 0) && (firstRecvFlag == false)) + { + FirstRecvTime = MOOSTime(); + firstRecvFlag = true; + } + if(m_Motion_fault_fb == 1) + { + uFH_errorMsg2_fb["FaultLevel"] = 2; + //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg2_fb["FaultMsgs"]["uMotion_fault_fb"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg2_fb["FaultMsgs"]["uMotion_fault_fb"]["Source"] = "MotionControl"; + uFH_errorMsg2_fb["FaultMsgs"]["uMotion_fault_fb"]["FaultID"] = 131; + + } + else if(m_Motion_fault_fb > 2) + { + uFH_errorMsg2_fb["FaultLevel"] = 2; + //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg2_fb["FaultMsgs"]["uMotion_fault_fb"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg2_fb["FaultMsgs"]["uMotion_fault_fb"]["Source"] = "MotionControl"; + uFH_errorMsg2_fb["FaultMsgs"]["uMotion_fault_fb"]["FaultID"] = 133; + } + else if(m_Motion_fault_fb == 2) + { + uFH_errorMsg1_fb["FaultLevel"] = 1; + //uFH_errorMsg1_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg1_fb["FaultMsgs"]["uMotion_fault_fb"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg1_fb["FaultMsgs"]["uMotion_fault_fb"]["Source"] = "MotionControl"; + uFH_errorMsg1_fb["FaultMsgs"]["uMotion_fault_fb"]["FaultID"] = 132; + } + else + { + uFH_errorMsg1_fb["FaultMsgs"]["uMotion_fault_fb"]["FaultID"] = 999;//未知错误 + } +} +//------------------------------------------------------------------------- +//获取AUV设备信息，判断漏水状态 +void FaultHandle::DeviceLeakageMonitor() +{ + static bool firstRecvFlag = false; + static double FirstRecvTime = 0; + if((m_faultLeakSensor != 0) && (firstRecvFlag == false)) + { + FirstRecvTime = MOOSTime(); + firstRecvFlag = true; + } + + if(m_faultLeakSensor != 0) + { + uFH_errorMsg3_fb["FaultLevel"] = 3; + //uFH_errorMsg3_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["Source"] = "AUV"; + switch (m_faultLeakSensor) + { + case 1: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 1; + break; + case 2: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 2; + break; + case 4: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 3; + break; + case 8: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 4; + break; + case 16: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 5; + break; + case 32: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 6; + break; + case 64: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 7; + break; + case 128: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 8; + break; + case 256: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 9; + break; + case 512: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 10; + break; + case 1024: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 11; + break; + case 2048: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 12; + break; + default: + uFH_errorMsg3_fb["FaultMsgs"]["uSensor_ leakage_fault"]["FaultID"] = 999;//未知错误 + break; + } + } +} +//------------------------------------------------------------------------- +//获取AUV设备信息，判断电池状态 +void FaultHandle::BatteryStatusMonitor() +{ + static bool firstRecvFlag = false; + static double FirstRecvTime = 0; + if((m_faultBattery != 0) && (firstRecvFlag == false)) + { + FirstRecvTime = MOOSTime(); + firstRecvFlag = true; + } + + if(m_batteryLevel < 20) + { + uFH_errorMsg1_fb["FaultLevel"] = 1; + //uFH_errorMsg1_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg1_fb["FaultMsgs"]["uBattery_status_fault"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg1_fb["FaultMsgs"]["uBattery_status_fault"]["Source"] = "AUV"; + uFH_errorMsg1_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 37; + } + if(m_faultBattery != 0) + { + uFH_errorMsg2_fb["FaultLevel"] = 2; + //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["Source"] = "AUV"; + switch (m_faultBattery) + { + case 1: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 21; + + break; + case 2: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 22; + break; + case 4: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 23; + break; + case 8: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 24; + break; + case 16: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 25; + break; + case 32: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 26; + break; + case 64: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 27; + break; + case 128: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 28; + break; + default: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 999; + break; + } + } +} +//------------------------------------------------------------------------- +//获取AUV设备信息，判断以及反馈应急电池状态//20231110修改 +void FaultHandle::EmergencyBatteryMonitor() +{ + static bool firstRecvFlag = false; + static double FirstRecvTime = 0; + if((m_faultEmergencyBattery != 0) && (firstRecvFlag == false)) + { + FirstRecvTime = MOOSTime(); + firstRecvFlag = true; + } + + if(m_faultEmergencyBattery != 0) + { + uFH_errorMsg2_fb["FaultLevel"] = 2; + //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["Source"] = "AUV"; + switch (m_faultEmergencyBattery) + { + case 1: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 29; + + break; + case 2: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 30; + break; + case 4: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 31; + break; + case 8: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 32; + break; + case 16: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 33; + break; + case 32: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 34; + break; + case 64: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 35; + break; + case 128: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 36; + break; + default: + uFH_errorMsg2_fb["FaultMsgs"]["uBattery_status_fault"]["FaultID"] = 999; + break; + } + } +} + +//------------------------------------------------------------------------- +//获取AUV设备信息，判断以及反馈推进器状态 +void FaultHandle::ThrustStatusMonitor() +{ + + static bool firstRecvFlag = false; + static double FirstRecvTime = 0; + if((m_faultThrust != 0) && (firstRecvFlag == false)) + { + FirstRecvTime = MOOSTime(); + firstRecvFlag = true; + } + + if(m_faultThrust != 0) + { + uFH_errorMsg2_fb["FaultLevel"] = 2; + //uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg2_fb["FaultMsgs"]["uPropeller_status_fault"]["FirstRecvTime"] = FirstRecvTime; + uFH_errorMsg2_fb["FaultMsgs"]["uPropeller_status_fault"]["Source"] = "AUV"; + + switch (m_faultThrust) + { + case 1: + uFH_errorMsg2_fb["FaultMsgs"]["uPropeller_status_fault"]["FaultID"] = 51; + break; + case 2: + uFH_errorMsg2_fb["FaultMsgs"]["uPropeller_status_fault"]["FaultID"] = 52; + break; + default: + uFH_errorMsg2_fb["FaultMsgs"]["uPropeller_status_fault"]["FaultID"] = 999; + break; + } + } + + +} + + +//------------------------------------------------------------------------- +//added by zhangwei 20231101 +//获取当前时间（秒），转换为string格式时间，用于故障日志记录 +string FaultHandle::convertSecondsToString(double seconds) +{ + // 获取当前时间的时间点 + //time_t now = (int)seconds; + time_t now = time(0); + + tm* localTime = localtime(&now); + // 设置时间格式 + stringstream ss; + ss << setw(2) << setfill('0') << localTime->tm_year + 1900 << '-' + << setw(2) << setfill('0') << localTime->tm_mon + 1 << '-' + << setw(2) << setfill('0') << localTime->tm_mday << ' ' + << setw(2) << setfill('0') << localTime->tm_hour << ':' + << setw(2) << setfill('0') << localTime->tm_min << ':' + << setw(2) << setfill('0') << localTime->tm_sec; + // 返回时间字符串 + return ss.str(); +} + +//--------------------------------------------------------------------------- +//获取CPU温度并反馈到MOOSDB +void FaultHandle::CPUTempMonitor() +{ + std::ifstream file("/sys/class/thermal/thermal_zone0/temp"); + if (file.is_open()) { + std::string line; + if (std::getline(file, line)) + { + std::istringstream iss(line); + if (iss >> m_CPU_temperature) + { + //std::ostringstream os; + m_CPU_temperature = m_CPU_temperature/1000; + //os << m_CPU_temperature; + //std::string CPUTemperature = os.str() + "°C"; + Notify("CPUTemperature", m_CPU_temperature); + } + else + { + Notify("CPUTemperature", "Failed to parse CPU temperature"); + } + } + else + { + Notify("CPUTemperature", "Failed to read CPU temperature"); + } + file.close(); + } + else + { + Notify("CPUTemperature", "Failed to open CPU temperature file"); + } + +} + +void FaultHandle::errorMsgInit() +{ + uFH_errorMsg1_fb["FaultLevel"] = 0; + uFH_errorMsg1_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg1_fb["FaultMsgs"]; + + uFH_errorMsg2_fb["FaultLevel"] = 0; + uFH_errorMsg2_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg2_fb["FaultMsgs"]; + + uFH_errorMsg3_fb["FaultLevel"] = 0; + uFH_errorMsg3_fb["CurrentTime"] = convertSecondsToString(m_curr_time); + uFH_errorMsg3_fb["FaultMsgs"]; +} + +//----------------------------------------------------------------------------- +//MOOSAPP运行是否异常 +bool FaultHandle::MoosAppMissing() +{ + if ((MoosAppStatus["pOdometry"]["Status"] == "MISSING") || + (MoosAppStatus["pBoardSupportComm"]["Status"] == "MISSING")|| + (MoosAppStatus["pDataManagement"]["Status"] == "MISSING")|| + (MoosAppStatus["pMotionControl"]["Status"] == "MISSING")|| + (MoosAppStatus["pSurfaceSupportComm"]["Status"] == "MISSING")|| + (MoosAppStatus["pTaskManagement"]["Status"] == "MISSING")) + { + return true; + } + else + return false; +} + +//----------------------------------------------------------------------- +//MOOSAPP占用CPU是否超限 +bool FaultHandle::MoosAppOverload() +{ + if ((MoosAppStatus["pBoardSupportComm"]["CpuLoad"] > 80)|| + (MoosAppStatus["pDataManagement"]["CpuLoad"] > 80)|| + (MoosAppStatus["pMotionControl"]["CpuLoad"] > 80)|| + (MoosAppStatus["pSurfaceSupportComm"]["CpuLoad"] > 80)|| + (MoosAppStatus["pTaskManagement"]["CpuLoad"] > 80)) + { + return true; + } + else + return false; +} +//----------------------------------------------------------------------- \ No newline at end of file diff --git a/src/pFaultHandle/FaultHandle.h b/src/pFaultHandle/FaultHandle.h new file mode 100644 index 0000000..89d4451 --- /dev/null +++ b/src/pFaultHandle/FaultHandle.h @@ -0,0 +1,170 @@ +/************************************************************/ +/* NAME: wade */ +/* ORGN: MIT */ +/* FILE: FaultHandle.h */ +/* DATE: */ +/************************************************************/ + +#ifndef FaultHandle_HEADER +#define FaultHandle_HEADER + +#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" +#include "MOOS/libMOOS/MOOSLib.h" +//#include "moos.h" +#include +#include +#include +#include +#include +#include +#include +#include + +//class FaultHandle : public CMOOSApp +class FaultHandle : public AppCastingMOOSApp +{ + public: + FaultHandle(); + ~FaultHandle(); + + + protected: // Standard MOOSApp functions to overload + bool OnNewMail(MOOSMSG_LIST &NewMail); + bool Iterate(); + bool OnConnectToServer(); + bool OnStartUp(); + bool buildReport(); + + void RegisterVariables(); + + protected: + void handleMailNewDBClients(); + void handleMailStatusUpdate(std::string); + + bool handleConfigWatchList(std::string); + bool handleConfigWatchItem(std::string); + + bool handleConfigExcludeList(std::string); + bool handleConfigExcludeItem(std::string); + + protected: + bool addToWatchList(std::string); + + void checkForIndividualUpdates(); + void postFullSummary(); + + bool isAlive(std::string); + void procNotedHere(std::string); + void procNotedGone(std::string); + void procNotedExcused(std::string); + + void handlePostMapping(std::string); + std::string postVar(std::string); + + void populateAntlerList(); + + bool processIncluded(const std::string& procname); + bool processExcluded(const std::string& procname); + + protected: + std::string convertSecondsToString(double); + + void errorMsgInit(); + + void MoosAppStatusMonitor(); + void MoosAppOverloadMonitor(); + void MissionFaultMonitor(); + void MotionControlMonitor(); + void DeviceLeakageMonitor(); + void BatteryStatusMonitor(); + void EmergencyBatteryMonitor(); + void ThrustStatusMonitor(); + void CPUTempMonitor(); + + + bool MoosAppMissing(); + bool MoosAppOverload(); + + + protected: // State Variables + bool m_proc_watch_summary_changed; + double m_last_posting_time; + + // Clients and Summary stored across iterations + std::string m_db_clients; + std::string m_proc_watch_summary; + + std::vector m_watch_list; + std::vector m_excused_list; + + // Mapping from proc name to data + std::map m_map_alive; + std::map m_map_alive_prev; + std::map m_map_noted_gone; + std::map m_map_noted_here; + std::map m_map_now_cpuload; + std::map m_map_max_cpuload; + + std::set m_set_db_clients; + std::set m_set_watch_clients; + std::set m_set_antler_clients; + + protected: // Configurations Variables + bool m_watch_all_db; + bool m_watch_all_antler; + double m_allow_retractions; + double m_min_wait; + double m_noted_gone_wait; + + // Include List + std::vector m_include_list; + std::vector m_include_list_prefix; + + // Exclude List + std::vector m_exclude_list; + std::vector m_exclude_list_prefix; + + // A dedicated MOOS var for posting when a proc chgs status + std::map m_map_proc_post; + + // A map for changing the MOOS variable posted. + std::map m_map_chgpost; + + protected: + int m_Mission_fault_fb; + int m_Motion_fault_fb; + double m_CPU_temperature; + + uint32_t m_faultBattery; + uint32_t m_faultEmergencyBattery; + uint32_t m_faultLeakSensor; + uint32_t m_faultThrust; + uint32_t m_batteryLevel; + + Json::Value Device_monitor; + Json::Value MoosAppStatus; + Json::Value uFH_ErrorMsg_fb; + + Json::Value uFH_errorMsg1_fb; + Json::Value uFH_errorMsg2_fb; + Json::Value uFH_errorMsg3_fb; + + protected: + //Contingency variable + + // bool uSensor_leakage_fb; + // bool uMission_deepOut_fb; + // bool uMission_timeOut_fb; + // bool uDrive_batteryLow_fb; + // double uSoftWare_appMiss_fb; //AppID + + //------------------------------------------------ + //subscribe variables + + + private: // Configuration variables + + private: // State variables +}; + +#endif diff --git a/src/pFaultHandle/FaultHandle_Info.cpp b/src/pFaultHandle/FaultHandle_Info.cpp new file mode 100644 index 0000000..0885bcc --- /dev/null +++ b/src/pFaultHandle/FaultHandle_Info.cpp @@ -0,0 +1,115 @@ +/****************************************************************/ +/* NAME: wade */ +/* ORGN: MIT Cambridge MA */ +/* FILE: FaultHandle_Info.cpp */ +/* DATE: Dec 29th 1963 */ +/****************************************************************/ + +#include +#include +#include "FaultHandle_Info.h" +#include "ColorParse.h" +#include "ReleaseInfo.h" + +using namespace std; + +//---------------------------------------------------------------- +// Procedure: showSynopsis + +void showSynopsis() +{ + blk("SYNOPSIS: "); + blk("------------------------------------ "); + blk(" The pFaultHandle application is used for "); + blk(" "); + blk(" "); + blk(" "); + blk(" "); +} + +//---------------------------------------------------------------- +// Procedure: showHelpAndExit + +void showHelpAndExit() +{ + blk(" "); + blu("=============================================================== "); + blu("Usage: pFaultHandle file.moos [OPTIONS] "); + blu("=============================================================== "); + blk(" "); + showSynopsis(); + blk(" "); + blk("Options: "); + mag(" --alias","= "); + blk(" Launch pFaultHandle with the given process name "); + blk(" rather than pFaultHandle. "); + mag(" --example, -e "); + blk(" Display example MOOS configuration block. "); + mag(" --help, -h "); + blk(" Display this help message. "); + mag(" --interface, -i "); + blk(" Display MOOS publications and subscriptions. "); + mag(" --version,-v "); + blk(" Display the release version of pFaultHandle. "); + blk(" "); + blk("Note: If argv[2] does not otherwise match a known option, "); + blk(" then it will be interpreted as a run alias. This is "); + blk(" to support pAntler launching conventions. "); + blk(" "); + exit(0); +} + +//---------------------------------------------------------------- +// Procedure: showExampleConfigAndExit + +void showExampleConfigAndExit() +{ + blk(" "); + blu("=============================================================== "); + blu("pFaultHandle Example MOOS Configuration "); + blu("=============================================================== "); + blk(" "); + blk("ProcessConfig = pFaultHandle "); + blk("{ "); + blk(" AppTick = 4 "); + blk(" CommsTick = 4 "); + blk(" "); + blk("} "); + blk(" "); + exit(0); +} + + +//---------------------------------------------------------------- +// Procedure: showInterfaceAndExit + +void showInterfaceAndExit() +{ + blk(" "); + blu("=============================================================== "); + blu("pFaultHandle INTERFACE "); + blu("=============================================================== "); + blk(" "); + showSynopsis(); + blk(" "); + blk("SUBSCRIPTIONS: "); + blk("------------------------------------ "); + blk(" NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO, "); + blk(" string_val=BAR "); + blk(" "); + blk("PUBLICATIONS: "); + blk("------------------------------------ "); + blk(" Publications are determined by the node message content. "); + blk(" "); + exit(0); +} + +//---------------------------------------------------------------- +// Procedure: showReleaseInfoAndExit + +void showReleaseInfoAndExit() +{ + showReleaseInfo("pFaultHandle", "gpl"); + exit(0); +} + diff --git a/src/pFaultHandle/FaultHandle_Info.h b/src/pFaultHandle/FaultHandle_Info.h new file mode 100644 index 0000000..e5c7f64 --- /dev/null +++ b/src/pFaultHandle/FaultHandle_Info.h @@ -0,0 +1,18 @@ +/****************************************************************/ +/* NAME: wade */ +/* ORGN: MIT Cambridge MA */ +/* FILE: FaultHandle_Info.h */ +/* DATE: Dec 29th 1963 */ +/****************************************************************/ + +#ifndef FaultHandle_INFO_HEADER +#define FaultHandle_INFO_HEADER + +void showSynopsis(); +void showHelpAndExit(); +void showExampleConfigAndExit(); +void showInterfaceAndExit(); +void showReleaseInfoAndExit(); + +#endif + diff --git a/src/pFaultHandle/main.cpp b/src/pFaultHandle/main.cpp new file mode 100644 index 0000000..4fcb35a --- /dev/null +++ b/src/pFaultHandle/main.cpp @@ -0,0 +1,52 @@ +/************************************************************/ +/* NAME: wade */ +/* ORGN: MIT */ +/* FILE: main.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "ColorParse.h" +#include "FaultHandle.h" +#include "FaultHandle_Info.h" + +using namespace std; + +int main(int argc, char *argv[]) +{ + string mission_file; + string run_command = argv[0]; + + for(int i=1; i= 0) && (yawTemp <= M_PI)) { estimatedStateMsg.psi = yawTemp; - std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI - << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl; + // std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI + // << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl; } if ((yawTemp > M_PI) && (yawTemp <= 2 * M_PI)) { estimatedStateMsg.psi = -1 * (2 * M_PI - yawTemp); - std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI - << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl; + // std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI + // << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl; } estimatedStateMsg.vx = estimatedState.linearVelocityNorth; //x axis estimatedStateMsg.vy = estimatedState.linearVelocityEast; //y axis estimatedStateMsg.vz = estimatedState.linearVelocityDown; //z axis -#endif - - double TimeScheduled = TimeLast + 0.5; // double TimeScheduled = TimeLast + 1.0; @@ -349,58 +327,41 @@ bool SurfaceSupportComm::Iterate() double TimeNow = MOOS::Time(); TimeLast = TimeNow; -#if 1 tcpReceiveFromClient(recv_buf, sizeof(recv_buf)/sizeof(char), 0.005); -#endif // AppCastingMOOSApp::PostReport(); return(true); } -int SurfaceSupportComm::simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value) -{ - int result = -1; +// double dfMean = 0; +// double dfVar = 0; +// unsigned int N = 0; - DUNE::IMC::EntityParameter subMsg; - subMsg.name = name; - subMsg.value = value; - entityParameter.params.push_back(subMsg); +// bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * /*pParamCaller*/) +// { +// //lets wait a while +// MOOSPause(2); - result = 0; - return result; -} +// //some stats +// double dfE = TimeNow-TimeScheduled; +// N=N+1; +// dfMean = dfMean*(N-1)/N+(dfE)/N; -double dfMean = 0; -double dfVar = 0; -unsigned int N = 0; +// if(N>1) +// dfVar = dfE/(N-1)+(N-1)/N*dfVar; -bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * /*pParamCaller*/) -{ - //lets wait a while - MOOSPause(2); - - //some stats - double dfE = TimeNow-TimeScheduled; - N=N+1; - dfMean = dfMean*(N-1)/N+(dfE)/N; - - if(N>1) - dfVar = dfE/(N-1)+(N-1)/N*dfVar; - - std::cout.setf(std::ios::fixed); - std::cout<type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_GET_STATE) { @@ -762,6 +682,11 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) DUNE::Utils::ByteBuffer planDBBuffer; DUNE::IMC::Packet::serialize(&planDBMsg, planDBBuffer); tcpSendToClient((char *)(planDBBuffer.getBuffer()), planDBMsg.getSerializationSize()); + std::stringstream ss; + ss << std::fixed << std::setprecision(6) + << "PlanDB Get" << ":" + << MOOS::Time(); + Notify("uClient_plandbGet_log", ss.str()); } } if (type == DUNE::IMC::SetEntityParameters::getIdStatic()) @@ -769,6 +694,10 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) DUNE::IMC::SetEntityParameters * msg = dynamic_cast(message); printf("server receive %s: %lf\n", msg->getName(), msg->getTimeStamp()); DUNE::IMC::MessageList::const_iterator iter = msg->params.begin(); + std::stringstream ss2; + ss2 << std::fixed << std::setprecision(6) + << "SetEntityParameters" << ":" + << MOOS::Time(); for(; iter < msg->params.end(); iter++) { DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter); @@ -815,8 +744,10 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) std::string dataValue; ss >> dataValue; printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str()); - } + } + ss2 << "," << parentName << "," << childName << "," << dataValueTemp; } + Notify("uClient_parameterSet_log", ss2.str()); } if (type == DUNE::IMC::PlanControl::getIdStatic()) { @@ -839,7 +770,7 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) outputTaskValue["action"] = action; Json::StreamWriterBuilder builder2; std::string outputTaskString = Json::writeString(builder2, outputTaskValue); - std::cout << outputTaskString << std::endl; + // std::cout << outputTaskString << std::endl; if (action == "start") { std::ifstream ifs; @@ -852,9 +783,10 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) float originLat = tempJsonValue[msg->plan_id]["origin"]["lat"].asFloat(); float originLon = tempJsonValue[msg->plan_id]["origin"]["lon"].asFloat(); Json::Value outputOrginValue; - outputOrginValue["altitude"] = originAlt; + outputOrginValue["alt"] = originAlt; outputOrginValue["lat"] = originLat; outputOrginValue["lon"] = originLon; + outputOrginValue["task"] = msg->plan_id; Json::StreamWriterBuilder builder1; std::string outputOriginString = Json::writeString(builder1, outputOrginValue); std::cout << outputOriginString << std::endl; @@ -866,7 +798,13 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) { Notify("uMission_action_cmd", outputTaskString); } - + std::stringstream ss; + ss << std::fixed << std::setprecision(6) + << "PlanControl" << ":" + << MOOS::Time() << "," + << msg->plan_id << "," + << action; + Notify("uClient_planControl_log", ss.str()); } } if (type == DUNE::IMC::VehicleCommand::getIdStatic()) @@ -884,113 +822,259 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) Notify("uManual_enable_cmd", (double)msg->command); } } + std::stringstream ss; + ss << std::fixed << std::setprecision(6) + << "VehicleCommand" << ":" + << MOOS::Time() << "," + << (int)msg->command; + Notify("uClient_manualEnable_log", ss.str()); } if (type == DUNE::IMC::RemoteActions::getIdStatic()) { DUNE::IMC::RemoteActions * msg = dynamic_cast(message); printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->actions); Notify("uManual_drive_cmd", msg->actions); + std::string err; + Json::Value recvCommand; + std::istringstream iss(msg->actions); + Json::CharReaderBuilder builder; + bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); + std::stringstream ss; + ss << std::fixed << std::setprecision(6) + << "RemoteActions" << ":" + << MOOS::Time() << "," + << "Thrust" << "," + << recvCommand["Thrust"].asInt() << "," + << "Heading" << "," + << recvCommand["Heading"].asFloat(); + Notify("uClient_manualDrive_log", ss.str()); + } + if (type == DUNE::IMC::LogBookControl::getIdStatic()) + { + DUNE::IMC::LogBookControl * msg = dynamic_cast(message); + + uint8_t logBookType = 0; + std::string logBookContext = ""; + std::string logBookText = ""; + DUNE::IMC::MessageList::const_iterator iter = msg->msg.begin(); + for (; iter != msg->msg.end(); ++iter) + { + DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast(*iter); + logBookType = msgLogBookEntry->type; + logBookContext = msgLogBookEntry->context; + logBookText = msgLogBookEntry->text; + } + + printf("server receive %s: %lf, %u, %u, %s\n", msg->getName(), msg->getTimeStamp(), + msg->command, logBookType, logBookContext.c_str()); + + std::string saveLogDir; + m_MissionReader.GetValue("LogDir", saveLogDir); + char lastChar = saveLogDir.at(saveLogDir.size()-1); + if (std::string(1, lastChar) != std::string("/")) + { + saveLogDir += "/"; + } + if (msg->command == DUNE::IMC::LogBookControl::CommandEnum::LBC_GET) + { + if (logBookType == DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO && logBookContext == "LIST") + { + std::vector localAUVNamePath; + getAllFiles(saveLogDir.c_str(), localAUVNamePath); + std::vector localDayPath; + for (int i=0; i localTimePath; + for (int i=0; i localFilePath; + for (int i=0; icommand == DUNE::IMC::LogBookControl::CommandEnum::LBC_CLEAR) + { + if (logBookType == DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO && logBookContext == "DELE") + { + std::string err; + Json::Value inputFileObject; + std::istringstream iss(logBookText); + Json::CharReaderBuilder builder1; + + bool parsingResult = Json::parseFromStream(builder1, iss, &inputFileObject, &err); + + if (parsingResult) + { + Json::Value inputFileArray = inputFileObject["file"]; + Json::Value outputFileObject; + for (int i = 0; i < inputFileArray.size(); i++) + { + std::string targetFile = saveLogDir + inputFileArray[i].asString(); + + Json::Value singleFileObject; + struct stat info; + if (stat(targetFile.c_str(), &info) == 0) + { + remove(targetFile.c_str()); + singleFileObject["result"] = "success"; + } + else + { + singleFileObject["result"] = "failure"; + } + outputFileObject[inputFileArray[i].asString()] = singleFileObject; + } + + Json::StreamWriterBuilder builder2; + std::string outputFileString = Json::writeString(builder2, outputFileObject); + DUNE::IMC::LogBookControl msgFB; + msgFB.setTimeStamp(); + msgFB.setSource(header_src); + msgFB.setSourceEntity(header_src_ent); + msgFB.setDestination(header_dst); + msgFB.setDestinationEntity(header_dst_ent); + msgFB.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_REPLY; + + DUNE::IMC::LogBookEntry msgLogBookEntry; + msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO; + msgLogBookEntry.text = outputFileString; + msgLogBookEntry.context = logBookContext; + msgFB.msg.push_back(msgLogBookEntry); + + DUNE::Utils::ByteBuffer logBookControlBuffer; + DUNE::IMC::Packet::serialize(&msgFB, logBookControlBuffer); + tcpSendToClient((char *)(logBookControlBuffer.getBuffer()), msgFB.getSerializationSize()); + } + } + } } } -void SurfaceSupportComm::ParsePlanConfig(Json::Value inputJsonValue) +void SurfaceSupportComm::getAllFiles(const char * dir_name, std::vector& filePath) { - int taskCount = inputJsonValue.size(); - std::vector taskList = inputJsonValue.getMemberNames(); - std::cout << "taskCount: " << taskCount << std::endl; - double lastChangeStamp = 0; - std::string lastChangeSourceAddress; - std::string lastChangeSourceName; - for (int i=0; id_name , "." ) == 0 || strcmp(filename->d_name , "..") == 0 ) + continue; - std::string taskName = subTask["taskName"].asString(); - std::string taskId = subTask["taskId"].asString(); - int priority = subTask["priority"].asInt(); - float duration = subTask["duration"].asFloat(); - bool closedLoop = subTask["closedLoop"].asBool(); - float constSpeed = subTask["constSpeed"].asFloat(); - int repeat = subTask["repeat"].asInt(); - bool perpetual = subTask["perpetual"].asBool(); - float minDepth = subTask["minDepth"].asFloat(); - float maxDepth = subTask["maxDepth"].asFloat(); - std::string sourceName = subTask["sourceName"].asString(); - std::string sourceAddress = subTask["sourceAddress"].asString(); - double boardStamp = subTask["boardStamp"].asDouble(); - double clientStamp = subTask["clientStamp"].asDouble(); - float originLat = subTask["origin"]["lat"].asFloat(); - float originLon = subTask["origin"]["lon"].asFloat(); - float originAlt = subTask["origin"]["altitude"].asFloat(); - - Json::StreamWriterBuilder builder; - std::string subTaskString = Json::writeString(builder, subTask); - - if (boardStamp > lastChangeStamp) - {Json::Value inputJsonValue; - lastChangeStamp = boardStamp; - lastChangeSourceAddress = sourceAddress; - lastChangeSourceName = sourceName; - } - - std::cout << "taskName: " << taskName << std::endl; - std::cout << "taskId: " << taskId << std::endl; - std::cout << "originLon: " << originLon << std::endl; - std::cout << "originLat: " << originLat << std::endl; - std::cout << "originAlt: " << originAlt << std::endl; - std::cout << "sourceName: " << sourceName << std::endl; - std::cout << "sourceAddress: " << sourceAddress << std::endl; - std::cout << "boardStamp: " << boardStamp << std::endl; - std::cout << "clientStamp: " << clientStamp << std::endl; - std::cout << "priority: " << priority << std::endl; - std::cout << "duration: " << duration << std::endl; - std::cout << "closedLoop: " << closedLoop << std::endl; - std::cout << "constSpeed: " << constSpeed << std::endl; - std::cout << "repeat: " << repeat << std::endl; - std::cout << "perpetual: " << perpetual << std::endl; - std::cout << "minDepth: " << minDepth << std::endl; - std::cout << "maxDepth: " << maxDepth << std::endl; - - if (subTask["origin"].isArray()) + char lastChar = dir_name[strlen(dir_name) - 1]; + if (std::string(1, lastChar) == std::string("/")) { - Json::Value array = subTask["origin"]; - if (array.size() == 3) - { - float origin_lon = array[0].asFloat(); - float origin_lat = array[1].asFloat(); - float origin_alt = array[2].asFloat(); - std::cout << "origin: [" << origin_lon << ", " - << origin_lat << ", " << origin_alt << "] " << std::endl; - } + filePath.push_back(std::string(dir_name) + std::string(filename->d_name)); } - - if (subTask["points"].isArray()) + else { - Json::Value array = subTask["points"]; - for (int j = 0; j < array.size(); j++) - { - std::string name = array[j]["name"].asString(); - float lon = array[j]["lon"].asFloat(); - float lat = array[j]["lat"].asFloat(); - float depth = array[j]["depth"].asFloat(); - float speed = array[j]["speed"].asFloat(); - float north = array[j]["north"].asFloat(); - float east = array[j]["east"].asFloat(); - std::string type = array[j]["type"].asString(); - std::cout << "point " << j << ": " - << name << ", " - << lon << ", " - << lat << ", " - << north << ", " - << east << ", " - << depth << ", " - << speed << ", " - << type << ", " - << std::endl; - } + filePath.push_back(std::string(dir_name) + "/" + std::string(filename->d_name)); } } + return ; } void SurfaceSupportComm::BatchConvertCoordinate(Json::Value &object) @@ -1019,14 +1103,16 @@ void SurfaceSupportComm::BatchConvertCoordinate(Json::Value &object) void SurfaceSupportComm::closeConnection(Client& c, std::exception& e) { - // long unsigned int client_count = m_clients.size() - 1; m_poll.remove(*c.socket); delete c.socket; } bool SurfaceSupportComm::OnStartUp() { - // AppCastingMOOSApp::OnStartUp(); + // AppCastingMOOSApp::OnStartUp(); + + m_MissionReader.GetValue("VehicleName", vehicleName); + planConfigPath = ""; m_MissionReader.GetValue("PlanConfigPath", planConfigPath); diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.h b/src/pSurfaceSupportComm/SurfaceSupportComm.h index 456637a..c95e996 100644 --- a/src/pSurfaceSupportComm/SurfaceSupportComm.h +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.h @@ -121,13 +121,13 @@ private: std::vector point_enu, std::vector &point_lla); - void BatchConvertCoordinate(Json::Value &object); - void ParsePlanConfig(Json::Value inputJsonValue); - int simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value); + void BatchConvertCoordinate(Json::Value &object); int retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList); template inline int getEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, Type& value); + void getAllFiles(const char * dir_name, std::vector& filePath); + int testFlag = 0; double TimeLast; int sendCount = 0; @@ -135,6 +135,7 @@ private: PeriodicUDPEvent udpEvent; // PeriodicTCPEvent tcpEvent; std::string missionStatusString; + std::string vehicleName; }; #endif diff --git a/src/pTaskManger/TaskManger.cpp b/src/pTaskManger/TaskManger.cpp index d1f74d5..c988fd2 100644 --- a/src/pTaskManger/TaskManger.cpp +++ b/src/pTaskManger/TaskManger.cpp @@ -48,7 +48,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) string msg_str = msg.GetString(); double msg_dval = msg.GetDouble(); bool msg_bval = msg.GetBinaryData(); - // cout << msg_name + ": " << msg_str << endl; + // 内部消息 if(msg_name == MSG_ENDFLAG) { if(msg_str == "true") @@ -82,7 +82,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) faultNumber = 99; } - if(msg_name == MSG_SENDSAFTRULES && msg_str == "true") + if(msg_name == MSG_SENDSAFTRULES && msg_str == "true") // 暂时没用配置功能 { state = CONFIG; st = 40; @@ -92,7 +92,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) st = 39; state = FAULT; } - + // 交互消息 if(msg_name == MSG_IN_SSM) { Json::Value j; @@ -119,6 +119,20 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) RepList["RunWaring"] = "form SSM have unhandle action : " + j["action"].asString(); } } + + if((msg_name == MSG_FAULT_LEV2) || (msg_name == MSG_FALUT_LEV3)) + { + Json::Value j; + Json::Reader a; + a.parse(msg_str,j); + int falut_num = j["FaultLevel"].asInt(); + if(falut_num != 0) + { + faultNumber = 20; + state = FAULT; + } + } + if(msg_name == MSG_IN_MAN) { @@ -254,7 +268,6 @@ bool TaskManger::Iterate() start = false; Notify(MSG_ENDFLAG,"false"); Notify(MSG_WPTFLAG,"false"); - // Notify(MSG_START,"false"); Notify(MSG_RUN,"false"); Notify("MOOS_MANUAL_OVERRIDE","true"); cout << "Wating task start......" << endl; @@ -283,6 +296,8 @@ bool TaskManger::Iterate() st = 31; else if(faultNumber>10 && faultNumber<20) st = 32; + else if(faultNumber==20) + st = 33; else st = 38; break; @@ -299,6 +314,20 @@ bool TaskManger::Iterate() state = UNRUN; break; } + case 20: //接收到来自其他组件的故障 + { + RepList["FALT"] = "Reresive fault from others"; + current_node_complete = false; + current_pol_complete = false; + current_node=""; + start = false; + Notify(MSG_ENDFLAG,"false"); + Notify(MSG_WPTFLAG,"false"); + Notify(MSG_RUN,"false"); + Notify("MOOS_MANUAL_OVERRIDE","true"); + cout << "Wating task start......" << endl; + st = 21; + } case 38: //未知故障 { postFaultNumber(99); @@ -573,14 +602,17 @@ bool TaskManger::setWayConfig(string lead, string lead_damper, string capture_li void TaskManger::RegisterVariables() { AppCastingMOOSApp::RegisterVariables(); + //内部消息订阅 Register(MSG_ENDFLAG, 0); - // Register(MSG_START, 0); Register(MSG_WPTFLAG,0); Register(MSG_SENDSAFTRULES,0); Register(MSG_FALUT,0); Register(MSG_CLEARFAULT,0); + //交互消息订阅 Register(MSG_IN_SSM,0); Register(MSG_IN_MAN,0); + Register(MSG_FAULT_LEV2,1); + Register(MSG_FALUT_LEV3,1); } /** * @description: read task File to nodeList @@ -941,7 +973,7 @@ bool TaskManger::buildReport() } m_msgs << "=========================================================" << endl; RepList["Current Node"] = current_node; - RepList["remaining number of nodes"] = nodeList.size()+1; + RepList["remaining number of nodes"] = (int)nodeList.size()+1; string rep = Json::writeString(RepJsBuilder, RepList); m_msgs << rep << endl; return true; @@ -992,6 +1024,7 @@ void TaskManger::postReportToSSM() //3.超出区域 //4.低于最小高度 //11:任务文本 +//20:接收到其余程序传递的故障 //99.未知故障 void TaskManger::postFaultNumber(int n) { diff --git a/src/pTaskManger/TaskManger.h b/src/pTaskManger/TaskManger.h index fcb20b8..69d8a67 100644 --- a/src/pTaskManger/TaskManger.h +++ b/src/pTaskManger/TaskManger.h @@ -2,7 +2,7 @@ * @Author: 1553836110 1553836110@qq.com * @Date: 2023-09-28 15:45:17 * @LastEditors: zjk 1553836110@qq.com - * @LastEditTime: 2023-11-08 14:53:05 + * @LastEditTime: 2023-11-15 09:31:32 * @FilePath: /moos-ivp-pi/src/pTaskManger/TaskManger.h * @Description: * @@ -74,7 +74,7 @@ class TaskManger : public AppCastingMOOSApp inline void taskFinish(); void FaultFlagClear(); inline void InitConfig(); - + //内部消息发布 const string UPDATE_WPT = "WPT_UPDATE"; const string UPDATE_SPD = "SPEED_UPDATE"; const string UPDATE_DEP = "DEPTH_UPDATE"; @@ -82,7 +82,7 @@ class TaskManger : public AppCastingMOOSApp const string UPDATE_MAXDEP = "MAXDEEP_UPDATES"; const string UPDATE_OPREGION = "OPREGION_UPDATES"; const string UPDATE_RESETFAULT = "OPREGION_RESET"; - + //内部消息订阅 const string MSG_WPTFLAG = "CurrentPointComplete"; const string MSG_ENDFLAG = "END_WayPoint"; const string MSG_START = "START"; @@ -90,12 +90,13 @@ class TaskManger : public AppCastingMOOSApp const string MSG_FALUT = "TaskFault"; const string MSG_RUN = "RUN"; const string MSG_CLEARFAULT = "ClearFalut"; - + //交互消息 const string MSG_IN_SSM = "uMission_action_cmd"; const string MSG_TO_SSM = "uMission_task_fb"; const string MSG_TO_FH = "uMission_fault_fb"; - const string MSG_IN_FH = "uDrive_fault_fb"; const string MSG_IN_MAN = "uManual_enable_cmd"; //TODO: 增加手操状态 + const string MSG_FAULT_LEV2 = "uFH_errorMsg2_fb"; + const string MSG_FALUT_LEV3 = "uFH_errorMsg3_fb"; protected: // 任务参数