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
MotionControlLog = motionControl.txt
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
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

34
setting/PlanConfigure.json
View File

@@ -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,

174
src/pBoardSupportComm/BoardSupportComm.cpp
View File

@@ -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<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> 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);

45
src/pBoardSupportComm/BoardSupportComm.h
View File

@@ -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<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_enu);
@@ -129,7 +112,6 @@ private:
std::vector<double> &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;
};

2
src/pClientViewer/ClientViewer.cpp
View File

@@ -765,7 +765,7 @@ void ClientViewer::processMessage(DUNE::IMC::Message * message) {
for (; iter2 != entityParameter->params.end(); ++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
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<string> sParams;
m_MissionReader.EnableVerbatimQuoting(false);
if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
list<string>::iterator p;
for(p=sParams.begin(); p!=sParams.end(); p++) {
string line = *p;
string param = tolower(biteStringX(line, '='));
string value = line;
// list<string> sParams;
// m_MissionReader.EnableVerbatimQuoting(false);
// if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
// list<string>::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();
}
}

4
src/pDataManagement/DataManagement.h
View File

@@ -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<std::string, int> logVarList;
std::string contentFromStream;
struct MotionControlInfo motionControlInfo;
bool logEnable;
std::string saveLogDir;
};
#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.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]

4
src/pFaultHandle/FaultHandle.cpp
View File

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

14
src/pStateManagement/StateManagement.cpp
View File

@@ -47,12 +47,12 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
bool mstr = msg.IsString();
Json::Value deviceState;
double manualState;
int missionState;
std::string manualState;
int missionState;
if(key == "uManual_enable_cmd")
{
manualState = msg.GetDouble();
manualState = msg.GetString();
}
if(key == "uMission_task_fb")
{
@@ -69,13 +69,13 @@ 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
{
switch (missionState)
else if (manualState == "faluse")
{
if(missionState == 0)
{
case 0:
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_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<DUNE::IMC::PlanDB *>(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<DUNE::IMC::SetEntityParameters *>(message);
printf("server receive %s: %lf\n", msg->getName(), msg->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::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<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;
std::istringstream iss(subEntityParameter->value);
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &parameterValue, &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, &parameterValue, &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<DUNE::IMC::PlanControl *>(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<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->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<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);
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<std::string> 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<std::string> 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()
{

6
src/pSurfaceSupportComm/SurfaceSupportComm.h
View File

@@ -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<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_dval == 1)
if(msg_str == "true")
state = MANUAL;
else
state = UNRUN;