zjk/moos-ivp-pi
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

解决了冲突

Browse Source
This commit is contained in:
chenlizhi
2023-11-28 11:11:01 +08:00
parent bee3fea02d c71064fcb4
commit faac316a30
24 changed files with 511 additions and 1324 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
bin/pBoardSupportComm Executable file
View File

Binary file not shown.

BIN
bin/pClientViewer Executable file
View File

Binary file not shown.

BIN
bin/pDataManagement Executable file
View File

Binary file not shown.

BIN
bin/pEmulator Executable file
View File

Binary file not shown.

BIN
bin/pStateManagement Executable file
View File

Binary file not shown.

BIN
bin/pSurfaceSupportComm Executable file
View File

Binary file not shown.

BIN
bin/pTaskManagement Executable file
View File

Binary file not shown.

0
bin/placeholder
View File

116
build/.cmake/api/v1/reply/index-2023-11-24T08-55-39-0072.json
View File

@@ -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
}
}
]
}
}
}
}

1
launch/launch.moos
View File

@@ -25,7 +25,6 @@ ClientCommandLog = clientCommand.txt
FaultLog = faultLog.txt FaultLog = faultLog.txt
MotionControlLog = motionControl.txt MotionControlLog = motionControl.txt
llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json
//------------------------------------------ //------------------------------------------

2
launch/launch_local.moos
View File

@@ -17,7 +17,7 @@ AltOrigin = 0
VehicleName = lauv-150 VehicleName = lauv-150
LogEnable = true LogEnable = false
//LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/ //LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/
LogDir = /home/ben/project/moos-ivp-pi/log/ LogDir = /home/ben/project/moos-ivp-pi/log/
AuvDataLog = auvData.mdat AuvDataLog = auvData.mdat

34
setting/PlanConfigure.json
View File

@@ -67,8 +67,8 @@
}, },
"plan1_toMoos" : "plan1_toMoos" :
{ {
"boardStamp" : 1699602762.2845099, "boardStamp" : 1700965801.2335429,
"clientStamp" : 1699602762.7520001, "clientStamp" : 1700965795.187,
"closedLoop" : false, "closedLoop" : false,
"duration" : -1.0, "duration" : -1.0,
"maxDepth" : -1.0, "maxDepth" : -1.0,
@@ -76,49 +76,49 @@
"origin" : "origin" :
{ {
"altitude" : 0.0, "altitude" : 0.0,
"lat" : 50.825299999999999, "lat" : 43.825299999999999,
"lon" : -90.330399999999997 "lon" : -70.330399999999997
}, },
"perpetual" : false, "perpetual" : false,
"points" : "points" :
[ [
{ {
"depth" : 2.0, "depth" : 2.0,
"east" : 117.83291847226671,
"lat" : 43.825713999999998, "lat" : 43.825713999999998,
"lon" : -70.32893, "lon" : -70.32893,
"name" : "Goto1", "name" : "Goto1",
"north" : 46.200319317940647,
"speed" : 2.0, "speed" : 2.0,
"type" : "point" "type" : "point"
}, },
{ {
"depth" : 2.0, "depth" : 2.0,
"east" : -17.18366087421261,
"lat" : 43.826782000000001, "lat" : 43.826782000000001,
"lon" : -70.330609999999993, "lon" : -70.330609999999993,
"name" : "Goto2", "name" : "Goto2",
"north" : 164.87635389378988,
"speed" : 2.0, "speed" : 2.0,
"type" : "point" "type" : "point"
}, },
{ {
"depth" : 2.0, "depth" : 2.0,
"east" : -241.19025325837993,
"lat" : 43.825465999999999, "lat" : 43.825465999999999,
"lon" : -70.333399999999997, "lon" : -70.333399999999997,
"name" : "Goto3", "name" : "Goto3",
"north" : 18.653618776002617,
"speed" : 2.0, "speed" : 2.0,
"type" : "point" "type" : "point"
}, },
{ {
"depth" : 2.0, "depth" : 2.0,
"east" : -203.76118848802312,
"lat" : 43.823234999999997, "lat" : 43.823234999999997,
"lon" : -70.332930000000005, "lon" : -70.332930000000005,
"name" : "Goto4", "name" : "Goto4",
"north" : -229.29782627916489, "speed" : 2.0,
"type" : "point"
},
{
"depth" : 2.0,
"lat" : 43.82414,
"lon" : -70.328389999999999,
"name" : "Goto5",
"speed" : 2.0, "speed" : 2.0,
"type" : "point" "type" : "point"
} }
@@ -127,7 +127,7 @@
"repeat" : 1, "repeat" : 1,
"sourceAddress" : "10.25.0.163", "sourceAddress" : "10.25.0.163",
"sourceName" : "CCU JHL 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" "taskName" : "plan1_toMoos"
}, },
"west_waypt_survey" : "west_waypt_survey" :
@@ -156,7 +156,7 @@
"name" : "station_1", "name" : "station_1",
"north" : -122.49101460421512, "north" : -122.49101460421512,
"speed" : 4.0, "speed" : 4.0,
"type" : "point" "type" : "point"
}, },
{ {
"depth" : 7.0, "depth" : 7.0,
@@ -166,7 +166,7 @@
"name" : "station_2", "name" : "station_2",
"north" : -111.04778559533926, "north" : -111.04778559533926,
"speed" : 6.0, "speed" : 6.0,
"type" : "point" "type" : "point"
}, },
{ {
"depth" : 5.0, "depth" : 5.0,
@@ -176,7 +176,7 @@
"name" : "station_3", "name" : "station_3",
"north" : -197.93630920628678, "north" : -197.93630920628678,
"speed" : 8.0, "speed" : 8.0,
"type" : "track" "type" : "track"
}, },
{ {
"depth" : 3.0, "depth" : 3.0,
@@ -186,7 +186,7 @@
"name" : "station_4", "name" : "station_4",
"north" : -232.26737690334403, "north" : -232.26737690334403,
"speed" : 10.0, "speed" : 10.0,
"type" : "track" "type" : "track"
} }
], ],
"priority" : 10, "priority" : 10,

174
src/pBoardSupportComm/BoardSupportComm.cpp
View File

