diff --git a/bin/placeholder b/bin/placeholder deleted file mode 100644 index e69de29..0000000 diff --git a/build/.cmake/api/v1/reply/index-2023-11-24T08-55-39-0072.json b/build/.cmake/api/v1/reply/index-2023-11-24T08-55-39-0072.json deleted file mode 100644 index 27a9d91..0000000 --- a/build/.cmake/api/v1/reply/index-2023-11-24T08-55-39-0072.json +++ /dev/null @@ -1,116 +0,0 @@ -{ - "cmake" : - { - "generator" : - { - "name" : "Unix Makefiles" - }, - "paths" : - { - "cmake" : "/usr/bin/cmake", - "cpack" : "/usr/bin/cpack", - "ctest" : "/usr/bin/ctest", - "root" : "/usr/share/cmake-3.16" - }, - "version" : - { - "isDirty" : false, - "major" : 3, - "minor" : 16, - "patch" : 3, - "string" : "3.16.3", - "suffix" : "" - } - }, - "objects" : - [ - { - "jsonFile" : "codemodel-v2-e5f65b12d81cb1175a33.json", - "kind" : "codemodel", - "version" : - { - "major" : 2, - "minor" : 0 - } - }, - { - "jsonFile" : "cache-v2-dd2c937f009d9a652f28.json", - "kind" : "cache", - "version" : - { - "major" : 2, - "minor" : 0 - } - }, - { - "jsonFile" : "cmakeFiles-v1-a2b0be97f994224a37d6.json", - "kind" : "cmakeFiles", - "version" : - { - "major" : 1, - "minor" : 0 - } - } - ], - "reply" : - { - "client-vscode" : - { - "query.json" : - { - "requests" : - [ - { - "kind" : "cache", - "version" : 2 - }, - { - "kind" : "codemodel", - "version" : 2 - }, - { - "kind" : "toolchains", - "version" : 1 - }, - { - "kind" : "cmakeFiles", - "version" : 1 - } - ], - "responses" : - [ - { - "jsonFile" : "cache-v2-dd2c937f009d9a652f28.json", - "kind" : "cache", - "version" : - { - "major" : 2, - "minor" : 0 - } - }, - { - "jsonFile" : "codemodel-v2-e5f65b12d81cb1175a33.json", - "kind" : "codemodel", - "version" : - { - "major" : 2, - "minor" : 0 - } - }, - { - "error" : "unknown request kind 'toolchains'" - }, - { - "jsonFile" : "cmakeFiles-v1-a2b0be97f994224a37d6.json", - "kind" : "cmakeFiles", - "version" : - { - "major" : 1, - "minor" : 0 - } - } - ] - } - } - } -} diff --git a/launch/launch.moos b/launch/launch.moos index 5d5f7e8..b66a8bd 100644 --- a/launch/launch.moos +++ b/launch/launch.moos @@ -25,7 +25,6 @@ ClientCommandLog = clientCommand.txt FaultLog = faultLog.txt MotionControlLog = motionControl.txt - llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json //------------------------------------------ diff --git a/launch/launch_local.moos b/launch/launch_local.moos index 2d914e1..ec833e9 100644 --- a/launch/launch_local.moos +++ b/launch/launch_local.moos @@ -17,7 +17,7 @@ AltOrigin = 0 VehicleName = lauv-150 -LogEnable = true +LogEnable = false //LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/ LogDir = /home/ben/project/moos-ivp-pi/log/ AuvDataLog = auvData.mdat diff --git a/setting/PlanConfigure.json b/setting/PlanConfigure.json index 8737b95..545935b 100644 --- a/setting/PlanConfigure.json +++ b/setting/PlanConfigure.json @@ -67,8 +67,8 @@ }, "plan1_toMoos" : { - "boardStamp" : 1699602762.2845099, - "clientStamp" : 1699602762.7520001, + "boardStamp" : 1700965801.2335429, + "clientStamp" : 1700965795.187, "closedLoop" : false, "duration" : -1.0, "maxDepth" : -1.0, @@ -76,49 +76,49 @@ "origin" : { "altitude" : 0.0, - "lat" : 50.825299999999999, - "lon" : -90.330399999999997 + "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" + }, + { + "depth" : 2.0, + "lat" : 43.82414, + "lon" : -70.328389999999999, + "name" : "Goto5", "speed" : 2.0, "type" : "point" } @@ -127,7 +127,7 @@ "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", + "taskId" : "-104,-48,78,56,26,-62,60,-67,-16,20,-117,108,100,79,-81,-97", "taskName" : "plan1_toMoos" }, "west_waypt_survey" : @@ -156,7 +156,7 @@ "name" : "station_1", "north" : -122.49101460421512, "speed" : 4.0, - "type" : "point" + "type" : "point" }, { "depth" : 7.0, @@ -166,7 +166,7 @@ "name" : "station_2", "north" : -111.04778559533926, "speed" : 6.0, - "type" : "point" + "type" : "point" }, { "depth" : 5.0, @@ -176,7 +176,7 @@ "name" : "station_3", "north" : -197.93630920628678, "speed" : 8.0, - "type" : "track" + "type" : "track" }, { "depth" : 3.0, @@ -186,7 +186,7 @@ "name" : "station_4", "north" : -232.26737690334403, "speed" : 10.0, - "type" : "track" + "type" : "track" } ], "priority" : 10, diff --git a/src/pBoardSupportComm/BoardSupportComm.cpp b/src/pBoardSupportComm/BoardSupportComm.cpp index 5e882ab..8dc768a 100644 --- a/src/pBoardSupportComm/BoardSupportComm.cpp +++ b/src/pBoardSupportComm/BoardSupportComm.cpp @@ -13,31 +13,14 @@ #define TCP_RECEIVE_PORT 8001 #define TCP_SERVER_ADDRESS "127.0.0.1" -#define MOOS_AUV_SIM -// #define MATLAB_AUV_SIM -// #ifdef TRUE_AUV +// #define MOOS_AUV_SIM +//#define MATLAB_AUV_SIM +#define TRUE_AUV using namespace std; BoardSupportComm::BoardSupportComm() { - estimatedState.currentLon = 0; - estimatedState.currentLat = 0; - estimatedState.currentAltitude = 0; - estimatedState.referenceLon = 0; - estimatedState.referenceLat = 0; - estimatedState.referenceAltitude = 0; - estimatedState.offsetNorth = 0; - estimatedState.offsetEast = 0; - estimatedState.offsetDown = 0; - estimatedState.roll = 0; - estimatedState.pitch = 0; - estimatedState.yaw = 0; - estimatedState.linearVelocityNorth = 0; - estimatedState.linearVelocityEast = 0; - estimatedState.linearVelocityDown = 0; - estimatedState.height = 0; - estimatedState.depth = 0; embeddedInfo.header = 0xEBA1; embeddedInfo.count = 0; @@ -89,26 +72,23 @@ BoardSupportComm::BoardSupportComm() executeCommand.footer = 0xEE2A; //16:[19,20] executeCommand.manual_mode = false; - tcpSockFD = -1; + embeddedInfo.header = 0xEBA1; + embeddedInfo.count = 0; tcpConnectRet = -1; + tcpSockFD = -1; + recvLen = -1; } //--------------------------------------------------------- // Destructor - BoardSupportComm::~BoardSupportComm() { - // delete tcpReceiveBuffer; close(tcpSockFD); - } -//--------------------------------------------------------- -// Procedure: OnNewMail - bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) { - // AppCastingMOOSApp::OnNewMail(NewMail); + AppCastingMOOSApp::OnNewMail(NewMail); MOOSMSG_LIST::iterator p; for(p=NewMail.begin(); p!=NewMail.end(); p++) @@ -127,77 +107,78 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) if(key == "NAV_X") { //E->N - estimatedState.offsetEast = msg.GetDouble(); + embeddedInfo.east = (int)dval; } if(key == "NAV_Y") { //N->E - estimatedState.offsetNorth = msg.GetDouble(); + embeddedInfo.north = (int)dval; } if(key == "NAV_Z") { //U->D - estimatedState.offsetDown = -msg.GetDouble(); + embeddedInfo.depth = -1 * dval * 100; } if(key == "NAV_YAW") { - double yawTemp = msg.GetDouble(); + double yawTemp = dval; //radian if (std::abs(yawTemp) <= M_PI) { if (yawTemp <= 0) { - estimatedState.yaw = -yawTemp * 180 / M_PI; + embeddedInfo.yaw = -yawTemp * 180 / M_PI * 100; } else { - estimatedState.yaw = (2 * M_PI - yawTemp) * 180 / M_PI; + embeddedInfo.yaw = (2 * M_PI - yawTemp) * 180 / M_PI * 100; } } } if(key == "NAV_PITCH") { - estimatedState.pitch = msg.GetDouble(); + embeddedInfo.pitch = dval * 100; } if(key == "NAV_LAT") { - estimatedState.currentLat = msg.GetDouble(); + embeddedInfo.lat = dval * 1000000; } if(key == "NAV_LONG") { - estimatedState.currentLon = msg.GetDouble(); + embeddedInfo.lon = dval * 1000000; } if(key == "NAV_SPEED") { - estimatedState.linearVelocityNorth = msg.GetDouble() * cos(estimatedState.yaw); - estimatedState.linearVelocityEast = -msg.GetDouble() * sin(estimatedState.yaw); - estimatedState.linearVelocityDown = 0; + float tempYaw = (float)(embeddedInfo.yaw) / 100.0; + embeddedInfo.ins_vx = dval * cos(tempYaw) * 100; + embeddedInfo.ins_vy = -dval * sin(tempYaw) * 100; + embeddedInfo.ins_vz = 0; + } if(key == "NAV_DEPTH") { - estimatedState.depth = msg.GetDouble(); + embeddedInfo.depth = dval * 100; } +#endif if(key == "Fault_LeakSensor") { - embeddedInfo.fault_leakSensor = (uint32_t)msg.GetDouble(); + embeddedInfo.fault_leakSensor = (uint32_t)dval; } if(key == "Fault_Battery") { - embeddedInfo.fault_battery = (uint8_t)msg.GetDouble(); + embeddedInfo.fault_battery = (uint8_t)dval; } if(key == "Fault_EmergencyBattery") { - embeddedInfo.fault_emergencyBattery = (uint8_t)(msg.GetDouble()); + embeddedInfo.fault_emergencyBattery = (uint8_t)dval; } if(key == "Fault_Thrust") { - embeddedInfo.fault_thrust = (uint8_t)(msg.GetDouble()); + embeddedInfo.fault_thrust = (uint8_t)dval; } -#endif - if(key == "uManual_enable_cmd") { - if (msg.GetDouble() == 1.0) + if (sval == "true") { executeCommand.drive_mode = 0x02; executeCommand.manual_mode = true; @@ -212,11 +193,15 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) if(key == "uManual_drive_cmd") { +#ifdef MOOS_AUV_SIM if (executeCommand.manual_mode) +#else + if (embeddedInfo.drive_mode == 0x02) +#endif { std::string err; Json::Value recvCommand; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); if (!parsingResult) @@ -271,7 +256,7 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) } std::string err; Json::Value recvCommand; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); if (!parsingResult) @@ -319,7 +304,7 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) { std::string err; Json::Value recvCommand; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); if (!parsingResult) @@ -327,9 +312,9 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) std::cerr << "Failed to parse JSON string." << std::endl; return false; } - estimatedState.referenceAltitude = recvCommand["alt"].asFloat(); - estimatedState.referenceLat = recvCommand["lat"].asFloat(); - estimatedState.referenceLon = recvCommand["lon"].asFloat(); + embeddedInfo.refLon = recvCommand["lon"].asFloat(); + embeddedInfo.refLat = recvCommand["lat"].asFloat(); + embeddedInfo.refAlt = recvCommand["alt"].asFloat(); struct stat info; if (stat(llaOriginPath.c_str(), &info) == 0) @@ -366,9 +351,6 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) ofs.close(); } } - - - } return(true); @@ -415,15 +397,15 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) embeddedState["driveMode"] = 0xFF; embeddedState["height"] = 0; embeddedState["depth"] = 0; - embeddedState["yaw"] = estimatedState.yaw; + embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; embeddedState["pitch"] = 0; embeddedState["roll"] = 0; - embeddedState["insVX"] = estimatedState.linearVelocityNorth * 0.01; - embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01; - embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01; - embeddedState["currentLon"] = estimatedState.currentLon; - embeddedState["currentLat"] = estimatedState.currentLat; - embeddedState["currentAltitude"] = 0; + embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01; + embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01; + embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01; + embeddedState["currentLon"] = embeddedInfo.lon * 0.000001; + embeddedState["currentLat"] = embeddedInfo.lat * 0.000001; + embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01; embeddedState["dvlVX"] = 0; embeddedState["dvlVY"] = 0; embeddedState["dvlVZ"] = 0; @@ -432,10 +414,6 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) embeddedState["batteryVoltage"] = 15; embeddedState["batteryLevel"] = 60; embeddedState["batteryTemp"] = 25; - // embeddedState["faultLeakSensor"] = 0; - // embeddedState["faultBattery"] = 0; - // embeddedState["faultEmergencyBattery"] = 0; - // embeddedState["faultThrust"] = 0; embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor; embeddedState["faultBattery"] = embeddedInfo.fault_battery; embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery; @@ -482,7 +460,7 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) embeddedState["depth"] = embeddedInfo.depth * 0.01; embeddedState["roll"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->N embeddedState["pitch"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->E - embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D + embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D radian<-degree embeddedState["insVX"] = embeddedInfo.ins_vy * 0.01; //E->N embeddedState["insVY"] = embeddedInfo.ins_vx * 0.01; //N->E embeddedState["insVZ"] = -embeddedInfo.ins_vz * 0.01; //U->D @@ -509,19 +487,18 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState) double currentLon = embeddedInfo.lon * 0.000001; double currentLat = embeddedInfo.lat * 0.000001; double currentAlt = embeddedInfo.alt * 0.01; - std::vector reference = {estimatedState.referenceLon, estimatedState.referenceLat, estimatedState.referenceAltitude}; + std::vector reference = {embeddedInfo.refLon, embeddedInfo.refLat, embeddedInfo.refAlt}; std::vector current = {currentLon, currentLat, currentAlt}; std::vector ned = {0, 0, 0}; 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; + embeddedState["referenceLon"]= embeddedInfo.refLon; + embeddedState["referenceLat"]= embeddedInfo.refLat; + embeddedState["referenceAltitude"]= embeddedInfo.refAlt; Json::StreamWriterBuilder builder; - std::string embeddedStateString = Json::writeString(builder, embeddedState); - return embeddedStateString; + return Json::writeString(builder, embeddedState); } @@ -530,15 +507,15 @@ void BoardSupportComm::tcpProcessThread() while(1) { bzero(tcpReceiveBuffer, 0); - int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer)); - if(lens>0) + recvLen = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer)); + if(recvLen > 0) { #ifndef MOOS_AUV_SIM - parseMessage((unsigned char* )tcpReceiveBuffer, lens); + parseMessage((unsigned char* )tcpReceiveBuffer, recvLen); #endif Json::Value embeddedState; - std::string embeddedStateString = convertEmbeddedFormat(embeddedState); - Notify("uDevice_monitor_fb", embeddedStateString); + recvContent = convertEmbeddedFormat(embeddedState); + Notify("uDevice_monitor_fb", recvContent); #ifndef MOOS_AUV_SIM Notify("NAV_X", embeddedState["north"].asDouble()); Notify("NAV_Y", embeddedState["east"].asDouble()); @@ -559,15 +536,16 @@ void BoardSupportComm::tcpProcessThread() } } -// bool BoardSupportComm::buildReport() -// { -// m_msgs << "buildReport:" << embeddedStateString << endl; -// return true; -// } +bool BoardSupportComm::buildReport() +{ + m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvLen:" << recvLen << endl; + m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvContent:" << recvContent << endl; + return true; +} bool BoardSupportComm::Iterate() { - // AppCastingMOOSApp::Iterate(); + AppCastingMOOSApp::Iterate(); if(tcpSockFD == -1) { @@ -591,8 +569,10 @@ bool BoardSupportComm::Iterate() return false; } } + if ((tcpSockFD != -1) && (tcpConnectRet != -1)) { + std::thread t1(&BoardSupportComm::tcpProcessThread, this); t1.detach(); } @@ -665,7 +645,7 @@ bool BoardSupportComm::Iterate() } #endif - //AppCastingMOOSApp::PostReport(); + AppCastingMOOSApp::PostReport(); return true; } @@ -763,7 +743,7 @@ int BoardSupportComm::parseMessage(unsigned char* buffer, int size) bool BoardSupportComm::OnStartUp() { - // AppCastingMOOSApp::OnStartUp(); + AppCastingMOOSApp::OnStartUp(); m_MissionReader.GetValue("llaOriginPath", llaOriginPath); try { @@ -782,9 +762,9 @@ bool BoardSupportComm::OnStartUp() originJsonValue.isMember("LatOrigin") && originJsonValue.isMember("AltOrigin")) { - estimatedState.referenceLon = originJsonValue["LongOrigin"].asFloat(); - estimatedState.referenceLat = originJsonValue["LatOrigin"].asFloat(); - estimatedState.referenceAltitude = originJsonValue["AltOrigin"].asFloat(); + embeddedInfo.refLon = originJsonValue["LongOrigin"].asFloat(); + embeddedInfo.refLat = originJsonValue["LatOrigin"].asFloat(); + embeddedInfo.refAlt = originJsonValue["AltOrigin"].asFloat(); } else { @@ -798,22 +778,18 @@ bool BoardSupportComm::OnStartUp() } catch(...) { - m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon); - m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat); - m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude); + m_MissionReader.GetValue("LongOrigin", embeddedInfo.refLon); + m_MissionReader.GetValue("LatOrigin", embeddedInfo.refLat); + m_MissionReader.GetValue("AltOrigin", embeddedInfo.refAlt); } - // std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", " - // << estimatedState.referenceLat << ", " - // << estimatedState.referenceAltitude << std::endl; - RegisterVariables(); return(true); } void BoardSupportComm::RegisterVariables() { - // AppCastingMOOSApp::RegisterVariables(); + AppCastingMOOSApp::RegisterVariables(); #ifdef MOOS_AUV_SIM Register("NAV_X", 0); diff --git a/src/pBoardSupportComm/BoardSupportComm.h b/src/pBoardSupportComm/BoardSupportComm.h index 974524b..255fab1 100644 --- a/src/pBoardSupportComm/BoardSupportComm.h +++ b/src/pBoardSupportComm/BoardSupportComm.h @@ -53,6 +53,12 @@ struct AUVEmbedded uint8_t dvl_status; //30:[55] uint8_t crc; //31:[56] uint16_t footer; //32:[57,58] + + float refLon; + float refLat; + float refAlt; + float north; + float east; }; struct AUVExecuteCommand @@ -76,45 +82,22 @@ struct AUVExecuteCommand bool manual_mode; }; -struct EstimatedState { - float referenceLon; - float referenceLat; - float referenceAltitude; - float currentLon; - float currentLat; - float currentAltitude; - float offsetNorth; - float offsetEast; - float offsetDown; - float roll; - float pitch; - float yaw; - float linearVelocityNorth; - float linearVelocityEast; - float linearVelocityDown; - float height; - float depth; -}; - -class BoardSupportComm : public CMOOSApp -// class BoardSupportComm : public AppCastingMOOSApp +class BoardSupportComm : public AppCastingMOOSApp { public: BoardSupportComm(); ~BoardSupportComm(); -protected: // Standard MOOSApp functions to overload +protected: bool OnNewMail(MOOSMSG_LIST &NewMail); bool Iterate(); bool OnConnectToServer(); bool OnStartUp(); void RegisterVariables(); - // bool buildReport(); + bool buildReport(); private: - struct EstimatedState estimatedState; - void ConvertLLAToENU(std::vector init_lla, std::vector point_lla, std::vector& point_enu); @@ -129,7 +112,6 @@ private: std::vector &point_lla); void tcpProcessThread(); - // std::string convertEmbeddedFormat(); std::string convertEmbeddedFormat(Json::Value &embeddedState); int tcpSockFD; int tcpConnectRet; @@ -144,12 +126,11 @@ private: struct AUVEmbedded embeddedInfo; struct AUVExecuteCommand executeCommand; - int testFlag = 0; - bool testCount = false; - int sockfd = -1; - int connectFlg = -1; - std::string llaOriginPath; + std::string llaOriginPath; + int recvLen; + std::string recvContent; + std::string castLogStream; }; diff --git a/src/pClientViewer/ClientViewer.cpp b/src/pClientViewer/ClientViewer.cpp index 8d5954b..d2d69e9 100644 --- a/src/pClientViewer/ClientViewer.cpp +++ b/src/pClientViewer/ClientViewer.cpp @@ -765,7 +765,7 @@ void ClientViewer::processMessage(DUNE::IMC::Message * message) { for (; iter2 != entityParameter->params.end(); ++iter2) { DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); - std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; + //std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; } } } diff --git a/src/pDataManagement/DataManagement.cpp b/src/pDataManagement/DataManagement.cpp index c0a6ccb..94fed60 100644 --- a/src/pDataManagement/DataManagement.cpp +++ b/src/pDataManagement/DataManagement.cpp @@ -17,6 +17,7 @@ using namespace std; // Constructor DataManagement::DataManagement() { + logEnable = false; motionControlInfo.desiredHeading = 0; motionControlInfo.desiredSpeed = 0; @@ -44,26 +45,7 @@ DataManagement::DataManagement() // 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(); - } - if(motionControlStream.is_open()) - { - motionControlStream.close(); - } + CloseOutputStream(); } //--------------------------------------------------------- @@ -89,11 +71,25 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) bool mdbl = msg.IsDouble(); bool mstr = msg.IsString(); + if (key == "uClient_logEnable_cmd") + { + if (sval == "true") + { + logEnable = true; + OpenOutputStream(); + } + else + { + logEnable = false; + CloseOutputStream(); + } + } + if(key == "uDevice_monitor_fb") { std::string err; Json::Value estimatedStateData; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err); if (!parsingResult) @@ -138,7 +134,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) { std::string err; Json::Value missionStatusObject; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err); try @@ -168,10 +164,9 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) { std::string err; Json::Value errorStatus; - std::istringstream iss(msg.GetString()); + std::istringstream iss(sval); Json::CharReaderBuilder builder; bool parsingResult = Json::parseFromStream(builder, iss, &errorStatus, &err); - // std::cout << "uFH_errorMsg: " << msg.GetString() << std::endl; try { if (!parsingResult) @@ -242,7 +237,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "DESIRED_HEADING") { - motionControlInfo.desiredHeading = msg.GetDouble(); + motionControlInfo.desiredHeading = dval; std::stringstream ss; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << motionControlInfo.desiredHeading << "," @@ -252,7 +247,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "DESIRED_SPEED") { - motionControlInfo.desiredSpeed = msg.GetDouble(); + motionControlInfo.desiredSpeed = dval; std::stringstream ss; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << motionControlInfo.desiredHeading << "," @@ -262,7 +257,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "DESIRED_DEPTH") { - motionControlInfo.desiredDepth = msg.GetDouble(); + motionControlInfo.desiredDepth = dval; std::stringstream ss; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << motionControlInfo.desiredHeading << "," @@ -372,92 +367,92 @@ bool DataManagement::OnStartUp() { AppCastingMOOSApp::OnStartUp(); - list sParams; - m_MissionReader.EnableVerbatimQuoting(false); - if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { - list::iterator p; - for(p=sParams.begin(); p!=sParams.end(); p++) { - string line = *p; - string param = tolower(biteStringX(line, '=')); - string value = line; + // list sParams; + // m_MissionReader.EnableVerbatimQuoting(false); + // if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { + // list::iterator p; + // for(p=sParams.begin(); p!=sParams.end(); p++) { + // string line = *p; + // string param = tolower(biteStringX(line, '=')); + // string value = line; - if(param == "foo") { - //handled - } - else if(param == "bar") { - //handled - } - } - } - std::string saveLogDir; + // if(param == "foo") { + // //handled + // } + // else if(param == "bar") { + // //handled + // } + // } + // } + 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); - std::string motionControlFile; - m_MissionReader.GetValue("MotionControlLog", motionControlFile); + // 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); + // std::string motionControlFile; + // m_MissionReader.GetValue("MotionControlLog", motionControlFile); + 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); + } 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; + // std::string motionControlSavePath = saveLogDir + "/" + motionControlFile; - 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; - std::string motionControlSavePath = saveLogDir + "/" + motionControlFile; + // 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"); + // if(!OpenFile(motionControlStream, motionControlSavePath)) + // return MOOSFail("Failed to Open faultLog file"); - 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"); - if(!OpenFile(motionControlStream, motionControlSavePath)) - return MOOSFail("Failed to Open faultLog file"); + // DoAuvDataLogBanner(auvDataStream); + // DoMissionHistoryBanner(missionHistoryStream); + // DoFaultHandleBanner(faultLogStream); + // DoMotionControlBanner(motionControlStream); - DoAuvDataLogBanner(auvDataStream); - DoMissionHistoryBanner(missionHistoryStream); - DoFaultHandleBanner(faultLogStream); - DoMotionControlBanner(motionControlStream); + OpenOutputStream(); } RegisterVariables(); @@ -488,7 +483,8 @@ void DataManagement::RegisterVariables() Register("DESIRED_HEADING", 0); Register("DESIRED_SPEED", 0); Register("DESIRED_DEPTH", 0); - Register("uMotion_desired_log", 0); + Register("uMotion_desired_log", 0); + Register("uClient_logEnable_cmd", 0); } bool DataManagement::buildReport() @@ -638,3 +634,82 @@ void DataManagement::GenerateFileName(std::string &fileDir, std::string &fileNam ss << hour << minute << second; fileName = ss.str(); } + +bool DataManagement::OpenOutputStream() +{ + std::string subDir; + std::string localTime; + GenerateFileName(subDir, localTime); + std::string saveSubLogDir = saveLogDir + "/" + subDir; + if (access(saveSubLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveSubLogDir.c_str(), mode); + } + saveSubLogDir += "/" + localTime; + if (access(saveSubLogDir.c_str(), F_OK) == -1 ) + { + mode_t mode = 0775; + mkdir(saveSubLogDir.c_str(), mode); + } + + 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); + std::string motionControlFile; + m_MissionReader.GetValue("MotionControlLog", motionControlFile); + + std::string auvDataSavePath = saveSubLogDir + "/" + auvDataFile; + std::string missionHistorySavePath = saveSubLogDir + "/" + missionHistoryFile; + std::string clientCommandSavePath = saveSubLogDir + "/" + clientCommandFile; + std::string faultLogSavePath = saveSubLogDir + "/" + faultLogFile; + std::string motionControlSavePath = saveSubLogDir + "/" + motionControlFile; + + 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"); + if(!OpenFile(motionControlStream, motionControlSavePath)) + return MOOSFail("Failed to Open faultLog file"); + + DoAuvDataLogBanner(auvDataStream); + DoMissionHistoryBanner(missionHistoryStream); + DoFaultHandleBanner(faultLogStream); + DoMotionControlBanner(motionControlStream); + + return true; +} + + +void DataManagement::CloseOutputStream() +{ + if(auvDataStream.is_open()) + { + auvDataStream.close(); + } + if(missionHistoryStream.is_open()) + { + missionHistoryStream.close(); + } + if(clientCommandStream.is_open()) + { + clientCommandStream.close(); + } + if(faultLogStream.is_open()) + { + faultLogStream.close(); + } + if(motionControlStream.is_open()) + { + motionControlStream.close(); + } +} \ No newline at end of file diff --git a/src/pDataManagement/DataManagement.h b/src/pDataManagement/DataManagement.h index f2b57f3..0f45827 100644 --- a/src/pDataManagement/DataManagement.h +++ b/src/pDataManagement/DataManagement.h @@ -44,6 +44,8 @@ protected: void DoMotionControlBanner(std::ofstream &os); void GenerateFileName(std::string &fileDir, std::string &fileName); + bool OpenOutputStream(); + void CloseOutputStream(); double getTimeStamp(); int nDoublePrecision; std::ofstream auvDataStream; @@ -54,6 +56,8 @@ protected: std::map logVarList; std::string contentFromStream; struct MotionControlInfo motionControlInfo; + bool logEnable; + std::string saveLogDir; }; #endif diff --git a/src/pDataManagement/NavigationInfo.pb.h b/src/pDataManagement/NavigationInfo.pb.h deleted file mode 100644 index b7e4a5d..0000000 --- a/src/pDataManagement/NavigationInfo.pb.h +++ /dev/null @@ -1,809 +0,0 @@ -// Generated by the protocol buffer compiler. DO NOT EDIT! -// source: NavigationInfo.proto - -#ifndef GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto -#define GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto - -#include -#include - -#include -#if PROTOBUF_VERSION < 3021000 -#error This file was generated by a newer version of protoc which is -#error incompatible with your Protocol Buffer headers. Please update -#error your headers. -#endif -#if 3021012 < PROTOBUF_MIN_PROTOC_VERSION -#error This file was generated by an older version of protoc which is -#error incompatible with your Protocol Buffer headers. Please -#error regenerate this file with a newer version of protoc. -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include // IWYU pragma: export -#include // IWYU pragma: export -#include -// @@protoc_insertion_point(includes) -#include -#define PROTOBUF_INTERNAL_EXPORT_NavigationInfo_2eproto -PROTOBUF_NAMESPACE_OPEN -namespace internal { -class AnyMetadata; -} // namespace internal -PROTOBUF_NAMESPACE_CLOSE - -// Internal implementation detail -- do not use these members. -struct TableStruct_NavigationInfo_2eproto { - static const uint32_t offsets[]; -}; -extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_NavigationInfo_2eproto; -namespace NavigationInfo { -class EstimatedState; -struct EstimatedStateDefaultTypeInternal; -extern EstimatedStateDefaultTypeInternal _EstimatedState_default_instance_; -} // namespace NavigationInfo -PROTOBUF_NAMESPACE_OPEN -template<> ::NavigationInfo::EstimatedState* Arena::CreateMaybeMessage<::NavigationInfo::EstimatedState>(Arena*); -PROTOBUF_NAMESPACE_CLOSE -namespace NavigationInfo { - -// =================================================================== - -class EstimatedState final : - public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavigationInfo.EstimatedState) */ { - public: - inline EstimatedState() : EstimatedState(nullptr) {} - ~EstimatedState() override; - explicit PROTOBUF_CONSTEXPR EstimatedState(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized); - - EstimatedState(const EstimatedState& from); - EstimatedState(EstimatedState&& from) noexcept - : EstimatedState() { - *this = ::std::move(from); - } - - inline EstimatedState& operator=(const EstimatedState& from) { - CopyFrom(from); - return *this; - } - inline EstimatedState& operator=(EstimatedState&& from) noexcept { - if (this == &from) return *this; - if (GetOwningArena() == from.GetOwningArena() - #ifdef PROTOBUF_FORCE_COPY_IN_MOVE - && GetOwningArena() != nullptr - #endif // !PROTOBUF_FORCE_COPY_IN_MOVE - ) { - InternalSwap(&from); - } else { - CopyFrom(from); - } - return *this; - } - - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() { - return GetDescriptor(); - } - static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() { - return default_instance().GetMetadata().descriptor; - } - static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() { - return default_instance().GetMetadata().reflection; - } - static const EstimatedState& default_instance() { - return *internal_default_instance(); - } - static inline const EstimatedState* internal_default_instance() { - return reinterpret_cast( - &_EstimatedState_default_instance_); - } - static constexpr int kIndexInFileMessages = - 0; - - friend void swap(EstimatedState& a, EstimatedState& b) { - a.Swap(&b); - } - inline void Swap(EstimatedState* other) { - if (other == this) return; - #ifdef PROTOBUF_FORCE_COPY_IN_SWAP - if (GetOwningArena() != nullptr && - GetOwningArena() == other->GetOwningArena()) { - #else // PROTOBUF_FORCE_COPY_IN_SWAP - if (GetOwningArena() == other->GetOwningArena()) { - #endif // !PROTOBUF_FORCE_COPY_IN_SWAP - InternalSwap(other); - } else { - ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other); - } - } - void UnsafeArenaSwap(EstimatedState* other) { - if (other == this) return; - GOOGLE_DCHECK(GetOwningArena() == other->GetOwningArena()); - InternalSwap(other); - } - - // implements Message ---------------------------------------------- - - EstimatedState* New(::PROTOBUF_NAMESPACE_ID::Arena* arena = nullptr) const final { - return CreateMaybeMessage(arena); - } - using ::PROTOBUF_NAMESPACE_ID::Message::CopyFrom; - void CopyFrom(const EstimatedState& from); - using ::PROTOBUF_NAMESPACE_ID::Message::MergeFrom; - void MergeFrom( const EstimatedState& from) { - EstimatedState::MergeImpl(*this, from); - } - private: - static void MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTOBUF_NAMESPACE_ID::Message& from_msg); - public: - PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final; - bool IsInitialized() const final; - - size_t ByteSizeLong() const final; - const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final; - uint8_t* _InternalSerialize( - uint8_t* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final; - int GetCachedSize() const final { return _impl_._cached_size_.Get(); } - - private: - void SharedCtor(::PROTOBUF_NAMESPACE_ID::Arena* arena, bool is_message_owned); - void SharedDtor(); - void SetCachedSize(int size) const final; - void InternalSwap(EstimatedState* other); - - private: - friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata; - static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() { - return "NavigationInfo.EstimatedState"; - } - protected: - explicit EstimatedState(::PROTOBUF_NAMESPACE_ID::Arena* arena, - bool is_message_owned = false); - public: - - static const ClassData _class_data_; - const ::PROTOBUF_NAMESPACE_ID::Message::ClassData*GetClassData() const final; - - ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final; - - // nested types ---------------------------------------------------- - - // accessors ------------------------------------------------------- - - enum : int { - kInfoFieldNumber = 1, - kLonFieldNumber = 2, - kLatFieldNumber = 3, - kHeightFieldNumber = 4, - kOffsetNorthFieldNumber = 5, - kOffsetEastFieldNumber = 6, - kOffsetDownFieldNumber = 7, - kRollFieldNumber = 8, - kPitchFieldNumber = 9, - kYawFieldNumber = 10, - kLinearVelocityNorthFieldNumber = 11, - kLinearVelocityEastFieldNumber = 12, - kLinearVelocityDownFieldNumber = 13, - kAngularVelocityNorthFieldNumber = 14, - kAngularVelocityEastFieldNumber = 15, - kAngularVelocityDownFieldNumber = 16, - kDepthFieldNumber = 17, - kAltitudeFieldNumber = 18, - }; - // string info = 1; - void clear_info(); - const std::string& info() const; - template - void set_info(ArgT0&& arg0, ArgT... args); - std::string* mutable_info(); - PROTOBUF_NODISCARD std::string* release_info(); - void set_allocated_info(std::string* info); - private: - const std::string& _internal_info() const; - inline PROTOBUF_ALWAYS_INLINE void _internal_set_info(const std::string& value); - std::string* _internal_mutable_info(); - public: - - // float lon = 2; - void clear_lon(); - float lon() const; - void set_lon(float value); - private: - float _internal_lon() const; - void _internal_set_lon(float value); - public: - - // float lat = 3; - void clear_lat(); - float lat() const; - void set_lat(float value); - private: - float _internal_lat() const; - void _internal_set_lat(float value); - public: - - // float height = 4; - void clear_height(); - float height() const; - void set_height(float value); - private: - float _internal_height() const; - void _internal_set_height(float value); - public: - - // float offsetNorth = 5; - void clear_offsetnorth(); - float offsetnorth() const; - void set_offsetnorth(float value); - private: - float _internal_offsetnorth() const; - void _internal_set_offsetnorth(float value); - public: - - // float offsetEast = 6; - void clear_offseteast(); - float offseteast() const; - void set_offseteast(float value); - private: - float _internal_offseteast() const; - void _internal_set_offseteast(float value); - public: - - // float offsetDown = 7; - void clear_offsetdown(); - float offsetdown() const; - void set_offsetdown(float value); - private: - float _internal_offsetdown() const; - void _internal_set_offsetdown(float value); - public: - - // float roll = 8; - void clear_roll(); - float roll() const; - void set_roll(float value); - private: - float _internal_roll() const; - void _internal_set_roll(float value); - public: - - // float pitch = 9; - void clear_pitch(); - float pitch() const; - void set_pitch(float value); - private: - float _internal_pitch() const; - void _internal_set_pitch(float value); - public: - - // float yaw = 10; - void clear_yaw(); - float yaw() const; - void set_yaw(float value); - private: - float _internal_yaw() const; - void _internal_set_yaw(float value); - public: - - // float linearVelocityNorth = 11; - void clear_linearvelocitynorth(); - float linearvelocitynorth() const; - void set_linearvelocitynorth(float value); - private: - float _internal_linearvelocitynorth() const; - void _internal_set_linearvelocitynorth(float value); - public: - - // float linearVelocityEast = 12; - void clear_linearvelocityeast(); - float linearvelocityeast() const; - void set_linearvelocityeast(float value); - private: - float _internal_linearvelocityeast() const; - void _internal_set_linearvelocityeast(float value); - public: - - // float linearVelocityDown = 13; - void clear_linearvelocitydown(); - float linearvelocitydown() const; - void set_linearvelocitydown(float value); - private: - float _internal_linearvelocitydown() const; - void _internal_set_linearvelocitydown(float value); - public: - - // float angularVelocityNorth = 14; - void clear_angularvelocitynorth(); - float angularvelocitynorth() const; - void set_angularvelocitynorth(float value); - private: - float _internal_angularvelocitynorth() const; - void _internal_set_angularvelocitynorth(float value); - public: - - // float angularVelocityEast = 15; - void clear_angularvelocityeast(); - float angularvelocityeast() const; - void set_angularvelocityeast(float value); - private: - float _internal_angularvelocityeast() const; - void _internal_set_angularvelocityeast(float value); - public: - - // float angularVelocityDown = 16; - void clear_angularvelocitydown(); - float angularvelocitydown() const; - void set_angularvelocitydown(float value); - private: - float _internal_angularvelocitydown() const; - void _internal_set_angularvelocitydown(float value); - public: - - // float depth = 17; - void clear_depth(); - float depth() const; - void set_depth(float value); - private: - float _internal_depth() const; - void _internal_set_depth(float value); - public: - - // float altitude = 18; - void clear_altitude(); - float altitude() const; - void set_altitude(float value); - private: - float _internal_altitude() const; - void _internal_set_altitude(float value); - public: - - // @@protoc_insertion_point(class_scope:NavigationInfo.EstimatedState) - private: - class _Internal; - - template friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper; - typedef void InternalArenaConstructable_; - typedef void DestructorSkippable_; - struct Impl_ { - ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr info_; - float lon_; - float lat_; - float height_; - float offsetnorth_; - float offseteast_; - float offsetdown_; - float roll_; - float pitch_; - float yaw_; - float linearvelocitynorth_; - float linearvelocityeast_; - float linearvelocitydown_; - float angularvelocitynorth_; - float angularvelocityeast_; - float angularvelocitydown_; - float depth_; - float altitude_; - mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_; - }; - union { Impl_ _impl_; }; - friend struct ::TableStruct_NavigationInfo_2eproto; -}; -// =================================================================== - - -// =================================================================== - -#ifdef __GNUC__ - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wstrict-aliasing" -#endif // __GNUC__ -// EstimatedState - -// string info = 1; -inline void EstimatedState::clear_info() { - _impl_.info_.ClearToEmpty(); -} -inline const std::string& EstimatedState::info() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.info) - return _internal_info(); -} -template -inline PROTOBUF_ALWAYS_INLINE -void EstimatedState::set_info(ArgT0&& arg0, ArgT... args) { - - _impl_.info_.Set(static_cast(arg0), args..., GetArenaForAllocation()); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.info) -} -inline std::string* EstimatedState::mutable_info() { - std::string* _s = _internal_mutable_info(); - // @@protoc_insertion_point(field_mutable:NavigationInfo.EstimatedState.info) - return _s; -} -inline const std::string& EstimatedState::_internal_info() const { - return _impl_.info_.Get(); -} -inline void EstimatedState::_internal_set_info(const std::string& value) { - - _impl_.info_.Set(value, GetArenaForAllocation()); -} -inline std::string* EstimatedState::_internal_mutable_info() { - - return _impl_.info_.Mutable(GetArenaForAllocation()); -} -inline std::string* EstimatedState::release_info() { - // @@protoc_insertion_point(field_release:NavigationInfo.EstimatedState.info) - return _impl_.info_.Release(); -} -inline void EstimatedState::set_allocated_info(std::string* info) { - if (info != nullptr) { - - } else { - - } - _impl_.info_.SetAllocated(info, GetArenaForAllocation()); -#ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING - if (_impl_.info_.IsDefault()) { - _impl_.info_.Set("", GetArenaForAllocation()); - } -#endif // PROTOBUF_FORCE_COPY_DEFAULT_STRING - // @@protoc_insertion_point(field_set_allocated:NavigationInfo.EstimatedState.info) -} - -// float lon = 2; -inline void EstimatedState::clear_lon() { - _impl_.lon_ = 0; -} -inline float EstimatedState::_internal_lon() const { - return _impl_.lon_; -} -inline float EstimatedState::lon() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.lon) - return _internal_lon(); -} -inline void EstimatedState::_internal_set_lon(float value) { - - _impl_.lon_ = value; -} -inline void EstimatedState::set_lon(float value) { - _internal_set_lon(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.lon) -} - -// float lat = 3; -inline void EstimatedState::clear_lat() { - _impl_.lat_ = 0; -} -inline float EstimatedState::_internal_lat() const { - return _impl_.lat_; -} -inline float EstimatedState::lat() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.lat) - return _internal_lat(); -} -inline void EstimatedState::_internal_set_lat(float value) { - - _impl_.lat_ = value; -} -inline void EstimatedState::set_lat(float value) { - _internal_set_lat(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.lat) -} - -// float height = 4; -inline void EstimatedState::clear_height() { - _impl_.height_ = 0; -} -inline float EstimatedState::_internal_height() const { - return _impl_.height_; -} -inline float EstimatedState::height() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.height) - return _internal_height(); -} -inline void EstimatedState::_internal_set_height(float value) { - - _impl_.height_ = value; -} -inline void EstimatedState::set_height(float value) { - _internal_set_height(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.height) -} - -// float offsetNorth = 5; -inline void EstimatedState::clear_offsetnorth() { - _impl_.offsetnorth_ = 0; -} -inline float EstimatedState::_internal_offsetnorth() const { - return _impl_.offsetnorth_; -} -inline float EstimatedState::offsetnorth() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetNorth) - return _internal_offsetnorth(); -} -inline void EstimatedState::_internal_set_offsetnorth(float value) { - - _impl_.offsetnorth_ = value; -} -inline void EstimatedState::set_offsetnorth(float value) { - _internal_set_offsetnorth(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetNorth) -} - -// float offsetEast = 6; -inline void EstimatedState::clear_offseteast() { - _impl_.offseteast_ = 0; -} -inline float EstimatedState::_internal_offseteast() const { - return _impl_.offseteast_; -} -inline float EstimatedState::offseteast() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetEast) - return _internal_offseteast(); -} -inline void EstimatedState::_internal_set_offseteast(float value) { - - _impl_.offseteast_ = value; -} -inline void EstimatedState::set_offseteast(float value) { - _internal_set_offseteast(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetEast) -} - -// float offsetDown = 7; -inline void EstimatedState::clear_offsetdown() { - _impl_.offsetdown_ = 0; -} -inline float EstimatedState::_internal_offsetdown() const { - return _impl_.offsetdown_; -} -inline float EstimatedState::offsetdown() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetDown) - return _internal_offsetdown(); -} -inline void EstimatedState::_internal_set_offsetdown(float value) { - - _impl_.offsetdown_ = value; -} -inline void EstimatedState::set_offsetdown(float value) { - _internal_set_offsetdown(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetDown) -} - -// float roll = 8; -inline void EstimatedState::clear_roll() { - _impl_.roll_ = 0; -} -inline float EstimatedState::_internal_roll() const { - return _impl_.roll_; -} -inline float EstimatedState::roll() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.roll) - return _internal_roll(); -} -inline void EstimatedState::_internal_set_roll(float value) { - - _impl_.roll_ = value; -} -inline void EstimatedState::set_roll(float value) { - _internal_set_roll(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.roll) -} - -// float pitch = 9; -inline void EstimatedState::clear_pitch() { - _impl_.pitch_ = 0; -} -inline float EstimatedState::_internal_pitch() const { - return _impl_.pitch_; -} -inline float EstimatedState::pitch() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.pitch) - return _internal_pitch(); -} -inline void EstimatedState::_internal_set_pitch(float value) { - - _impl_.pitch_ = value; -} -inline void EstimatedState::set_pitch(float value) { - _internal_set_pitch(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.pitch) -} - -// float yaw = 10; -inline void EstimatedState::clear_yaw() { - _impl_.yaw_ = 0; -} -inline float EstimatedState::_internal_yaw() const { - return _impl_.yaw_; -} -inline float EstimatedState::yaw() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.yaw) - return _internal_yaw(); -} -inline void EstimatedState::_internal_set_yaw(float value) { - - _impl_.yaw_ = value; -} -inline void EstimatedState::set_yaw(float value) { - _internal_set_yaw(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.yaw) -} - -// float linearVelocityNorth = 11; -inline void EstimatedState::clear_linearvelocitynorth() { - _impl_.linearvelocitynorth_ = 0; -} -inline float EstimatedState::_internal_linearvelocitynorth() const { - return _impl_.linearvelocitynorth_; -} -inline float EstimatedState::linearvelocitynorth() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityNorth) - return _internal_linearvelocitynorth(); -} -inline void EstimatedState::_internal_set_linearvelocitynorth(float value) { - - _impl_.linearvelocitynorth_ = value; -} -inline void EstimatedState::set_linearvelocitynorth(float value) { - _internal_set_linearvelocitynorth(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityNorth) -} - -// float linearVelocityEast = 12; -inline void EstimatedState::clear_linearvelocityeast() { - _impl_.linearvelocityeast_ = 0; -} -inline float EstimatedState::_internal_linearvelocityeast() const { - return _impl_.linearvelocityeast_; -} -inline float EstimatedState::linearvelocityeast() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityEast) - return _internal_linearvelocityeast(); -} -inline void EstimatedState::_internal_set_linearvelocityeast(float value) { - - _impl_.linearvelocityeast_ = value; -} -inline void EstimatedState::set_linearvelocityeast(float value) { - _internal_set_linearvelocityeast(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityEast) -} - -// float linearVelocityDown = 13; -inline void EstimatedState::clear_linearvelocitydown() { - _impl_.linearvelocitydown_ = 0; -} -inline float EstimatedState::_internal_linearvelocitydown() const { - return _impl_.linearvelocitydown_; -} -inline float EstimatedState::linearvelocitydown() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityDown) - return _internal_linearvelocitydown(); -} -inline void EstimatedState::_internal_set_linearvelocitydown(float value) { - - _impl_.linearvelocitydown_ = value; -} -inline void EstimatedState::set_linearvelocitydown(float value) { - _internal_set_linearvelocitydown(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityDown) -} - -// float angularVelocityNorth = 14; -inline void EstimatedState::clear_angularvelocitynorth() { - _impl_.angularvelocitynorth_ = 0; -} -inline float EstimatedState::_internal_angularvelocitynorth() const { - return _impl_.angularvelocitynorth_; -} -inline float EstimatedState::angularvelocitynorth() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityNorth) - return _internal_angularvelocitynorth(); -} -inline void EstimatedState::_internal_set_angularvelocitynorth(float value) { - - _impl_.angularvelocitynorth_ = value; -} -inline void EstimatedState::set_angularvelocitynorth(float value) { - _internal_set_angularvelocitynorth(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityNorth) -} - -// float angularVelocityEast = 15; -inline void EstimatedState::clear_angularvelocityeast() { - _impl_.angularvelocityeast_ = 0; -} -inline float EstimatedState::_internal_angularvelocityeast() const { - return _impl_.angularvelocityeast_; -} -inline float EstimatedState::angularvelocityeast() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityEast) - return _internal_angularvelocityeast(); -} -inline void EstimatedState::_internal_set_angularvelocityeast(float value) { - - _impl_.angularvelocityeast_ = value; -} -inline void EstimatedState::set_angularvelocityeast(float value) { - _internal_set_angularvelocityeast(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityEast) -} - -// float angularVelocityDown = 16; -inline void EstimatedState::clear_angularvelocitydown() { - _impl_.angularvelocitydown_ = 0; -} -inline float EstimatedState::_internal_angularvelocitydown() const { - return _impl_.angularvelocitydown_; -} -inline float EstimatedState::angularvelocitydown() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityDown) - return _internal_angularvelocitydown(); -} -inline void EstimatedState::_internal_set_angularvelocitydown(float value) { - - _impl_.angularvelocitydown_ = value; -} -inline void EstimatedState::set_angularvelocitydown(float value) { - _internal_set_angularvelocitydown(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityDown) -} - -// float depth = 17; -inline void EstimatedState::clear_depth() { - _impl_.depth_ = 0; -} -inline float EstimatedState::_internal_depth() const { - return _impl_.depth_; -} -inline float EstimatedState::depth() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.depth) - return _internal_depth(); -} -inline void EstimatedState::_internal_set_depth(float value) { - - _impl_.depth_ = value; -} -inline void EstimatedState::set_depth(float value) { - _internal_set_depth(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.depth) -} - -// float altitude = 18; -inline void EstimatedState::clear_altitude() { - _impl_.altitude_ = 0; -} -inline float EstimatedState::_internal_altitude() const { - return _impl_.altitude_; -} -inline float EstimatedState::altitude() const { - // @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.altitude) - return _internal_altitude(); -} -inline void EstimatedState::_internal_set_altitude(float value) { - - _impl_.altitude_ = value; -} -inline void EstimatedState::set_altitude(float value) { - _internal_set_altitude(value); - // @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.altitude) -} - -#ifdef __GNUC__ - #pragma GCC diagnostic pop -#endif // __GNUC__ - -// @@protoc_insertion_point(namespace_scope) - -} // namespace NavigationInfo - -// @@protoc_insertion_point(global_scope) - -#include -#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto diff --git a/src/pEmulator/Emulator.cpp b/src/pEmulator/Emulator.cpp index d1edd47..7ae7d78 100644 --- a/src/pEmulator/Emulator.cpp +++ b/src/pEmulator/Emulator.cpp @@ -401,13 +401,13 @@ void Emulator::set150Info() p_150server_1.embeddedInfoSrc.dvl_vz = (int16_t)remus100.w; //18:[36,37] p_150server_1.embeddedInfoSrc.rpm = (int16_t)remus100.thrust; //19:[38,39] p_150server_1.embeddedInfoSrc.lightEnable = (uint8_t)0x01; //20:[40] - p_150server_1.embeddedInfoSrc.battery_voltage = (uint16_t)1024; //21:[41,42] + p_150server_1.embeddedInfoSrc.battery_voltage = (uint16_t)30; //21:[41,42] p_150server_1.embeddedInfoSrc.battery_level = 60; //22:[43] p_150server_1.embeddedInfoSrc.battery_temp = 21.3 / 0.1; //23:[44,45] - p_150server_1.embeddedInfoSrc.fault_leakSensor = 0x80; //24:[46,47,48,49] - p_150server_1.embeddedInfoSrc.fault_battery = 0x2E; //25:[50] - p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0xAA; //26:[51] - p_150server_1.embeddedInfoSrc.fault_thrust = 0x76; //27:[52] + p_150server_1.embeddedInfoSrc.fault_leakSensor = 0x0; //24:[46,47,48,49] + p_150server_1.embeddedInfoSrc.fault_battery = 0x0; //25:[50] + p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0x0; //26:[51] + p_150server_1.embeddedInfoSrc.fault_thrust = 0x0; //27:[52] p_150server_1.embeddedInfoSrc.iridium = 0xFF; //28:[53] p_150server_1.embeddedInfoSrc.throwing_load = 0xFF; //29:[54] p_150server_1.embeddedInfoSrc.dvl_status = 0x00; //30:[55] diff --git a/src/pStateManagement/StateManagement.cpp b/src/pStateManagement/StateManagement.cpp index 787cf75..315f326 100644 --- a/src/pStateManagement/StateManagement.cpp +++ b/src/pStateManagement/StateManagement.cpp @@ -48,12 +48,12 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) #endif Json::Value deviceState; - double manualState; + std::string manualState; double missionState; if(key == "uManual_enable_cmd") { - manualState = msg.GetDouble(); + manualState = msg.GetString(); } if(key == "uMission_task_fb") { @@ -70,11 +70,11 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) missionState = missionStateData["state"].asInt(); } - if(fabs(manualState - 1) < 1e-6) //manualState=1 + if(manualState == "true") { deviceState["opMode"] = opModeLists.external; } - else if (fabs(manualState - 0) < 1e-6) //manualState=0 + else if (manualState == "faluse") { if(missionState == 0) { diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.cpp b/src/pSurfaceSupportComm/SurfaceSupportComm.cpp index 11d61dd..b0b3c65 100644 --- a/src/pSurfaceSupportComm/SurfaceSupportComm.cpp +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.cpp @@ -20,10 +20,10 @@ using namespace std; #define TCP_RECEIVE_PORT 8000 #define TCP_SEND_FILE_PORT 8002 -// #define DEST_IP_ADDRESS "10.25.0.230" //树莓派 -#define DEST_IP_ADDRESS "127.0.0.1" -//#define DEST_IP_ADDRESS "10.25.0.163" -//#define DEST_IP_ADDRESS "10.25.0.160" +#define SRC_IP_ADDRESS "127.0.0.1" +#define DEST_IP_ADDRESS "10.25.0.230" //树莓派 +// #define DEST_IP_ADDRESS "127.0.0.1" +// #define DEST_IP_ADDRESS "10.25.0.163" //--------------------------------------------------------- // Constructor @@ -96,7 +96,7 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) if(key == "uDevice_monitor_fb") { - std::string estimatedStateString = msg.GetString(); + std::string estimatedStateString = sval; std::string err; Json::Value estimatedStateData; std::istringstream iss(estimatedStateString); @@ -133,13 +133,13 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) } if(key == "uMission_task_fb") { - missionStatusString = msg.GetString(); + missionStatusString = sval; } if(key == "CPUTemperature") { - if (msg.GetString() != "Failed") + if (sval != "Failed") { - deviceStatus.controllerTemp = atof(msg.GetString().c_str()); + deviceStatus.controllerTemp = atof(sval.c_str()); } } @@ -189,10 +189,8 @@ bool SurfaceSupportComm::Iterate() announceMsg.lat = estimatedState.currentLat * M_PI / 180; announceMsg.lon = estimatedState.currentLon * M_PI / 180; announceMsg.height = estimatedState.depth; - // announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+udp://127.0.0.1:6001/;imc+tcp://127.0.0.1:8000/"); - announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+tcp://10.25.0.230:8000/"); - //announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+udp://10.25.0.160:6001/;imc+tcp://10.25.0.160:8000/"); - + // announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+tcp://10.25.0.160:8000/"); + announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+tcp://10.25.0.230:8000/"); DUNE::IMC::PlanControlState planControlStateMsg; planControlStateMsg.setTimeStamp(); @@ -608,56 +606,74 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) DUNE::IMC::PlanDB * msg = dynamic_cast(message); printf("server receive %s: %lf, %u, %u, %s\n", \ msg->getName(), msg->getTimeStamp(), msg->type, msg->op, msg->plan_id.c_str()); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << (int)msg->type << "," + << (int)msg->op << "," + << msg->plan_id; + appCastContent = appCastStream.str(); + if (msg->type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_SET) { std::string behaviorSpecString = msg->info; std::string err; Json::Value behaviorSpecData; std::istringstream iss(behaviorSpecString); - Json::CharReaderBuilder builder; - bool parsingResult = Json::parseFromStream(builder, iss, &behaviorSpecData, &err); - if (!parsingResult) { - std::cerr << "Failed to parse JSON string." << std::endl; - return; - } + Json::CharReaderBuilder builder; - behaviorSpecData["boardStamp"] = MOOS::Time(); - BatchConvertCoordinate(behaviorSpecData); - - std::string queryMemberName = behaviorSpecData["taskName"].asString(); - struct stat info; - if (stat(planConfigPath.c_str(), &info) != 0) + try { - Json::Value saveJsonValue; - saveJsonValue[queryMemberName] = behaviorSpecData; - Json::StreamWriterBuilder builder; - std::ofstream ofs; - ofs.open(planConfigPath, std::ios::out); - ofs << Json::writeString(builder, saveJsonValue) << std::endl; - ofs.close(); - } - else - { - std::ifstream ifs; - ifs.open(planConfigPath, std::ios::in); - Json::Reader taskConfigureReader; - Json::Value tempJsonValue; - taskConfigureReader.parse(ifs, tempJsonValue); - ifs.close(); + bool parsingResult = Json::parseFromStream(builder, iss, &behaviorSpecData, &err); + if (!parsingResult) { + throw ("PlanDB DBOP_SET parse error"); + } - Json::StreamWriterBuilder builder; - tempJsonValue[queryMemberName] = behaviorSpecData; - std::ofstream ofs; - ofs.open(planConfigPath, std::ios::out); - ofs << Json::writeString(builder, tempJsonValue) << std::endl; - ofs.close(); + behaviorSpecData["boardStamp"] = MOOS::Time(); + BatchConvertCoordinate(behaviorSpecData); + + std::string queryMemberName = behaviorSpecData["taskName"].asString(); + struct stat info; + if (stat(planConfigPath.c_str(), &info) != 0) + { + Json::Value saveJsonValue; + saveJsonValue[queryMemberName] = behaviorSpecData; + Json::StreamWriterBuilder builder; + std::ofstream ofs; + ofs.open(planConfigPath, std::ios::out); + ofs << Json::writeString(builder, saveJsonValue) << std::endl; + ofs.close(); + } + else + { + std::ifstream ifs; + ifs.open(planConfigPath, std::ios::in); + Json::Reader taskConfigureReader; + Json::Value tempJsonValue; + taskConfigureReader.parse(ifs, tempJsonValue); + ifs.close(); + + Json::StreamWriterBuilder builder; + tempJsonValue[queryMemberName] = behaviorSpecData; + std::ofstream ofs; + ofs.open(planConfigPath, std::ios::out); + ofs << Json::writeString(builder, tempJsonValue) << std::endl; + ofs.close(); + } + std::stringstream ss; + ss << std::fixed << std::setprecision(6) + << "PlanDB Set" << ":" + << MOOS::Time() << "," + << behaviorSpecData["taskName"].asString(); + Notify("uClient_plandbSet_log", ss.str()); + } + catch(std::string s) + { + std::cout << s << std::endl; + appCastContent = s; } - std::stringstream ss; - ss << std::fixed << std::setprecision(6) - << "PlanDB Set" << ":" - << MOOS::Time() << "," - << behaviorSpecData["taskName"].asString(); - Notify("uClient_plandbSet_log", ss.str()); } if (msg->type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_GET_STATE) { @@ -707,65 +723,91 @@ 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) + std::stringstream ss1; + ss1 << std::fixed << std::setprecision(6) << "SetEntityParameters" << ":" << MOOS::Time(); - for(; iter < msg->params.end(); iter++) + + try { - DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter); + for(; iter < msg->params.end(); iter++) + { + DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter); - std::string err; + std::string err; - Json::Value parameterValue; - std::istringstream iss(subEntityParameter->value); - Json::CharReaderBuilder builder; - bool parsingResult = Json::parseFromStream(builder, iss, ¶meterValue, &err); - if (!parsingResult) { - std::cerr << "Failed to parse JSON string." << std::endl; - return; - } + Json::Value parameterValue; + std::istringstream iss(subEntityParameter->value); + Json::CharReaderBuilder builder; + bool parsingResult = Json::parseFromStream(builder, iss, ¶meterValue, &err); + if (!parsingResult) + { + throw ("SetEntityParameters parse error"); + } + std::string parentName = msg->name; + std::string childName = subEntityParameter->name; + std::string dataType = parameterValue["type"].asString(); + std::string dataValueTemp = parameterValue["value"].asString(); + if (dataType == "float") + { + std::stringstream ss2(dataValueTemp); + float dataValue; + ss2 >> dataValue; + printf("%s, %s: %s, %f\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else if (dataType == "bool") + { + std::stringstream ss2(dataValueTemp); + bool dataValue; + ss2 >> dataValue; + printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else if (dataType == "int") + { + std::stringstream ss2(dataValueTemp); + int dataValue; + ss2 >> dataValue; + printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else + { + std::stringstream ss2(dataValueTemp); + std::string dataValue; + ss2 >> dataValue; + printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str()); + } + ss1 << "," << parentName << "," << childName << "," << dataValueTemp; + } + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << ss1.str(); + appCastContent = appCastStream.str(); - std::string parentName = msg->name; - std::string childName = subEntityParameter->name; - std::string dataType = parameterValue["type"].asString(); - std::string dataValueTemp = parameterValue["value"].asString(); - if (dataType == "float") - { - std::stringstream ss(dataValueTemp); - float dataValue; - ss >> dataValue; - printf("%s, %s: %s, %f\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); - } - else if (dataType == "bool") - { - std::stringstream ss(dataValueTemp); - bool dataValue; - ss >> dataValue; - printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); - } - else if (dataType == "int") - { - std::stringstream ss(dataValueTemp); - int dataValue; - ss >> dataValue; - printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); - } - else - { - std::stringstream ss(dataValueTemp); - 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", ss1.str()); + } + catch(std::string s) + { + std::cout << s << std::endl; + appCastContent = s; } - Notify("uClient_parameterSet_log", ss2.str()); } if (type == DUNE::IMC::PlanControl::getIdStatic()) { DUNE::IMC::PlanControl * msg = dynamic_cast(message); printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str()); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << (int)msg->type << "," + << (int)msg->op << "," + << msg->plan_id; + appCastContent = appCastStream.str(); + if (msg->type == DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST) { std::string action = ""; @@ -781,8 +823,8 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) Json::Value outputTaskValue; outputTaskValue["taskName"] = msg->plan_id; outputTaskValue["action"] = action; - Json::StreamWriterBuilder builder2; - std::string outputTaskString = Json::writeString(builder2, outputTaskValue); + Json::StreamWriterBuilder builder; + std::string outputTaskString = Json::writeString(builder, outputTaskValue); if (action == "start") { std::ifstream ifs; @@ -821,16 +863,25 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) if (type == DUNE::IMC::VehicleCommand::getIdStatic()) { DUNE::IMC::VehicleCommand * msg = dynamic_cast(message); - printf("server receive %s: %lf, %u, %u\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->command); + printf("server receive %s: %lf, %u, %u\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->command); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << (int)msg->type << "," + << (int)msg->command; + appCastContent = appCastStream.str(); + if (msg->type == DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST) { if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_EXEC_MANEUVER) { - Notify("uManual_enable_cmd", (double)msg->command); + Notify("uManual_enable_cmd", "true"); } if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_STOP_MANEUVER) { - Notify("uManual_enable_cmd", (double)msg->command); + Notify("uManual_enable_cmd", "false"); } } std::stringstream ss; @@ -843,22 +894,41 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) 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); + printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->actions.c_str()); + std::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << msg->actions; + appCastContent = appCastStream.str(); 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()); + try + { + bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); + if (!parsingResult) + { + throw ("RemoteActions parse error"); + } + 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()); + } + catch(std::string s) + { + std::cout << s << std::endl; + appCastContent = s; + } } if (type == DUNE::IMC::LogBookControl::getIdStatic()) { @@ -875,9 +945,18 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) 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::stringstream appCastStream; + appCastStream << std::fixed << std::setprecision(6) + << "server receive: " + << msg->getTimeStamp() << "," + << std::string(msg->getName()) << "," + << (int)msg->command << "," + << (int)logBookType << "," + << logBookContext << "," + << logBookText; + appCastContent = appCastStream.str(); std::string saveLogDir; m_MissionReader.GetValue("LogDir", saveLogDir); @@ -973,14 +1052,13 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) Json::Value retrFileObject; std::istringstream iss(logBookText); Json::CharReaderBuilder builder; - // std::cout << logBookText << std::endl; try { bool parsingResult = Json::parseFromStream(builder, iss, &retrFileObject, &err); if (!parsingResult) { - throw std::string("parsing json failure"); + throw std::string("LogBookEntry RETR parsing json failure"); } std::vector dirList = retrFileObject.getMemberNames(); @@ -1014,7 +1092,8 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) } catch(std::string s) { - std::cerr << s << std::endl; + std::cout << s << std::endl; + appCastContent = s; } } } @@ -1031,7 +1110,6 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) if (parsingResult) { - std::vector dirList = inputObject.getMemberNames(); int dirCount = dirList.size(); // Json::Value outputJsonValue; @@ -1130,7 +1208,6 @@ void SurfaceSupportComm::tcpProcessThread(const std::string& downloadFile) while (remaining > 0) { long int sendSize = sendfile(tcpSockConnect, fid, &offset, c_block_size); - std::cout << "sendSize: " << sendSize << std::endl; if (sendSize == -1) { break; @@ -1142,6 +1219,7 @@ void SurfaceSupportComm::tcpProcessThread(const std::string& downloadFile) catch(std::string s) { std::cout << s << std::endl; + appCastContent = s; } } @@ -1253,7 +1331,7 @@ bool SurfaceSupportComm::OnStartUp() if(tcpBindRet == -1) { struct sockaddr_in saddr; - saddr.sin_addr.s_addr = inet_addr(DEST_IP_ADDRESS); + saddr.sin_addr.s_addr = inet_addr(SRC_IP_ADDRESS); // saddr.sin_addr.s_addr = INADDR_ANY; saddr.sin_family = AF_INET; saddr.sin_port = htons(TCP_SEND_FILE_PORT); @@ -1271,7 +1349,8 @@ bool SurfaceSupportComm::OnStartUp() } catch(std::string s) { - std::cout << s << '\n'; + std::cout << s << std::endl; + appCastContent = s; } TimeLast = MOOS::Time(); @@ -1279,11 +1358,11 @@ bool SurfaceSupportComm::OnStartUp() return(true); } -// bool SurfaceSupportComm::buildReport() -// { -// m_msgs << "buildReport:" << testFlag++ << endl; -// return true; -// } +bool SurfaceSupportComm::buildReport() +{ + // m_msgs << appCastContent << endl; + return true; +} void SurfaceSupportComm::RegisterVariables() { diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.h b/src/pSurfaceSupportComm/SurfaceSupportComm.h index 7da2788..88d3d6f 100644 --- a/src/pSurfaceSupportComm/SurfaceSupportComm.h +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.h @@ -78,7 +78,7 @@ protected: // Standard MOOSApp functions to overload bool OnConnectToServer(); bool OnStartUp(); void RegisterVariables(); - // bool buildReport(); + bool buildReport(); private: enum MissionStatus{FAULT=0, UNRUN=1, MANUAL=2 ,RUN=3, CONFIG=5}; @@ -137,8 +137,6 @@ private: void tcpProcessThread(const std::string& downloadFile); double getFileSize(std::string filePath); - - int testFlag = 0; double TimeLast; int sendCount = 0; @@ -148,7 +146,7 @@ private: std::string vehicleName; unsigned int c_block_size; - std::list m_sockets; + std::string appCastContent; }; diff --git a/src/pTaskManagement/TaskManger.cpp b/src/pTaskManagement/TaskManger.cpp index 142553b..e99ccd7 100644 --- a/src/pTaskManagement/TaskManger.cpp +++ b/src/pTaskManagement/TaskManger.cpp @@ -123,7 +123,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail) if(msg_name == MSG_IN_MAN) { - if(msg_dval == 1) + if(msg_str == "true") state = MANUAL; else state = UNRUN;