@@ -13,31 +13,14 @@
#define TCP_RECEIVE_PORT 8001 #define TCP_RECEIVE_PORT 8001
#define TCP_SERVER_ADDRESS "127.0.0.1" #define TCP_SERVER_ADDRESS "127.0.0.1"
#define MOOS_AUV_SIM // #define MOOS_AUV_SIM
// #define MATLAB_AUV_SIM //#define MATLAB_AUV_SIM
// #ifdef TRUE_AUV #define TRUE_AUV
using namespace std; using namespace std;
BoardSupportComm::BoardSupportComm() 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.header = 0xEBA1;
embeddedInfo.count = 0; embeddedInfo.count = 0;
@@ -89,26 +72,23 @@ BoardSupportComm::BoardSupportComm()
executeCommand.footer = 0xEE2A; //16:[19,20] executeCommand.footer = 0xEE2A; //16:[19,20]
executeCommand.manual_mode = false; executeCommand.manual_mode = false;
tcpSockFD = -1; embeddedInfo.header = 0xEBA1;
embeddedInfo.count = 0;
tcpConnectRet = -1; tcpConnectRet = -1;
tcpSockFD = -1;
recvLen = -1;
} }
//--------------------------------------------------------- //---------------------------------------------------------
// Destructor // Destructor
BoardSupportComm::~BoardSupportComm() BoardSupportComm::~BoardSupportComm()
{ {
// delete tcpReceiveBuffer;
close(tcpSockFD); close(tcpSockFD);
} }
//---------------------------------------------------------
// Procedure: OnNewMail
bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
// AppCastingMOOSApp::OnNewMail(NewMail); AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p; MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) for(p=NewMail.begin(); p!=NewMail.end(); p++)
@@ -127,77 +107,78 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
if(key == "NAV_X") if(key == "NAV_X")
{ {
//E->N //E->N
estimatedState.offsetEast = msg.GetDouble(); embeddedInfo.east = (int)dval;
} }
if(key == "NAV_Y") if(key == "NAV_Y")
{ {
//N->E //N->E
estimatedState.offsetNorth = msg.GetDouble(); embeddedInfo.north = (int)dval;
} }
if(key == "NAV_Z") if(key == "NAV_Z")
{ {
//U->D //U->D
estimatedState.offsetDown = -msg.GetDouble(); embeddedInfo.depth = -1 * dval * 100;
} }
if(key == "NAV_YAW") if(key == "NAV_YAW")
{ {
double yawTemp = msg.GetDouble(); double yawTemp = dval; //radian
if (std::abs(yawTemp) <= M_PI) if (std::abs(yawTemp) <= M_PI)
{ {
if (yawTemp <= 0) if (yawTemp <= 0)
{ {
estimatedState.yaw = -yawTemp * 180 / M_PI; embeddedInfo.yaw = -yawTemp * 180 / M_PI * 100;
} }
else else
{ {
estimatedState.yaw = (2 * M_PI - yawTemp) * 180 / M_PI; embeddedInfo.yaw = (2 * M_PI - yawTemp) * 180 / M_PI * 100;
} }
} }
} }
if(key == "NAV_PITCH") if(key == "NAV_PITCH")
{ {
estimatedState.pitch = msg.GetDouble(); embeddedInfo.pitch = dval * 100;
} }
if(key == "NAV_LAT") if(key == "NAV_LAT")
{ {
estimatedState.currentLat = msg.GetDouble(); embeddedInfo.lat = dval * 1000000;
} }
if(key == "NAV_LONG") if(key == "NAV_LONG")
{ {
estimatedState.currentLon = msg.GetDouble(); embeddedInfo.lon = dval * 1000000;
} }
if(key == "NAV_SPEED") if(key == "NAV_SPEED")
{ {
estimatedState.linearVelocityNorth = msg.GetDouble() * cos(estimatedState.yaw); float tempYaw = (float)(embeddedInfo.yaw) / 100.0;
estimatedState.linearVelocityEast = -msg.GetDouble() * sin(estimatedState.yaw); embeddedInfo.ins_vx = dval * cos(tempYaw) * 100;
estimatedState.linearVelocityDown = 0; embeddedInfo.ins_vy = -dval * sin(tempYaw) * 100;
embeddedInfo.ins_vz = 0;
} }
if(key == "NAV_DEPTH") if(key == "NAV_DEPTH")
{ {
estimatedState.depth = msg.GetDouble(); embeddedInfo.depth = dval * 100;
} }
#endif
if(key == "Fault_LeakSensor") if(key == "Fault_LeakSensor")
{ {
embeddedInfo.fault_leakSensor = (uint32_t)msg.GetDouble(); embeddedInfo.fault_leakSensor = (uint32_t)dval;
} }
if(key == "Fault_Battery") if(key == "Fault_Battery")
{ {
embeddedInfo.fault_battery = (uint8_t)msg.GetDouble(); embeddedInfo.fault_battery = (uint8_t)dval;
} }
if(key == "Fault_EmergencyBattery") if(key == "Fault_EmergencyBattery")
{ {
embeddedInfo.fault_emergencyBattery = (uint8_t)(msg.GetDouble()); embeddedInfo.fault_emergencyBattery = (uint8_t)dval;
} }
if(key == "Fault_Thrust") if(key == "Fault_Thrust")
{ {
embeddedInfo.fault_thrust = (uint8_t)(msg.GetDouble()); embeddedInfo.fault_thrust = (uint8_t)dval;
} }
#endif
if(key == "uManual_enable_cmd") if(key == "uManual_enable_cmd")
{ {
if (msg.GetDouble() == 1.0) if (sval == "true")
{ {
executeCommand.drive_mode = 0x02; executeCommand.drive_mode = 0x02;
executeCommand.manual_mode = true; executeCommand.manual_mode = true;
@@ -212,11 +193,15 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
if(key == "uManual_drive_cmd") if(key == "uManual_drive_cmd")
{ {
#ifdef MOOS_AUV_SIM
if (executeCommand.manual_mode) if (executeCommand.manual_mode)
#else
if (embeddedInfo.drive_mode == 0x02)
#endif
{ {
std::string err; std::string err;
Json::Value recvCommand; Json::Value recvCommand;
std::istringstream iss(msg.GetString()); std::istringstream iss(sval);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult) if (!parsingResult)
@@ -271,7 +256,7 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
} }
std::string err; std::string err;
Json::Value recvCommand; Json::Value recvCommand;
std::istringstream iss(msg.GetString()); std::istringstream iss(sval);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult) if (!parsingResult)
@@ -319,7 +304,7 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
std::string err; std::string err;
Json::Value recvCommand; Json::Value recvCommand;
std::istringstream iss(msg.GetString()); std::istringstream iss(sval);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult) if (!parsingResult)
@@ -327,9 +312,9 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
std::cerr << "Failed to parse JSON string." << std::endl; std::cerr << "Failed to parse JSON string." << std::endl;
return false; return false;
} }
estimatedState.referenceAltitude = recvCommand["alt"].asFloat(); embeddedInfo.refLon = recvCommand["lon"].asFloat();
estimatedState.referenceLat = recvCommand["lat"].asFloat(); embeddedInfo.refLat = recvCommand["lat"].asFloat();
estimatedState.referenceLon = recvCommand["lon"].asFloat(); embeddedInfo.refAlt = recvCommand["alt"].asFloat();
struct stat info; struct stat info;
if (stat(llaOriginPath.c_str(), &info) == 0) if (stat(llaOriginPath.c_str(), &info) == 0)
@@ -366,9 +351,6 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
ofs.close(); ofs.close();
} }
} }
} }
return(true); return(true);
@@ -415,15 +397,15 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
embeddedState["driveMode"] = 0xFF; embeddedState["driveMode"] = 0xFF;
embeddedState["height"] = 0; embeddedState["height"] = 0;
embeddedState["depth"] = 0; embeddedState["depth"] = 0;
embeddedState["yaw"] = estimatedState.yaw; embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180;
embeddedState["pitch"] = 0; embeddedState["pitch"] = 0;
embeddedState["roll"] = 0; embeddedState["roll"] = 0;
embeddedState["insVX"] = estimatedState.linearVelocityNorth * 0.01; embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01;
embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01; embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01;
embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01; embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01;
embeddedState["currentLon"] = estimatedState.currentLon; embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
embeddedState["currentLat"] = estimatedState.currentLat; embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
embeddedState["currentAltitude"] = 0; embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
embeddedState["dvlVX"] = 0; embeddedState["dvlVX"] = 0;
embeddedState["dvlVY"] = 0; embeddedState["dvlVY"] = 0;
embeddedState["dvlVZ"] = 0; embeddedState["dvlVZ"] = 0;
@@ -432,10 +414,6 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
embeddedState["batteryVoltage"] = 15; embeddedState["batteryVoltage"] = 15;
embeddedState["batteryLevel"] = 60; embeddedState["batteryLevel"] = 60;
embeddedState["batteryTemp"] = 25; embeddedState["batteryTemp"] = 25;
// embeddedState["faultLeakSensor"] = 0;
// embeddedState["faultBattery"] = 0;
// embeddedState["faultEmergencyBattery"] = 0;
// embeddedState["faultThrust"] = 0;
embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor; embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
embeddedState["faultBattery"] = embeddedInfo.fault_battery; embeddedState["faultBattery"] = embeddedInfo.fault_battery;
embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery; embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
@@ -482,7 +460,7 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
embeddedState["depth"] = embeddedInfo.depth * 0.01; embeddedState["depth"] = embeddedInfo.depth * 0.01;
embeddedState["roll"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->N embeddedState["roll"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->N
embeddedState["pitch"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->E 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["insVX"] = embeddedInfo.ins_vy * 0.01; //E->N
embeddedState["insVY"] = embeddedInfo.ins_vx * 0.01; //N->E embeddedState["insVY"] = embeddedInfo.ins_vx * 0.01; //N->E
embeddedState["insVZ"] = -embeddedInfo.ins_vz * 0.01; //U->D 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 currentLon = embeddedInfo.lon * 0.000001;
double currentLat = embeddedInfo.lat * 0.000001; double currentLat = embeddedInfo.lat * 0.000001;
double currentAlt = embeddedInfo.alt * 0.01; double currentAlt = embeddedInfo.alt * 0.01;
std::vector<double> reference = {estimatedState.referenceLon, estimatedState.referenceLat, estimatedState.referenceAltitude}; std::vector<double> reference = {embeddedInfo.refLon, embeddedInfo.refLat, embeddedInfo.refAlt};
std::vector<double> current = {currentLon, currentLat, currentAlt}; std::vector<double> current = {currentLon, currentLat, currentAlt};
std::vector<double> ned = {0, 0, 0}; std::vector<double> ned = {0, 0, 0};
ConvertLLAToNED(reference, current, ned); ConvertLLAToNED(reference, current, ned);
embeddedState["north"] = ned.at(0); embeddedState["north"] = ned.at(0);
embeddedState["east"]= ned.at(1); embeddedState["east"]= ned.at(1);
embeddedState["referenceLon"]= estimatedState.referenceLon; embeddedState["referenceLon"]= embeddedInfo.refLon;
embeddedState["referenceLat"]= estimatedState.referenceLat; embeddedState["referenceLat"]= embeddedInfo.refLat;
embeddedState["referenceAltitude"]= estimatedState.referenceAltitude; embeddedState["referenceAltitude"]= embeddedInfo.refAlt;
Json::StreamWriterBuilder builder; Json::StreamWriterBuilder builder;
std::string embeddedStateString = Json::writeString(builder, embeddedState); return Json::writeString(builder, embeddedState);
return embeddedStateString;
} }
@@ -530,15 +507,15 @@ void BoardSupportComm::tcpProcessThread()
while(1) while(1)
{ {
bzero(tcpReceiveBuffer, 0); bzero(tcpReceiveBuffer, 0);
int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer)); recvLen = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
if(lens>0) if(recvLen > 0)
{ {
#ifndef MOOS_AUV_SIM #ifndef MOOS_AUV_SIM
parseMessage((unsigned char* )tcpReceiveBuffer, lens); parseMessage((unsigned char* )tcpReceiveBuffer, recvLen);
#endif #endif
Json::Value embeddedState; Json::Value embeddedState;
std::string embeddedStateString = convertEmbeddedFormat(embeddedState); recvContent = convertEmbeddedFormat(embeddedState);
Notify("uDevice_monitor_fb", embeddedStateString); Notify("uDevice_monitor_fb", recvContent);
#ifndef MOOS_AUV_SIM #ifndef MOOS_AUV_SIM
Notify("NAV_X", embeddedState["north"].asDouble()); Notify("NAV_X", embeddedState["north"].asDouble());
Notify("NAV_Y", embeddedState["east"].asDouble()); Notify("NAV_Y", embeddedState["east"].asDouble());
@@ -559,15 +536,16 @@ void BoardSupportComm::tcpProcessThread()
} }
} }
// bool BoardSupportComm::buildReport() bool BoardSupportComm::buildReport()
// { {
// m_msgs << "buildReport:" << embeddedStateString << endl; m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvLen:" << recvLen << endl;
// return true; m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvContent:" << recvContent << endl;
// } return true;
}
bool BoardSupportComm::Iterate() bool BoardSupportComm::Iterate()
{ {
// AppCastingMOOSApp::Iterate(); AppCastingMOOSApp::Iterate();
if(tcpSockFD == -1) if(tcpSockFD == -1)
{ {
@@ -591,8 +569,10 @@ bool BoardSupportComm::Iterate()
return false; return false;
} }
} }
if ((tcpSockFD != -1) && (tcpConnectRet != -1)) if ((tcpSockFD != -1) && (tcpConnectRet != -1))
{ {
std::thread t1(&BoardSupportComm::tcpProcessThread, this); std::thread t1(&BoardSupportComm::tcpProcessThread, this);
t1.detach(); t1.detach();
} }
@@ -665,7 +645,7 @@ bool BoardSupportComm::Iterate()
} }
#endif #endif
//AppCastingMOOSApp::PostReport(); AppCastingMOOSApp::PostReport();
return true; return true;
} }
@@ -763,7 +743,7 @@ int BoardSupportComm::parseMessage(unsigned char* buffer, int size)
bool BoardSupportComm::OnStartUp() bool BoardSupportComm::OnStartUp()
{ {
// AppCastingMOOSApp::OnStartUp(); AppCastingMOOSApp::OnStartUp();
m_MissionReader.GetValue("llaOriginPath", llaOriginPath); m_MissionReader.GetValue("llaOriginPath", llaOriginPath);
try try
{ {
@@ -782,9 +762,9 @@ bool BoardSupportComm::OnStartUp()
originJsonValue.isMember("LatOrigin") && originJsonValue.isMember("LatOrigin") &&
originJsonValue.isMember("AltOrigin")) originJsonValue.isMember("AltOrigin"))
{ {
estimatedState.referenceLon = originJsonValue["LongOrigin"].asFloat(); embeddedInfo.refLon = originJsonValue["LongOrigin"].asFloat();
estimatedState.referenceLat = originJsonValue["LatOrigin"].asFloat(); embeddedInfo.refLat = originJsonValue["LatOrigin"].asFloat();
estimatedState.referenceAltitude = originJsonValue["AltOrigin"].asFloat(); embeddedInfo.refAlt = originJsonValue["AltOrigin"].asFloat();
} }
else else
{ {
@@ -798,22 +778,18 @@ bool BoardSupportComm::OnStartUp()
} }
catch(...) catch(...)
{ {
m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon); m_MissionReader.GetValue("LongOrigin", embeddedInfo.refLon);
m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat); m_MissionReader.GetValue("LatOrigin", embeddedInfo.refLat);
m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude); m_MissionReader.GetValue("AltOrigin", embeddedInfo.refAlt);
} }
// std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", "
// << estimatedState.referenceLat << ", "
// << estimatedState.referenceAltitude << std::endl;
RegisterVariables(); RegisterVariables();
return(true); return(true);
} }
void BoardSupportComm::RegisterVariables() void BoardSupportComm::RegisterVariables()
{ {
// AppCastingMOOSApp::RegisterVariables(); AppCastingMOOSApp::RegisterVariables();
#ifdef MOOS_AUV_SIM #ifdef MOOS_AUV_SIM
Register("NAV_X", 0); Register("NAV_X", 0);

45
src/pBoardSupportComm/BoardSupportComm.h
View File

@@ -53,6 +53,12 @@ struct AUVEmbedded
uint8_t dvl_status; //30:[55] uint8_t dvl_status; //30:[55]
uint8_t crc; //31:[56] uint8_t crc; //31:[56]
uint16_t footer; //32:[57,58] uint16_t footer; //32:[57,58]
float refLon;
float refLat;
float refAlt;
float north;
float east;
}; };
struct AUVExecuteCommand struct AUVExecuteCommand
@@ -76,45 +82,22 @@ struct AUVExecuteCommand
bool manual_mode; bool manual_mode;
}; };
struct EstimatedState { class BoardSupportComm : public AppCastingMOOSApp
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
{ {
public: public:
BoardSupportComm(); BoardSupportComm();
~BoardSupportComm(); ~BoardSupportComm();
protected: // Standard MOOSApp functions to overload protected:
bool OnNewMail(MOOSMSG_LIST &NewMail); bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate(); bool Iterate();
bool OnConnectToServer(); bool OnConnectToServer();
bool OnStartUp(); bool OnStartUp();
void RegisterVariables(); void RegisterVariables();
// bool buildReport(); bool buildReport();
private: private:
struct EstimatedState estimatedState;
void ConvertLLAToENU(std::vector<double> init_lla, void ConvertLLAToENU(std::vector<double> init_lla,
std::vector<double> point_lla, std::vector<double> point_lla,
std::vector<double>& point_enu); std::vector<double>& point_enu);
@@ -129,7 +112,6 @@ private:
std::vector<double> &point_lla); std::vector<double> &point_lla);
void tcpProcessThread(); void tcpProcessThread();
// std::string convertEmbeddedFormat();
std::string convertEmbeddedFormat(Json::Value &embeddedState); std::string convertEmbeddedFormat(Json::Value &embeddedState);
int tcpSockFD; int tcpSockFD;
int tcpConnectRet; int tcpConnectRet;
@@ -144,12 +126,11 @@ private:
struct AUVEmbedded embeddedInfo; struct AUVEmbedded embeddedInfo;
struct AUVExecuteCommand executeCommand; 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;
}; };

2
src/pClientViewer/ClientViewer.cpp
View File

@@ -765,7 +765,7 @@ void ClientViewer::processMessage(DUNE::IMC::Message * message) {
for (; iter2 != entityParameter->params.end(); ++iter2) for (; iter2 != entityParameter->params.end(); ++iter2)
{ {
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2); DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; //std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;
} }
} }
} }

277
src/pDataManagement/DataManagement.cpp
View File

@@ -17,6 +17,7 @@ using namespace std;
// Constructor // Constructor
DataManagement::DataManagement() DataManagement::DataManagement()
{ {
logEnable = false;
motionControlInfo.desiredHeading = 0; motionControlInfo.desiredHeading = 0;
motionControlInfo.desiredSpeed = 0; motionControlInfo.desiredSpeed = 0;
@@ -44,26 +45,7 @@ DataManagement::DataManagement()
// Destructor // Destructor
DataManagement::~DataManagement() DataManagement::~DataManagement()
{ {
if(auvDataStream.is_open()) CloseOutputStream();
{
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();
}
} }
//--------------------------------------------------------- //---------------------------------------------------------
@@ -89,11 +71,25 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
bool mdbl = msg.IsDouble(); bool mdbl = msg.IsDouble();
bool mstr = msg.IsString(); bool mstr = msg.IsString();
if (key == "uClient_logEnable_cmd")
{
if (sval == "true")
{
logEnable = true;
OpenOutputStream();
}
else
{
logEnable = false;
CloseOutputStream();
}
}
if(key == "uDevice_monitor_fb") if(key == "uDevice_monitor_fb")
{ {
std::string err; std::string err;
Json::Value estimatedStateData; Json::Value estimatedStateData;
std::istringstream iss(msg.GetString()); std::istringstream iss(sval);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err); bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err);
if (!parsingResult) if (!parsingResult)
@@ -138,7 +134,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
std::string err; std::string err;
Json::Value missionStatusObject; Json::Value missionStatusObject;
std::istringstream iss(msg.GetString()); std::istringstream iss(sval);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err); bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err);
try try
@@ -168,10 +164,9 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
std::string err; std::string err;
Json::Value errorStatus; Json::Value errorStatus;
std::istringstream iss(msg.GetString()); std::istringstream iss(sval);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &errorStatus, &err); bool parsingResult = Json::parseFromStream(builder, iss, &errorStatus, &err);
// std::cout << "uFH_errorMsg: " << msg.GetString() << std::endl;
try try
{ {
if (!parsingResult) if (!parsingResult)
@@ -242,7 +237,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
} }
if(key == "DESIRED_HEADING") if(key == "DESIRED_HEADING")
{ {
motionControlInfo.desiredHeading = msg.GetDouble(); motionControlInfo.desiredHeading = dval;
std::stringstream ss; std::stringstream ss;
ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ",";
ss << motionControlInfo.desiredHeading << "," ss << motionControlInfo.desiredHeading << ","
@@ -252,7 +247,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
} }
if(key == "DESIRED_SPEED") if(key == "DESIRED_SPEED")
{ {
motionControlInfo.desiredSpeed = msg.GetDouble(); motionControlInfo.desiredSpeed = dval;
std::stringstream ss; std::stringstream ss;
ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ",";
ss << motionControlInfo.desiredHeading << "," ss << motionControlInfo.desiredHeading << ","
@@ -262,7 +257,7 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
} }
if(key == "DESIRED_DEPTH") if(key == "DESIRED_DEPTH")
{ {
motionControlInfo.desiredDepth = msg.GetDouble(); motionControlInfo.desiredDepth = dval;
std::stringstream ss; std::stringstream ss;
ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","; ss << std::fixed << std::setprecision(6) << MOOS::Time() << ",";
ss << motionControlInfo.desiredHeading << "," ss << motionControlInfo.desiredHeading << ","
@@ -372,92 +367,92 @@ bool DataManagement::OnStartUp()
{ {
AppCastingMOOSApp::OnStartUp(); AppCastingMOOSApp::OnStartUp();
list<string> sParams; // list<string> sParams;
m_MissionReader.EnableVerbatimQuoting(false); // m_MissionReader.EnableVerbatimQuoting(false);
if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { // if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
list<string>::iterator p; // list<string>::iterator p;
for(p=sParams.begin(); p!=sParams.end(); p++) { // for(p=sParams.begin(); p!=sParams.end(); p++) {
string line = *p; // string line = *p;
string param = tolower(biteStringX(line, '=')); // string param = tolower(biteStringX(line, '='));
string value = line; // string value = line;
if(param == "foo") { // if(param == "foo") {
//handled // //handled
} // }
else if(param == "bar") { // else if(param == "bar") {
//handled // //handled
} // }
} // }
} // }
std::string saveLogDir;
m_MissionReader.GetValue("LogDir", saveLogDir); m_MissionReader.GetValue("LogDir", saveLogDir);
std::string vehicleName; std::string vehicleName;
m_MissionReader.GetValue("VehicleName", vehicleName); m_MissionReader.GetValue("VehicleName", vehicleName);
bool logEnable = false;
m_MissionReader.GetValue("LogEnable", logEnable); m_MissionReader.GetValue("LogEnable", logEnable);
std::string auvDataFile; // std::string auvDataFile;
m_MissionReader.GetValue("AuvDataLog", auvDataFile); // m_MissionReader.GetValue("AuvDataLog", auvDataFile);
std::string missionHistoryFile; // std::string missionHistoryFile;
m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile); // m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile);
std::string clientCommandFile; // std::string clientCommandFile;
m_MissionReader.GetValue("ClientCommandLog", clientCommandFile); // m_MissionReader.GetValue("ClientCommandLog", clientCommandFile);
std::string faultLogFile; // std::string faultLogFile;
m_MissionReader.GetValue("FaultLog", faultLogFile); // m_MissionReader.GetValue("FaultLog", faultLogFile);
std::string motionControlFile; // std::string motionControlFile;
m_MissionReader.GetValue("MotionControlLog", 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 (logEnable)
{ {
if (access(saveLogDir.c_str(), F_OK) == -1 ) // std::string subDir;
{ // std::string localTime;
mode_t mode = 0775; // GenerateFileName(subDir, localTime);
mkdir(saveLogDir.c_str(), mode); // saveLogDir += "/" + subDir;
} // if (access(saveLogDir.c_str(), F_OK) == -1 )
saveLogDir += "/" + vehicleName; // {
if (access(saveLogDir.c_str(), F_OK) == -1 ) // mode_t mode = 0775;
{ // mkdir(saveLogDir.c_str(), mode);
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; // if(!OpenFile(auvDataStream, auvDataSavePath))
std::string localTime; // return MOOSFail("Failed to Open auvData file");
GenerateFileName(subDir, localTime); // if(!OpenFile(missionHistoryStream, missionHistorySavePath))
saveLogDir += "/" + subDir; // return MOOSFail("Failed to Open missionHistory file");
if (access(saveLogDir.c_str(), F_OK) == -1 ) // if(!OpenFile(clientCommandStream, clientCommandSavePath))
{ // return MOOSFail("Failed to Open clientCommand file");
mode_t mode = 0775; // if(!OpenFile(faultLogStream, faultLogSavePath))
mkdir(saveLogDir.c_str(), mode); // return MOOSFail("Failed to Open faultLog file");
} // if(!OpenFile(motionControlStream, motionControlSavePath))
saveLogDir += "/" + localTime; // return MOOSFail("Failed to Open faultLog file");
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)) // DoAuvDataLogBanner(auvDataStream);
return MOOSFail("Failed to Open auvData file"); // DoMissionHistoryBanner(missionHistoryStream);
if(!OpenFile(missionHistoryStream, missionHistorySavePath)) // DoFaultHandleBanner(faultLogStream);
return MOOSFail("Failed to Open missionHistory file"); // DoMotionControlBanner(motionControlStream);
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); OpenOutputStream();
DoMissionHistoryBanner(missionHistoryStream);
DoFaultHandleBanner(faultLogStream);
DoMotionControlBanner(motionControlStream);
} }
RegisterVariables(); RegisterVariables();
@@ -488,7 +483,8 @@ void DataManagement::RegisterVariables()
Register("DESIRED_HEADING", 0); Register("DESIRED_HEADING", 0);
Register("DESIRED_SPEED", 0); Register("DESIRED_SPEED", 0);
Register("DESIRED_DEPTH", 0); Register("DESIRED_DEPTH", 0);
Register("uMotion_desired_log", 0); Register("uMotion_desired_log", 0);
Register("uClient_logEnable_cmd", 0);
} }
bool DataManagement::buildReport() bool DataManagement::buildReport()
@@ -638,3 +634,82 @@ void DataManagement::GenerateFileName(std::string &fileDir, std::string &fileNam
ss << hour << minute << second; ss << hour << minute << second;
fileName = ss.str(); 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();
}
}

4
src/pDataManagement/DataManagement.h
View File

@@ -44,6 +44,8 @@ protected:
void DoMotionControlBanner(std::ofstream &os); void DoMotionControlBanner(std::ofstream &os);
void GenerateFileName(std::string &fileDir, std::string &fileName); void GenerateFileName(std::string &fileDir, std::string &fileName);
bool OpenOutputStream();
void CloseOutputStream();
double getTimeStamp(); double getTimeStamp();
int nDoublePrecision; int nDoublePrecision;
std::ofstream auvDataStream; std::ofstream auvDataStream;
@@ -54,6 +56,8 @@ protected:
std::map<std::string, int> logVarList; std::map<std::string, int> logVarList;
std::string contentFromStream; std::string contentFromStream;
struct MotionControlInfo motionControlInfo; struct MotionControlInfo motionControlInfo;
bool logEnable;
std::string saveLogDir;
}; };
#endif #endif

809
src/pDataManagement/NavigationInfo.pb.h
View File

@@ -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 <limits>
#include <string>
#include <google/protobuf/port_def.inc>
#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 <google/protobuf/port_undef.inc>
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/arena.h>
#include <google/protobuf/arenastring.h>
#include <google/protobuf/generated_message_util.h>
#include <google/protobuf/metadata_lite.h>
#include <google/protobuf/generated_message_reflection.h>
#include <google/protobuf/message.h>
#include <google/protobuf/repeated_field.h> // IWYU pragma: export
#include <google/protobuf/extension_set.h> // IWYU pragma: export
#include <google/protobuf/unknown_field_set.h>
// @@protoc_insertion_point(includes)
#include <google/protobuf/port_def.inc>
#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<const EstimatedState*>(
&_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<EstimatedState>(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 <typename ArgT0 = const std::string&, typename... ArgT>
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 <typename T> 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 <typename ArgT0, typename... ArgT>
inline PROTOBUF_ALWAYS_INLINE
void EstimatedState::set_info(ArgT0&& arg0, ArgT... args) {
_impl_.info_.Set(static_cast<ArgT0 &&>(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 <google/protobuf/port_undef.inc>
#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto

10
src/pEmulator/Emulator.cpp
View File

@@ -401,13 +401,13 @@ void Emulator::set150Info()
p_150server_1.embeddedInfoSrc.dvl_vz = (int16_t)remus100.w; //18:[36,37] 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.rpm = (int16_t)remus100.thrust; //19:[38,39]
p_150server_1.embeddedInfoSrc.lightEnable = (uint8_t)0x01; //20:[40] 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_level = 60; //22:[43]
p_150server_1.embeddedInfoSrc.battery_temp = 21.3 / 0.1; //23:[44,45] 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_leakSensor = 0x0; //24:[46,47,48,49]
p_150server_1.embeddedInfoSrc.fault_battery = 0x2E; //25:[50] p_150server_1.embeddedInfoSrc.fault_battery = 0x0; //25:[50]
p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0xAA; //26:[51] p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0x0; //26:[51]
p_150server_1.embeddedInfoSrc.fault_thrust = 0x76; //27:[52] p_150server_1.embeddedInfoSrc.fault_thrust = 0x0; //27:[52]
p_150server_1.embeddedInfoSrc.iridium = 0xFF; //28:[53] p_150server_1.embeddedInfoSrc.iridium = 0xFF; //28:[53]
p_150server_1.embeddedInfoSrc.throwing_load = 0xFF; //29:[54] p_150server_1.embeddedInfoSrc.throwing_load = 0xFF; //29:[54]
p_150server_1.embeddedInfoSrc.dvl_status = 0x00; //30:[55] p_150server_1.embeddedInfoSrc.dvl_status = 0x00; //30:[55]

4
src/pFaultHandle/FaultHandle.cpp
View File

@@ -1,8 +1,8 @@
/************************************************************/ /************************************************************/
/* NAME: wade */ /* NAME: wade */
/* ORGN: MIT */ /* ORGN: JHL */
/* FILE: FaultHandle.cpp */ /* FILE: FaultHandle.cpp */
/* DATE: */ /* LAST MODIFY DATE: 20231127 */
/************************************************************/ /************************************************************/
#include <iterator> #include <iterator>

14
src/pStateManagement/StateManagement.cpp
View File

@@ -47,12 +47,12 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
bool mstr = msg.IsString(); bool mstr = msg.IsString();
Json::Value deviceState; Json::Value deviceState;
double manualState; std::string manualState;
int missionState; int missionState;
if(key == "uManual_enable_cmd") if(key == "uManual_enable_cmd")
{ {
manualState = msg.GetDouble(); manualState = msg.GetString();
} }
if(key == "uMission_task_fb") if(key == "uMission_task_fb")
{ {
@@ -69,13 +69,13 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
missionState = missionStateData["state"].asInt(); missionState = missionStateData["state"].asInt();
} }
if(fabs(manualState - 1) < 1e-6) //manualState=1 if(manualState == "true")
{ {
deviceState["opMode"] = opModeLists.external; deviceState["opMode"] = opModeLists.external;
} }
else if (fabs(manualState - 0) < 1e-6) //manualState=0 else if (manualState == "faluse")
{ {
switch (missionState) if(missionState == 0)
{ {
case 0: case 0:
deviceState["opMode"] = opModeLists.error; deviceState["opMode"] = opModeLists.error;

335
src/pSurfaceSupportComm/SurfaceSupportComm.cpp
View File

@@ -20,10 +20,10 @@ using namespace std;
#define TCP_RECEIVE_PORT 8000 #define TCP_RECEIVE_PORT 8000
#define TCP_SEND_FILE_PORT 8002 #define TCP_SEND_FILE_PORT 8002
// #define DEST_IP_ADDRESS "10.25.0.230" //树莓派 #define SRC_IP_ADDRESS "127.0.0.1"
#define DEST_IP_ADDRESS "127.0.0.1" #define DEST_IP_ADDRESS "10.25.0.230" //树莓派
//#define DEST_IP_ADDRESS "10.25.0.163" // #define DEST_IP_ADDRESS "127.0.0.1"
//#define DEST_IP_ADDRESS "10.25.0.160" // #define DEST_IP_ADDRESS "10.25.0.163"
//--------------------------------------------------------- //---------------------------------------------------------
// Constructor // Constructor
@@ -96,7 +96,7 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
if(key == "uDevice_monitor_fb") if(key == "uDevice_monitor_fb")
{ {
std::string estimatedStateString = msg.GetString(); std::string estimatedStateString = sval;
std::string err; std::string err;
Json::Value estimatedStateData; Json::Value estimatedStateData;
std::istringstream iss(estimatedStateString); std::istringstream iss(estimatedStateString);
@@ -133,13 +133,13 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
} }
if(key == "uMission_task_fb") if(key == "uMission_task_fb")
{ {
missionStatusString = msg.GetString(); missionStatusString = sval;
} }
if(key == "CPUTemperature") 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.lat = estimatedState.currentLat * M_PI / 180;
announceMsg.lon = estimatedState.currentLon * M_PI / 180; announceMsg.lon = estimatedState.currentLon * M_PI / 180;
announceMsg.height = estimatedState.depth; 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.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/"); 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/");
DUNE::IMC::PlanControlState planControlStateMsg; DUNE::IMC::PlanControlState planControlStateMsg;
planControlStateMsg.setTimeStamp(); planControlStateMsg.setTimeStamp();
@@ -608,56 +606,74 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message); DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
printf("server receive %s: %lf, %u, %u, %s\n", \ printf("server receive %s: %lf, %u, %u, %s\n", \
msg->getName(), msg->getTimeStamp(), msg->type, msg->op, msg->plan_id.c_str()); 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) if (msg->type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_SET)
{ {
std::string behaviorSpecString = msg->info; std::string behaviorSpecString = msg->info;
std::string err; std::string err;
Json::Value behaviorSpecData; Json::Value behaviorSpecData;
std::istringstream iss(behaviorSpecString); std::istringstream iss(behaviorSpecString);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &behaviorSpecData, &err);
if (!parsingResult) {
std::cerr << "Failed to parse JSON string." << std::endl;
return;
}
behaviorSpecData["boardStamp"] = MOOS::Time(); try
BatchConvertCoordinate(behaviorSpecData);
std::string queryMemberName = behaviorSpecData["taskName"].asString();
struct stat info;
if (stat(planConfigPath.c_str(), &info) != 0)
{ {
Json::Value saveJsonValue; bool parsingResult = Json::parseFromStream(builder, iss, &behaviorSpecData, &err);
saveJsonValue[queryMemberName] = behaviorSpecData; if (!parsingResult) {
Json::StreamWriterBuilder builder; throw ("PlanDB DBOP_SET parse error");
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; behaviorSpecData["boardStamp"] = MOOS::Time();
tempJsonValue[queryMemberName] = behaviorSpecData; BatchConvertCoordinate(behaviorSpecData);
std::ofstream ofs;
ofs.open(planConfigPath, std::ios::out); std::string queryMemberName = behaviorSpecData["taskName"].asString();
ofs << Json::writeString(builder, tempJsonValue) << std::endl; struct stat info;
ofs.close(); 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) 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<DUNE::IMC::SetEntityParameters *>(message); DUNE::IMC::SetEntityParameters * msg = dynamic_cast<DUNE::IMC::SetEntityParameters *>(message);
printf("server receive %s: %lf\n", msg->getName(), msg->getTimeStamp()); printf("server receive %s: %lf\n", msg->getName(), msg->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter = msg->params.begin(); DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter = msg->params.begin();
std::stringstream ss2; std::stringstream ss1;
ss2 << std::fixed << std::setprecision(6) ss1 << std::fixed << std::setprecision(6)
<< "SetEntityParameters" << ":" << "SetEntityParameters" << ":"
<< MOOS::Time(); << MOOS::Time();
for(; iter < msg->params.end(); iter++)
try
{ {
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter); for(; iter < msg->params.end(); iter++)
{
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter);
std::string err; std::string err;
Json::Value parameterValue; Json::Value parameterValue;
std::istringstream iss(subEntityParameter->value); std::istringstream iss(subEntityParameter->value);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &parameterValue, &err); bool parsingResult = Json::parseFromStream(builder, iss, &parameterValue, &err);
if (!parsingResult) { if (!parsingResult)
std::cerr << "Failed to parse JSON string." << std::endl; {
return; 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; Notify("uClient_parameterSet_log", ss1.str());
std::string childName = subEntityParameter->name; }
std::string dataType = parameterValue["type"].asString(); catch(std::string s)
std::string dataValueTemp = parameterValue["value"].asString(); {
if (dataType == "float") std::cout << s << std::endl;
{ appCastContent = s;
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", ss2.str());
} }
if (type == DUNE::IMC::PlanControl::getIdStatic()) if (type == DUNE::IMC::PlanControl::getIdStatic())
{ {
DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message); DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message);
printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str()); 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) if (msg->type == DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST)
{ {
std::string action = ""; std::string action = "";
@@ -781,8 +823,8 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
Json::Value outputTaskValue; Json::Value outputTaskValue;
outputTaskValue["taskName"] = msg->plan_id; outputTaskValue["taskName"] = msg->plan_id;
outputTaskValue["action"] = action; outputTaskValue["action"] = action;
Json::StreamWriterBuilder builder2; Json::StreamWriterBuilder builder;
std::string outputTaskString = Json::writeString(builder2, outputTaskValue); std::string outputTaskString = Json::writeString(builder, outputTaskValue);
if (action == "start") if (action == "start")
{ {
std::ifstream ifs; std::ifstream ifs;
@@ -821,16 +863,25 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
if (type == DUNE::IMC::VehicleCommand::getIdStatic()) if (type == DUNE::IMC::VehicleCommand::getIdStatic())
{ {
DUNE::IMC::VehicleCommand * msg = dynamic_cast<DUNE::IMC::VehicleCommand *>(message); DUNE::IMC::VehicleCommand * msg = dynamic_cast<DUNE::IMC::VehicleCommand *>(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->type == DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST)
{ {
if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_EXEC_MANEUVER) 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) 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; std::stringstream ss;
@@ -843,22 +894,41 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
if (type == DUNE::IMC::RemoteActions::getIdStatic()) if (type == DUNE::IMC::RemoteActions::getIdStatic())
{ {
DUNE::IMC::RemoteActions * msg = dynamic_cast<DUNE::IMC::RemoteActions *>(message); DUNE::IMC::RemoteActions * msg = dynamic_cast<DUNE::IMC::RemoteActions *>(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); Notify("uManual_drive_cmd", msg->actions);
std::string err; std::string err;
Json::Value recvCommand; Json::Value recvCommand;
std::istringstream iss(msg->actions); std::istringstream iss(msg->actions);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err); try
std::stringstream ss; {
ss << std::fixed << std::setprecision(6) bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
<< "RemoteActions" << ":" if (!parsingResult)
<< MOOS::Time() << "," {
<< "Thrust" << "," throw ("RemoteActions parse error");
<< recvCommand["Thrust"].asInt() << "," }
<< "Heading" << "," std::stringstream ss;
<< recvCommand["Heading"].asFloat(); ss << std::fixed << std::setprecision(6)
Notify("uClient_manualDrive_log", ss.str()); << "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()) if (type == DUNE::IMC::LogBookControl::getIdStatic())
{ {
@@ -875,9 +945,18 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
logBookContext = msgLogBookEntry->context; logBookContext = msgLogBookEntry->context;
logBookText = msgLogBookEntry->text; logBookText = msgLogBookEntry->text;
} }
printf("server receive %s: %lf, %u, %u, %s\n", msg->getName(), msg->getTimeStamp(), printf("server receive %s: %lf, %u, %u, %s\n", msg->getName(), msg->getTimeStamp(),
msg->command, logBookType, logBookContext.c_str()); 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; std::string saveLogDir;
m_MissionReader.GetValue("LogDir", saveLogDir); m_MissionReader.GetValue("LogDir", saveLogDir);
@@ -973,14 +1052,13 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
Json::Value retrFileObject; Json::Value retrFileObject;
std::istringstream iss(logBookText); std::istringstream iss(logBookText);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
// std::cout << logBookText << std::endl;
try try
{ {
bool parsingResult = Json::parseFromStream(builder, iss, &retrFileObject, &err); bool parsingResult = Json::parseFromStream(builder, iss, &retrFileObject, &err);
if (!parsingResult) if (!parsingResult)
{ {
throw std::string("parsing json failure"); throw std::string("LogBookEntry RETR parsing json failure");
} }
std::vector<std::string> dirList = retrFileObject.getMemberNames(); std::vector<std::string> dirList = retrFileObject.getMemberNames();
@@ -1014,7 +1092,8 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
} }
catch(std::string s) 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) if (parsingResult)
{ {
std::vector<std::string> dirList = inputObject.getMemberNames(); std::vector<std::string> dirList = inputObject.getMemberNames();
int dirCount = dirList.size(); int dirCount = dirList.size();
// Json::Value outputJsonValue; // Json::Value outputJsonValue;
@@ -1130,7 +1208,6 @@ void SurfaceSupportComm::tcpProcessThread(const std::string& downloadFile)
while (remaining > 0) while (remaining > 0)
{ {
long int sendSize = sendfile(tcpSockConnect, fid, &offset, c_block_size); long int sendSize = sendfile(tcpSockConnect, fid, &offset, c_block_size);
std::cout << "sendSize: " << sendSize << std::endl;
if (sendSize == -1) if (sendSize == -1)
{ {
break; break;
@@ -1142,6 +1219,7 @@ void SurfaceSupportComm::tcpProcessThread(const std::string& downloadFile)
catch(std::string s) catch(std::string s)
{ {
std::cout << s << std::endl; std::cout << s << std::endl;
appCastContent = s;
} }
} }
@@ -1253,7 +1331,7 @@ bool SurfaceSupportComm::OnStartUp()
if(tcpBindRet == -1) if(tcpBindRet == -1)
{ {
struct sockaddr_in saddr; 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_addr.s_addr = INADDR_ANY;
saddr.sin_family = AF_INET; saddr.sin_family = AF_INET;
saddr.sin_port = htons(TCP_SEND_FILE_PORT); saddr.sin_port = htons(TCP_SEND_FILE_PORT);
@@ -1271,7 +1349,8 @@ bool SurfaceSupportComm::OnStartUp()
} }
catch(std::string s) catch(std::string s)
{ {
std::cout << s << '\n'; std::cout << s << std::endl;
appCastContent = s;
} }
TimeLast = MOOS::Time(); TimeLast = MOOS::Time();
@@ -1279,11 +1358,11 @@ bool SurfaceSupportComm::OnStartUp()
return(true); return(true);
} }
// bool SurfaceSupportComm::buildReport() bool SurfaceSupportComm::buildReport()
// { {
// m_msgs << "buildReport:" << testFlag++ << endl; // m_msgs << appCastContent << endl;
// return true; return true;
// } }
void SurfaceSupportComm::RegisterVariables() void SurfaceSupportComm::RegisterVariables()
{ {

6
src/pSurfaceSupportComm/SurfaceSupportComm.h
View File

@@ -78,7 +78,7 @@ protected: // Standard MOOSApp functions to overload
bool OnConnectToServer(); bool OnConnectToServer();
bool OnStartUp(); bool OnStartUp();
void RegisterVariables(); void RegisterVariables();
// bool buildReport(); bool buildReport();
private: private:
enum MissionStatus{FAULT=0, UNRUN=1, MANUAL=2 ,RUN=3, CONFIG=5}; enum MissionStatus{FAULT=0, UNRUN=1, MANUAL=2 ,RUN=3, CONFIG=5};
@@ -137,8 +137,6 @@ private:
void tcpProcessThread(const std::string& downloadFile); void tcpProcessThread(const std::string& downloadFile);
double getFileSize(std::string filePath); double getFileSize(std::string filePath);
int testFlag = 0;
double TimeLast; double TimeLast;
int sendCount = 0; int sendCount = 0;
@@ -148,7 +146,7 @@ private:
std::string vehicleName; std::string vehicleName;
unsigned int c_block_size; unsigned int c_block_size;
std::list<DUNE::Network::TCPSocket*> m_sockets; std::string appCastContent;
}; };

2
src/pTaskManagement/TaskManger.cpp
View File

@@ -123,7 +123,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
if(msg_name == MSG_IN_MAN) if(msg_name == MSG_IN_MAN)
{ {
if(msg_dval == 1) if(msg_str == "true")
state = MANUAL; state = MANUAL;
else else
state = UNRUN; state = UNRUN;