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

迁移pi上的feture-mc分支

Browse Source
This commit is contained in:
zjk
2023-11-20 09:58:12 +08:00
parent 036fbdbc9f
commit 1004233e20
26 changed files with 3173 additions and 618 deletions
Download Patch File Download Diff File Expand all files Collapse all files

65
PlanConfigure.json
View File

@@ -45,6 +45,71 @@
"taskId" : "1",
"taskName" : "east_waypt_survey"
},
"plan1_toMoos" :
{
"boardStamp" : 1699602762.2845099,
"clientStamp" : 1699602762.7520001,
"closedLoop" : false,
"duration" : -1.0,
"maxDepth" : -1.0,
"minDepth" : -1.0,
"origin" :
{
"altitude" : 0.0,
"lat" : 43.825299999999999,
"lon" : -70.330399999999997
},
"perpetual" : false,
"points" :
[
{
"depth" : 2.0,
"east" : 117.83291847226671,
"lat" : 43.825713999999998,
"lon" : -70.32893,
"name" : "Goto1",
"north" : 46.200319317940647,
"speed" : 2.0,
"type" : "point"
},
{
"depth" : 2.0,
"east" : -17.18366087421261,
"lat" : 43.826782000000001,
"lon" : -70.330609999999993,
"name" : "Goto2",
"north" : 164.87635389378988,
"speed" : 2.0,
"type" : "point"
},
{
"depth" : 2.0,
"east" : -241.19025325837993,
"lat" : 43.825465999999999,
"lon" : -70.333399999999997,
"name" : "Goto3",
"north" : 18.653618776002617,
"speed" : 2.0,
"type" : "point"
},
{
"depth" : 2.0,
"east" : -203.76118848802312,
"lat" : 43.823234999999997,
"lon" : -70.332930000000005,
"name" : "Goto4",
"north" : -229.29782627916489,
"speed" : 2.0,
"type" : "point"
}
],
"priority" : 10,
"repeat" : 1,
"sourceAddress" : "10.25.0.163",
"sourceName" : "CCU JHL 0_163",
"taskId" : "0,106,3,-96,8,-103,13,6,9,32,50,-13,47,-71,61,1",
"taskName" : "plan1_toMoos"
},
"west_waypt_survey" :
{
"boardStamp" : 1698135268.3958621,

34
launch/launch.moos
View File

@@ -15,6 +15,19 @@ LatOrigin = 43.825300
LongOrigin = -70.330400
AltOrigin = 0
VehicleName = lauv-150
LogEnable = false
LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/
AuvDataLog = auvData.txt
MissionHistoryLog = missionHistory.txt
ClientCommandLog = clientCommand.txt
FaultLog = faultLog.txt
planConfigPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/PlanConfigure.json
llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
@@ -27,18 +40,18 @@ ProcessConfig = ANTLER
Run = uProcessWatch @ NewConsole = false
Run = pRealm @ NewConsole = false
Run = pShare @ NewConsole = false
//Run = pMarineViewer @ NewConsole = false
//Run = pLogger @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
//===========Our define process====================
Run = pBoardSupportComm @ NewConsole = false
Run = pTaskManger @ NewConsole = false
Run = pMotionControler @ NewConsole = false
//Run = pSurfaceSupportComm @ NewConsole = false
Run = pSurfaceSupportComm @ NewConsole = false
Run = pDataManagement @ NewConsole = false
//===============For test process===================
Run = pEmulator @ NewConsole = false
//Run = uSimMarine @ NewConsole = false
//Run = pMarinePID @ NewConsole = false
Run = pFaultHandle @ NewConsole = false
}
ProcessConfig = pMarineViewer
@@ -195,9 +208,6 @@ ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
planConfigPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/PlanConfigure.json
//planConfigPath = /home/ben/project/moos-ivp-pi/PlanConfigure.json
}
ProcessConfig = pBoardSupportComm
@@ -259,13 +269,13 @@ ProcessConfig = pMotionControler
cheak_stalensee = true
delta_freqency = 5
config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/src/pMotionControler/ControlParam.json
//config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/src/pMotionControler/ControlParam.json
config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/ControlParam.json
}
ProcessConfig = pEmulator
{
AppTick = 100
AppTick = 5
CommsTick = 5
matlab_host = 10.25.0.137
matlab_port = 8085
@@ -304,3 +314,9 @@ ProcessConfig = pLogger
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
ProcessConfig = pFaultHandle
{
AppTick = 4
CommsTick = 4
}

43
setting/ControlParam.json Normal file
View File

@@ -0,0 +1,43 @@
{
"speed" :
{
"Kp" : 10.0,
"Ki" : 5.0,
"Kd" : 0.0,
"LimitDelta" : 50,
"MaxOut" : 100,
"MinOut" : 0
},
"heading" :
{
"Kp" : 0.8,
"Ki" : 0.05,
"Kd" : 2.2,
"LimitDelta" : 5,
"MaxOut" : 30,
"MinOut" : -30
},
"depth" :
{
"Kp" : 10.0,
"Ki" : 0.3,
"Kd" : 2.0,
"LimitDelta" : 5,
"MaxOut" : 10,
"MinOut" : -10
},
"pitch" :
{
"Kp" : 0.6,
"Ki" : 0.03,
"Kd" : 1.5,
"LimitDelta" : 5,
"MaxOut" : 30,
"MinOut" : -30
},
"const_thrust" : 0,
"dead_zone" : 10,
"speedCol" : true,
"depthCol" : true,
"HeadingCol" : true
}

6
setting/Origin.json Normal file
View File

@@ -0,0 +1,6 @@
{
"AltOrigin" : 0,
"LatOrigin" : 50.825298309326172,
"LongOrigin" : -90.330398559570312,
"TaskName" : "east_waypt_survey"
}

195
setting/PlanConfigure.json Normal file
View File

@@ -0,0 +1,195 @@
{
"east_waypt_survey" :
{
"boardStamp" : 1699945816.369447,
"clientStamp" : 1699945815.876014,
"closedLoop" : true,
"constSpeed" : -1.0,
"duration" : -1.0,
"maxDepth" : -1.0,
"minDepth" : -1.0,
"origin" :
{
"altitude" : 0.0,
"lat" : 50.825299999999999,
"lon" : -90.330399999999997
},
"perpetual" : false,
"points" :
[
{
"depth" : 10.0,
"east" : 121.51780491942635,
"lat" : 43.824428558349609,
"lon" : -70.328887939453125,
"name" : "station_1",
"north" : -96.635898081838207,
"speed" : 3.0,
"type" : "point"
},
{
"depth" : 8.0,
"east" : 201.91511278899367,
"lat" : 43.824676513671875,
"lon" : -70.327888488769531,
"name" : "station_2",
"north" : -69.083922179977435,
"speed" : 5.0,
"type" : "point"
},
{
"depth" : 6.0,
"east" : 203.75986507534614,
"lat" : 43.823623657226562,
"lon" : -70.327865600585938,
"name" : "station_3",
"north" : -186.06549181810368,
"speed" : 7.0,
"type" : "point"
},
{
"depth" : 4.0,
"east" : 131.33919016412341,
"lat" : 43.823623657226562,
"lon" : -70.328765869140625,
"name" : "station_4",
"north" : -186.06731482554795,
"speed" : 9.0,
"type" : "point"
}
],
"priority" : 10,
"repeat" : 1,
"sourceAddress" : "10.25.0.160",
"sourceName" : "CCU Neptus 0_163",
"taskId" : "1",
"taskName" : "east_waypt_survey"
},
"plan1_toMoos" :
{
"boardStamp" : 1699602762.2845099,
"clientStamp" : 1699602762.7520001,
"closedLoop" : false,
"duration" : -1.0,
"maxDepth" : -1.0,
"minDepth" : -1.0,
"origin" :
{
"altitude" : 0.0,
"lat" : 50.825299999999999,
"lon" : -90.330399999999997
},
"perpetual" : false,
"points" :
[
{
"depth" : 2.0,
"east" : 117.83291847226671,
"lat" : 43.825713999999998,
"lon" : -70.32893,
"name" : "Goto1",
"north" : 46.200319317940647,
"speed" : 2.0,
"type" : "point"
},
{
"depth" : 2.0,
"east" : -17.18366087421261,
"lat" : 43.826782000000001,
"lon" : -70.330609999999993,
"name" : "Goto2",
"north" : 164.87635389378988,
"speed" : 2.0,
"type" : "point"
},
{
"depth" : 2.0,
"east" : -241.19025325837993,
"lat" : 43.825465999999999,
"lon" : -70.333399999999997,
"name" : "Goto3",
"north" : 18.653618776002617,
"speed" : 2.0,
"type" : "point"
},
{
"depth" : 2.0,
"east" : -203.76118848802312,
"lat" : 43.823234999999997,
"lon" : -70.332930000000005,
"name" : "Goto4",
"north" : -229.29782627916489,
"speed" : 2.0,
"type" : "point"
}
],
"priority" : 10,
"repeat" : 1,
"sourceAddress" : "10.25.0.163",
"sourceName" : "CCU JHL 0_163",
"taskId" : "0,106,3,-96,8,-103,13,6,9,32,50,-13,47,-71,61,1",
"taskName" : "plan1_toMoos"
},
"west_waypt_survey" :
{
"boardStamp" : 1698135268.3958621,
"clientStamp" : 1698135268.2057669,
"closedLoop" : true,
"constSpeed" : -1.0,
"duration" : -1.0,
"maxDepth" : -1.0,
"minDepth" : -1.0,
"origin" :
{
"altitude" : 0.0,
"lat" : 43.825298309326172,
"lon" : -70.330398559570312
},
"perpetual" : true,
"points" :
[
{
"depth" : 9.0,
"east" : -91.445572043530944,
"lat" : 43.824195861816406,
"lon" : -70.331535339355469,
"name" : "station_1",
"north" : -122.49101460421512,
"speed" : 4.0
},
{
"depth" : 7.0,
"east" : 5.5235485468483718,
"lat" : 43.824298858642578,
"lon" : -70.330329895019531,
"name" : "station_2",
"north" : -111.04778559533926,
"speed" : 6.0
},
{
"depth" : 5.0,
"east" : 4.2961493948725868,
"lat" : 43.823516845703125,
"lon" : -70.330345153808594,
"name" : "station_3",
"north" : -197.93630920628678,
"speed" : 8.0
},
{
"depth" : 3.0,
"east" : -81.013520711457318,
"lat" : 43.823207855224609,
"lon" : -70.331405639648438,
"name" : "station_4",
"north" : -232.26737690334403,
"speed" : 10.0
}
],
"priority" : 10,
"repeat" : -1,
"sourceAddress" : "10.25.0.160",
"sourceName" : "CCU Neptus 0_163",
"taskId" : "2",
"taskName" : "west_waypt_survey"
}
}

1
src/CMakeLists.txt
View File

@@ -23,6 +23,7 @@ ADD_SUBDIRECTORY(pTaskManger)
ADD_SUBDIRECTORY(pTaskSend)
ADD_SUBDIRECTORY(pMotionControler)
ADD_SUBDIRECTORY(pEmulator)
ADD_SUBDIRECTORY(pFaultHandle)
##############################################################################
# END of CMakeLists.txt
##############################################################################

144
src/pBoardSupportComm/BoardSupportComm.cpp
View File

@@ -13,28 +13,14 @@
#define TCP_RECEIVE_PORT 8001
#define TCP_SERVER_ADDRESS "127.0.0.1"
// #define MOOS_AUV_SIM
#define MATLAB_AUV_SIM
#define MOOS_AUV_SIM
// #define MATLAB_AUV_SIM
// #ifdef TRUE_AUV
using namespace std;
BoardSupportComm::BoardSupportComm()
{
// uMotion_posX_fb = 0;
// uMotion_posY_fb = 0;
// uMotion_posZ_fb = 0;
// uMotion_roll_fb = 0;
// uMotion_pitch_fb = 0;
// uMotion_yaw_fb = 0;
// uMotion_longitude_fb = 0;
// uMotion_latitude_fb = 0;
// uMotion_velocityX_fb = 0;
// uMotion_velocityY_fb = 0;
// uMotion_velocityZ_fb = 0;
// uMotion_deep_fb = 0;
estimatedState.info = "lauv-150";
estimatedState.currentLon = 0;
estimatedState.currentLat = 0;
estimatedState.currentAltitude = 0;
@@ -108,12 +94,12 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
if(key == "NAV_X")
{
//E->N
estimatedState.offsetNorth = msg.GetDouble();
estimatedState.offsetEast = msg.GetDouble();
}
if(key == "NAV_Y")
{
//N->E
estimatedState.offsetEast = msg.GetDouble();
estimatedState.offsetNorth = msg.GetDouble();
}
if(key == "NAV_Z")
{
@@ -122,7 +108,18 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
}
if(key == "NAV_YAW")
{
estimatedState.yaw = -msg.GetDouble();
double yawTemp = msg.GetDouble();
if (std::abs(yawTemp) <= M_PI)
{
if (yawTemp <= 0)
{
estimatedState.yaw = -yawTemp * 180 / M_PI;
}
else
{
estimatedState.yaw = (2 * M_PI - yawTemp) * 180 / M_PI;
}
}
}
if(key == "NAV_PITCH")
{
@@ -224,10 +221,6 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{
return false;
}
// if (executeCommand.drive_mode != 0x02)
// {
// return false;
// }
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
@@ -286,12 +279,45 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
std::cerr << "Failed to parse JSON string." << std::endl;
return false;
}
estimatedState.referenceAltitude = recvCommand["altitude"].asFloat();
estimatedState.referenceAltitude = recvCommand["alt"].asFloat();
estimatedState.referenceLat = recvCommand["lat"].asFloat();
estimatedState.referenceLon = recvCommand["lon"].asFloat();
struct stat info;
if (stat(llaOriginPath.c_str(), &info) == 0)
{
std::ifstream ifs;
ifs.open(llaOriginPath, std::ios::in);
Json::Reader reader;
Json::Value originJsonValue;
reader.parse(ifs, originJsonValue);
ifs.close();
originJsonValue["LongOrigin"] = recvCommand["lon"].asFloat();
originJsonValue["LatOrigin"] = recvCommand["lat"].asFloat();
originJsonValue["AltOrigin"] = recvCommand["alt"].asFloat();
originJsonValue["TaskName"] = recvCommand["task"].asString();
Json::StreamWriterBuilder builder;
std::ofstream ofs;
ofs.open(llaOriginPath, std::ios::out);
ofs << Json::writeString(builder, originJsonValue) << std::endl;
ofs.close();
}
else
{
Json::Value originJsonValue;
originJsonValue["LongOrigin"] = recvCommand["lon"].asFloat();
originJsonValue["LatOrigin"] = recvCommand["lat"].asFloat();
originJsonValue["AltOrigin"] = recvCommand["alt"].asFloat();
originJsonValue["TaskName"] = recvCommand["task"].asString();
Json::StreamWriterBuilder builder;
std::ofstream ofs;
ofs.open(llaOriginPath, std::ios::out);
ofs << Json::writeString(builder, originJsonValue) << std::endl;
ofs.close();
}
}
}
return(true);
@@ -346,8 +372,8 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
embeddedState["insVX"] = estimatedState.linearVelocityNorth * 0.01;
embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01;
embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01;
embeddedState["currentLon"] = estimatedState.currentLon * 0.000001;
embeddedState["currentLat"] = estimatedState.currentLat * 0.000001;
embeddedState["currentLon"] = estimatedState.currentLon;
embeddedState["currentLat"] = estimatedState.currentLat;
embeddedState["currentAltitude"] = 0;
embeddedState["dvlVX"] = 0;
embeddedState["dvlVY"] = 0;
@@ -435,7 +461,9 @@ std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
ConvertLLAToNED(reference, current, ned);
embeddedState["north"] = ned.at(0);
embeddedState["east"]= ned.at(1);
embeddedState["referenceLon"]= estimatedState.referenceLon;
embeddedState["referenceLat"]= estimatedState.referenceLat;
embeddedState["referenceAltitude"]= estimatedState.referenceAltitude;
Json::StreamWriterBuilder builder;
std::string embeddedStateString = Json::writeString(builder, embeddedState);
@@ -447,19 +475,16 @@ void BoardSupportComm::tcpProcessThread()
{
while(1)
{
//bzero(tcpReceiveBuffer, 0);
//int lens = read(tcpSockFD, (unsigned char* )tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
bzero(tcpReceiveBuffer, 0);
int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
// std::cout << MOOS::Time() << " recv:" << lens << std::endl;
if(lens>0)
{
parseMessage((unsigned char* )tcpReceiveBuffer, lens);
// parseMessage(tcpReceiveBuffer, lens);
Json::Value embeddedState;
std::string embeddedStateString = convertEmbeddedFormat(embeddedState);
// std::cout << std::fixed << std::setprecision(6) << MOOS::Time() << embeddedStateString << std::endl;
Notify("uDevice_monitor_fb", embeddedStateString);
#ifndef MOOS_AUV_SIM
Notify("NAV_X", embeddedState["north"].asDouble());
Notify("NAV_Y", embeddedState["east"].asDouble());
Notify("NAV_Z", embeddedState["depth"].asDouble());
@@ -474,6 +499,7 @@ void BoardSupportComm::tcpProcessThread()
Notify("NAV_ROLL", embeddedState["roll"].asDouble() * 180 / M_PI);
Notify("NAV_PITCH", embeddedState["pitch"].asDouble() * 180 / M_PI);
Notify("NAV_YAW", embeddedState["yaw"].asDouble() * 180 / M_PI);
#endif
}
}
}
@@ -510,6 +536,7 @@ bool BoardSupportComm::Iterate()
return false;
}
}
// std::cout << "tcpSockFD: " << tcpSockFD << ", tcpConnectRet: " << tcpConnectRet << std::endl;
if ((tcpSockFD != -1) && (tcpConnectRet != -1))
{
std::thread t1(&BoardSupportComm::tcpProcessThread, this);
@@ -552,7 +579,7 @@ bool BoardSupportComm::Iterate()
printf("footer: %u\n", embeddedInfo.footer);
#endif
#if 0
#ifdef MOOS_AUV_SIM
executeCommand.header = 0xEBA2; //1:[0,1]
executeCommand.count = 16; //2:[2,3]
executeCommand.size = 21; //3:[4]
@@ -576,7 +603,6 @@ bool BoardSupportComm::Iterate()
try
{
write(tcpSockFD, tcpSendBuffer, executeCommand.size);
// sock_tcp_send.write(tcpSendBuffer, executeCommand.size);
}
catch(const std::exception& e)
{
@@ -590,18 +616,6 @@ bool BoardSupportComm::Iterate()
return true;
}
uint16_t BoardSupportComm::imcPollTCP(char* buffer, int bufferLength)
{
if (m_poll_1.poll(0))
{
bzero(buffer, 0);
uint16_t rv = sock_tcp_send.read(buffer, bufferLength);
return rv;
}
return 0;
}
template <typename Type>
inline uint16_t BoardSupportComm::serialize(const Type t, uint8_t* bfr, uint16_t& sumSize)
{
@@ -696,10 +710,44 @@ int BoardSupportComm::parseMessage(unsigned char* buffer, int size)
bool BoardSupportComm::OnStartUp()
{
// AppCastingMOOSApp::OnStartUp();
m_MissionReader.GetValue("llaOriginPath", llaOriginPath);
try
{
struct stat info;
if (stat(llaOriginPath.c_str(), &info) == 0)
{
std::ifstream ifs;
ifs.open(llaOriginPath, std::ios::in);
Json::Reader reader;
Json::Value originJsonValue;
reader.parse(ifs, originJsonValue);
ifs.close();
if (originJsonValue.isMember("LongOrigin") &&
originJsonValue.isMember("LatOrigin") &&
originJsonValue.isMember("AltOrigin"))
{
estimatedState.referenceLon = originJsonValue["LongOrigin"].asFloat();
estimatedState.referenceLat = originJsonValue["LatOrigin"].asFloat();
estimatedState.referenceAltitude = originJsonValue["AltOrigin"].asFloat();
}
else
{
throw -1;
}
}
else
{
throw -1;
}
}
catch(...)
{
m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon);
m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat);
m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude);
}
std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", "
<< estimatedState.referenceLat << ", "

19
src/pBoardSupportComm/BoardSupportComm.h
View File

@@ -77,7 +77,6 @@ struct AUVExecuteCommand
};
struct EstimatedState {
std::string info;
float referenceLon;
float referenceLat;
float referenceAltitude;
@@ -113,18 +112,6 @@ protected: // Standard MOOSApp functions to overload
// bool buildReport();
private:
// double uMotion_posX_fb;
// double uMotion_posY_fb;
// double uMotion_posZ_fb;
// double uMotion_roll_fb;
// double uMotion_pitch_fb;
// double uMotion_yaw_fb;
// double uMotion_longitude_fb;
// double uMotion_latitude_fb;
// double uMotion_velocityX_fb;
// double uMotion_velocityY_fb;
// double uMotion_velocityZ_fb;
// double uMotion_deep_fb;
struct EstimatedState estimatedState;
@@ -146,12 +133,8 @@ private:
std::string convertEmbeddedFormat(Json::Value &embeddedState);
int tcpSockFD;
int tcpConnectRet;
// unsigned char tcpReceiveBuffer[65535];
char tcpReceiveBuffer[65535];
unsigned char tcpSendBuffer[65535];
DUNE::IO::Poll m_poll_1;
DUNE::Network::TCPSocket sock_tcp_send;
uint16_t imcPollTCP(char* buffer, int bufferLength);
int parseMessage(unsigned char* buffer, int size);
int serializeMessage(unsigned char* buffer);
@@ -165,6 +148,8 @@ private:
bool testCount = false;
int sockfd = -1;
int connectFlg = -1;
std::string llaOriginPath;
};

319
src/pClientViewer/ClientViewer.cpp
View File

@@ -205,6 +205,54 @@ bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail)
msg.command = 1;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "GetLogBook") //PlanControl
{
DUNE::IMC::LogBookControl msg;
msg.setTimeStamp();
msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET;
DUNE::IMC::LogBookEntry msgLogBookEntry;
msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
msgLogBookEntry.context = "LIST";
msg.msg.push_back(msgLogBookEntry);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "RetrLogBook") //PlanControl
{
DUNE::IMC::LogBookControl msg;
msg.setTimeStamp();
msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET;
DUNE::IMC::LogBookEntry msgLogBookEntry;
msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
msgLogBookEntry.context = "RETR";
Json::Value retrFile;
retrFile["file"].append("lauv-150/2023-11-16/103058/auvData.txt");
retrFile["file"].append("lauv-150/2023-11-16/103058/clientCommand.txt");
retrFile["file"].append("lauv-150/2023-11-16/105131/missionHistory.txt");
Json::StreamWriterBuilder builder;
msgLogBookEntry.text = Json::writeString(builder, retrFile);
msg.msg.push_back(msgLogBookEntry);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "DeleLogBook") //PlanControl
{
DUNE::IMC::LogBookControl msg;
msg.setTimeStamp();
msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_CLEAR;
DUNE::IMC::LogBookEntry msgLogBookEntry;
msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
msgLogBookEntry.context = "DELE";
Json::Value retrFile;
retrFile["file"].append("lauv-150/2023-11-16/103058/auvData.txt");
// retrFile["file"].append("lauv-150/2023-11-17/195131/missionHistory.txt");
retrFile["file"].append("lauv-150/2023-11-16/103058/clientCommand.txt");
retrFile["file"].append("lauv-150/2023-11-16/105131/missionHistory.txt");
Json::StreamWriterBuilder builder;
msgLogBookEntry.text = Json::writeString(builder, retrFile);
msg.msg.push_back(msgLogBookEntry);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
}
}
@@ -222,7 +270,6 @@ bool ClientViewer::OnConnectToServer()
bool ClientViewer::Iterate()
{
#if 1
DUNE::IMC::Message * receiveUDPMessage;
while ((receiveUDPMessage = imcPollUDP()) != NULL)
@@ -230,90 +277,91 @@ bool ClientViewer::Iterate()
processMessage(receiveUDPMessage);
free(receiveUDPMessage);
}
#endif
# if 1
DUNE::IMC::Message * receiveTCPMessage;
while ((receiveTCPMessage = imcPollTCP()) != NULL)
{
processMessage(receiveTCPMessage);
free(receiveTCPMessage);
}
#endif
return(true);
}
bool processMessageCallback(DUNE::IMC::Message * message)
{
int type = message->getId();
#if 1
if (type == DUNE::IMC::Announce::getIdStatic())
{
DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
// printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
printf("server receive %s: %lf, %lf, %lf, %lf\n", \
msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
}
#endif
if (type == DUNE::IMC::PlanDB::getIdStatic())
{
DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
printf("%s\n", msg->info.c_str());
}
#if 1
if (type == DUNE::IMC::PlanControlState::getIdStatic())
{
DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
// bool processMessageCallback(DUNE::IMC::Message * message)
// {
// int type = message->getId();
// if (type == DUNE::IMC::Announce::getIdStatic())
// {
// DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
// // printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
// printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
// }
// if (type == DUNE::IMC::PlanDB::getIdStatic())
// {
// DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
// printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
// printf("%s\n", msg->info.c_str());
// }
// if (type == DUNE::IMC::PlanControlState::getIdStatic())
// {
// DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
printf("server receive %s: %lf, %s, %s, %u\n", \
msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
}
if (type == DUNE::IMC::MsgList::getIdStatic())
{
DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
for (; iter1 != msgList->msgs.end(); ++iter1)
{
DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
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;
}
}
}
if (type == DUNE::IMC::EstimatedState::getIdStatic())
{
DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
// printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// msg->getName(), msg->getTimeStamp(),
// msg->lat, msg->lon, msg->depth,
// msg->phi, msg->theta, msg->psi);
printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
msg->getName(), msg->getTimeStamp(),
msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
}
if (type == DUNE::IMC::VehicleState::getIdStatic())
{
DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
printf("server receive %s: %lf, %u\n",
msg->getName(), msg->getTimeStamp(), msg->op_mode);
}
if (type == DUNE::IMC::PlanControl::getIdStatic())
{
DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message);
printf("server receive %s: %lf, %u,\n",
msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str());
}
#endif
return true;
}
// printf("server receive %s: %lf, %s, %s, %u\n", \
// msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
// }
// if (type == DUNE::IMC::MsgList::getIdStatic())
// {
// DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
// printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
// DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
// for (; iter1 != msgList->msgs.end(); ++iter1)
// {
// DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
// DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
// 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;
// }
// }
// }
// if (type == DUNE::IMC::EstimatedState::getIdStatic())
// {
// DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
// // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// // msg->getName(), msg->getTimeStamp(),
// // msg->lat, msg->lon, msg->depth,
// // msg->phi, msg->theta, msg->psi);
// printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// msg->getName(), msg->getTimeStamp(),
// msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
// msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
// }
// if (type == DUNE::IMC::VehicleState::getIdStatic())
// {
// DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
// printf("server receive %s: %lf, %u\n",
// msg->getName(), msg->getTimeStamp(), msg->op_mode);
// }
// if (type == DUNE::IMC::LogBookControl::getIdStatic())
// {
// DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
// printf("server receive %s: %lf, %u\n",
// msg->getName(), msg->getTimeStamp(), msg->command);
// DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
// for (; iter != msg->msg.end(); ++iter)
// {
// DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast<DUNE::IMC::LogBookEntry *>(*iter);
// std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->text << std::endl;
// }
// }
// return true;
// }
//---------------------------------------------------------
// Procedure: OnStartUp()
@@ -333,12 +381,9 @@ bool ClientViewer::OnStartUp()
// tcpCommEvent.SetCallback(processMessageCallback,NULL);
// tcpCommEvent.Start();
#if 1
sock_udp_receive.bind(UDP_RECEIVE_PORT, DUNE::Network::Address::Any, false);
m_poll_0.add(sock_udp_receive);
#endif
#if 1
try
{
// sock_tcp_send.setSendTimeout(5);
@@ -351,16 +396,11 @@ bool ClientViewer::OnStartUp()
{
std::cerr << e.what() << '\n';
}
#endif
#if 1
char local_ip2[INET_ADDRSTRLEN] = {0};
get_local_ip_using_create_socket(local_ip2);
ethernetIP = local_ip2;
#endif
RegisterVariables();
return(true);
@@ -668,70 +708,73 @@ bool ClientViewer::tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, i
void ClientViewer::processMessage(DUNE::IMC::Message * message) {
int type = message->getId();
if (type == DUNE::IMC::Announce::getIdStatic())
{
DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
// if (type == DUNE::IMC::Announce::getIdStatic())
// {
// DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
// // printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
// printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
printf("server receive %s: %lf, %lf, %lf, %lf\n", \
msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
}
else if (type == DUNE::IMC::PlanDB::getIdStatic())
{
DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
printf("%s\n", msg->info.c_str());
}
else if (type == DUNE::IMC::PlanControlState::getIdStatic())
{
DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
// msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
// }
// if (type == DUNE::IMC::PlanDB::getIdStatic())
// {
// DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
// printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
// printf("%s\n", msg->info.c_str());
// }
// if (type == DUNE::IMC::PlanControlState::getIdStatic())
// {
// DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
printf("server receive %s: %lf, %s, %s, %u\n", \
msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
}
else if (type == DUNE::IMC::MsgList::getIdStatic())
{
DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
for (; iter1 != msgList->msgs.end(); ++iter1)
{
DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
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;
}
}
}
else if (type == DUNE::IMC::EstimatedState::getIdStatic())
{
DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
// printf("server receive %s: %lf, %s, %s, %u\n", \
// msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
// }
// if (type == DUNE::IMC::MsgList::getIdStatic())
// {
// DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
// printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
// DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
// for (; iter1 != msgList->msgs.end(); ++iter1)
// {
// DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
// DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
// 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;
// }
// }
// }
// if (type == DUNE::IMC::EstimatedState::getIdStatic())
// {
// DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
// // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// // msg->getName(), msg->getTimeStamp(),
// // msg->lat, msg->lon, msg->depth,
// // msg->phi, msg->theta, msg->psi);
// printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// msg->getName(), msg->getTimeStamp(),
// msg->lat, msg->lon, msg->depth,
// msg->phi, msg->theta, msg->psi);
printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
msg->getName(), msg->getTimeStamp(),
msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
}
else if (type == DUNE::IMC::VehicleState::getIdStatic())
// msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
// msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
// }
// if (type == DUNE::IMC::VehicleState::getIdStatic())
// {
// DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
// printf("server receive %s: %lf, %u\n",
// msg->getName(), msg->getTimeStamp(), msg->op_mode);
// }
if (type == DUNE::IMC::LogBookControl::getIdStatic())
{
DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
printf("server receive %s: %lf, %u\n",
msg->getName(), msg->getTimeStamp(), msg->op_mode);
}
else if (type == DUNE::IMC::PlanControl::getIdStatic())
{
DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message);
printf("server receive %s: %lf, %u,\n",
msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str());
}
else
{
msg->getName(), msg->getTimeStamp(), msg->command);
DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
for (; iter != msg->msg.end(); ++iter)
{
DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast<DUNE::IMC::LogBookEntry *>(*iter);
std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->context << ", " << msgLogBookEntry->text << std::endl;
}
}
}

407
src/pDataManagement/DataManagement.cpp
View File

@@ -6,9 +6,9 @@
/************************************************************/
#include <iterator>
#include <sys/stat.h>
#include "MBUtils.h"
#include "DataManagement.h"
//#include "NavigationInfo.pb.h"
#include <json/json.h>
using namespace std;
@@ -17,18 +17,50 @@ using namespace std;
// Constructor
DataManagement::DataManagement()
{
nDoublePrecision = 5;
//AUV状态信息:0~99
logVarList["uMotion_pose_log"] = 0;
//任务历史信息:100~199
logVarList["uMission_task_log"] = 100;
//客户端操作记录:200~299
logVarList["uClient_plandbSet_log"] = 200;
logVarList["uClient_plandbGet_log"] = 201;
logVarList["uClient_parameterSet_log"] = 202;
logVarList["uClient_planControl_log"] = 203;
logVarList["uClient_manualEnable_log"] = 204;
logVarList["uClient_manualDrive_log"] = 205;
//错误日志:300~399
}
//---------------------------------------------------------
// Destructor
DataManagement::~DataManagement()
{
if(auvDataStream.is_open())
{
auvDataStream.close();
}
if(missionHistoryStream.is_open())
{
missionHistoryStream.close();
}
if(clientCommandStream.is_open())
{
clientCommandStream.close();
}
if(faultLogStream.is_open())
{
faultLogStream.close();
}
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
{
DoAsyncLog(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
@@ -44,91 +76,126 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
bool mdbl = msg.IsDouble();
bool mstr = msg.IsString();
std::string estimatedStateString;
if(key == "uMotion_pose_fb")
if(key == "uDevice_monitor_fb")
{
estimatedStateString = msg.GetString();
std::string err;
Json::Value estimatedStateData;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
const std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
if (!reader->parse(estimatedStateString.c_str(),
estimatedStateString.c_str() + static_cast<int>(estimatedStateString.length()),
&estimatedStateData,
&err))
bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err);
if (!parsingResult)
{
std::cout << "error" << std::endl;
return EXIT_FAILURE;
std::cerr << "Failed to parse JSON string." << std::endl;
return false;
}
std::string info = estimatedStateData["info"].asString();
float currentLon = estimatedStateData["currentLon"].asFloat();
float currentLat = estimatedStateData["currentLat"].asFloat();
float currentAltitude = estimatedStateData["currentAltitude"].asFloat();
float referenceLon = estimatedStateData["referenceLon"].asFloat();
float referenceLat = estimatedStateData["referenceLat"].asFloat();
float referenceAltitude = estimatedStateData["referenceAltitude"].asFloat();
float offsetNorth = estimatedStateData["offsetNorth"].asFloat();
float offsetEast = estimatedStateData["offsetEast"].asFloat();
float offsetDown = estimatedStateData["offsetDown"].asFloat();
float roll = estimatedStateData["roll"].asFloat();
float pitch = estimatedStateData["pitch"].asFloat();
float yaw = estimatedStateData["yaw"].asFloat();
float linearVelocityNorth = estimatedStateData["linearVelocityNorth"].asFloat();
float linearVelocityEast = estimatedStateData["linearVelocityEast"].asFloat();
float linearVelocityDown = estimatedStateData["linearVelocityDown"].asFloat();
float angularVelocityNorth = estimatedStateData["angularVelocityNorth"].asFloat();
float angularVelocityEast = estimatedStateData["angularVelocityEast"].asFloat();
float angularVelocityDown = estimatedStateData["angularVelocityDown"].asFloat();
float height = estimatedStateData["height"].asFloat();
float depth = estimatedStateData["depth"].asFloat();
std::cout << "info: " << info << std::endl;
std::cout << "referenceLon: " << referenceLon << std::endl;
std::cout << "referenceLat: " << referenceLat << std::endl;
std::cout << "referenceAltitude: " << referenceAltitude << std::endl;
std::cout << "currentLon: " << currentLon << std::endl;
std::cout << "currentLat: " << currentLat << std::endl;
std::cout << "altitude: " << currentAltitude << std::endl;
std::cout << "offsetNorth: " << offsetNorth << std::endl;
std::cout << "offsetEast: " << offsetEast << std::endl;
std::cout << "offsetDown: " << offsetDown << std::endl;
std::cout << "roll: " << roll << std::endl;
std::cout << "pitch: " << pitch << std::endl;
std::cout << "yaw: " << yaw << std::endl;
std::cout << "linearVelocityNorth: " << linearVelocityNorth << std::endl;
std::cout << "linearVelocityEast: " << linearVelocityEast << std::endl;
std::cout << "linearVelocityDown: " << linearVelocityDown << std::endl;
std::cout << "angularVelocityNorth: " << angularVelocityNorth << std::endl;
std::cout << "angularVelocityEast: " << angularVelocityEast << std::endl;
std::cout << "angularVelocityDown: " << angularVelocityDown << std::endl;
std::cout << "height: " << height << std::endl;
std::cout << "depth: " << depth << std::endl;
std::stringstream ss;
ss << std::fixed << std::setprecision(6) << getTimeStamp() << ","
<< info << ","
<< referenceLon << ","
<< referenceLat << ","
<< referenceAltitude << ","
<< currentLon << ","
<< currentLat << ","
<< currentAltitude << ","
<< offsetNorth << ","
<< offsetEast << ","
<< offsetDown << ","
<< roll << ","
<< pitch << ","
<< yaw << ","
<< height << ","
<< depth << std::endl;
ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","
<< estimatedStateData["driveMode"].asUInt() << ","
<< estimatedStateData["referenceLon"].asFloat() << ","
<< estimatedStateData["referenceLat"].asFloat() << ","
<< estimatedStateData["referenceAltitude"].asFloat() << ","
<< estimatedStateData["currentLon"].asFloat() << ","
<< estimatedStateData["currentLat"].asFloat() << ","
<< estimatedStateData["currentAltitude"].asFloat() << ","
<< estimatedStateData["north"].asFloat() << ","
<< estimatedStateData["east"].asFloat() << ","
<< estimatedStateData["depth"].asFloat() << ","
<< estimatedStateData["roll"].asFloat() << ","
<< estimatedStateData["pitch"].asFloat() << ","
<< estimatedStateData["yaw"].asFloat() << ","
<< estimatedStateData["insVX"].asFloat() << ","
<< estimatedStateData["insVY"].asFloat() << ","
<< estimatedStateData["insVZ"].asFloat() << ","
<< estimatedStateData["dvlVX"].asFloat() << ","
<< estimatedStateData["dvlVY"].asFloat() << ","
<< estimatedStateData["dvlVZ"].asFloat() << ","
<< estimatedStateData["height"].asFloat() << ","
<< estimatedStateData["rpm"].asInt() << ","
<< estimatedStateData["lightEnable"].asUInt() << ","
<< estimatedStateData["throwingLoadEnable"].asUInt() << ","
<< estimatedStateData["dvlStatus"].asUInt() << ","
<< estimatedStateData["iridium"].asUInt() << ","
<< estimatedStateData["batteryVoltage"].asUInt() << ","
<< estimatedStateData["batteryLevel"].asUInt() << ","
<< estimatedStateData["batteryTemp"].asFloat();
Notify("uMotion_pose_log", ss.str());
}
if(key == "uMission_task_fb")
{
std::string err;
Json::Value missionStatusObject;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err);
std::stringstream ss;
ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","
<< missionStatusObject["state"].asInt() << ","
<< missionStatusObject["taskName"].asString() << ","
<< missionStatusObject["destName"].asString() << ","
<< missionStatusObject["errorCode"].asUInt();
Notify("uMission_task_log", ss.str());
}
}
return(true);
}
bool DataManagement::DoAsyncLog(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator q;
std::stringstream sStream;
int saveFileIndex = -1;
for(q = NewMail.begin();q!=NewMail.end();q++)
{
CMOOSMsg & rMsg = *q;
if(logVarList.find(rMsg.m_sKey)!=logVarList.end())
{
//ok we have a name at column nVar...(numerical order retained in vector)
std::map<std::string, int>::iterator q = logVarList.find(rMsg.m_sKey);
saveFileIndex = q->second;
std::stringstream sEntry;
sEntry.setf(ios::left);
sEntry.setf(ios::fixed);
if(rMsg.IsDataType(MOOS_STRING) || rMsg.IsDataType(MOOS_DOUBLE))
{
sEntry << rMsg.GetAsString(12,nDoublePrecision) << ' ';
}
if ((saveFileIndex >= 0) && (saveFileIndex < 100))
{
if(auvDataStream.is_open())
{
auvDataStream << sEntry.str() << std::endl;
}
}
if ((saveFileIndex >= 100) && (saveFileIndex < 200))
{
if(missionHistoryStream.is_open())
{
missionHistoryStream << sEntry.str() << std::endl;
}
}
if ((saveFileIndex >= 200) && (saveFileIndex < 300))
{
if(clientCommandStream.is_open())
{
clientCommandStream << sEntry.str() << std::endl;
}
}
if ((saveFileIndex >= 300) && (saveFileIndex < 400))
{
if(faultLogStream.is_open())
{
faultLogStream << sEntry.str() << std::endl;
}
}
}
}
return true;
}
//---------------------------------------------------------
// Procedure: OnConnectToServer
@@ -144,6 +211,10 @@ bool DataManagement::OnConnectToServer()
bool DataManagement::Iterate()
{
auvDataStream.flush();
missionHistoryStream.flush();
clientCommandStream.flush();
faultLogStream.flush();
return(true);
}
@@ -170,6 +241,67 @@ bool DataManagement::OnStartUp()
}
}
}
std::string saveLogDir;
m_MissionReader.GetValue("LogDir", saveLogDir);
std::string vehicleName;
m_MissionReader.GetValue("VehicleName", vehicleName);
bool logEnable = false;
m_MissionReader.GetValue("LogEnable", logEnable);
std::string auvDataFile;
m_MissionReader.GetValue("AuvDataLog", auvDataFile);
std::string missionHistoryFile;
m_MissionReader.GetValue("MissionHistoryLog", missionHistoryFile);
std::string clientCommandFile;
m_MissionReader.GetValue("ClientCommandLog", clientCommandFile);
std::string faultLogFile;
m_MissionReader.GetValue("FaultLog", faultLogFile);
if (logEnable)
{
if (access(saveLogDir.c_str(), F_OK) == -1 )
{
mode_t mode = 0775;
mkdir(saveLogDir.c_str(), mode);
}
saveLogDir += "/" + vehicleName;
if (access(saveLogDir.c_str(), F_OK) == -1 )
{
mode_t mode = 0775;
mkdir(saveLogDir.c_str(), mode);
}
std::string subDir;
std::string localTime;
GenerateFileName(subDir, localTime);
saveLogDir += "/" + subDir;
if (access(saveLogDir.c_str(), F_OK) == -1 )
{
mode_t mode = 0775;
mkdir(saveLogDir.c_str(), mode);
}
saveLogDir += "/" + localTime;
if (access(saveLogDir.c_str(), F_OK) == -1 )
{
mode_t mode = 0775;
mkdir(saveLogDir.c_str(), mode);
}
std::string auvDataSavePath = saveLogDir + "/" + auvDataFile;
std::string missionHistorySavePath = saveLogDir + "/" + missionHistoryFile;
std::string clientCommandSavePath = saveLogDir + "/" + clientCommandFile;
std::string faultLogSavePath = saveLogDir + "/" + faultLogFile;
if(!OpenFile(auvDataStream, auvDataSavePath))
return MOOSFail("Failed to Open auvData file");
if(!OpenFile(missionHistoryStream, missionHistorySavePath))
return MOOSFail("Failed to Open missionHistory file");
if(!OpenFile(clientCommandStream, clientCommandSavePath))
return MOOSFail("Failed to Open clientCommand file");
if(!OpenFile(faultLogStream, faultLogSavePath))
return MOOSFail("Failed to Open faultLog file");
DoLogBanner(auvDataStream);
}
RegisterVariables();
return(true);
@@ -180,15 +312,130 @@ bool DataManagement::OnStartUp()
void DataManagement::RegisterVariables()
{
// Register("FOOBAR", 0);
Register("uMotion_pose_fb", 0);
//Register("uMotion_pose_log", 0);
Register("uDevice_monitor_fb", 0);
Register("uMission_task_fb", 0);
Register("uMission_task_log", 0);
Register("uMotion_pose_log", 0);
Register("uClient_plandbSet_log", 0);
Register("uClient_plandbGet_log", 0);
Register("uClient_parameterSet_log", 0);
Register("uClient_planControl_log", 0);
Register("uClient_manualEnable_log", 0);
Register("uClient_manualDrive_log", 0);
}
double DataManagement::getTimeStamp()
// double DataManagement::getTimeStamp()
// {
// struct timeval tv;
// gettimeofday(&tv,NULL);
// double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000;
// return stamp;
// }
bool DataManagement::DoLogBanner(std::ofstream &os)
{
struct timeval tv;
gettimeofday(&tv,NULL);
double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000;
return stamp;
os << "stamp" << ",";
os << "mode" << ",";
os << "refLon" << ",";
os << "refLat" << ",";
os << "refAlt" << ",";
os << "curLon" << ",";
os << "curLat" << ",";
os << "curAlt" << ",";
os << "north" << ",";
os << "east" << ",";
os << "depth" << ",";
os << "roll" << ",";
os << "pitch" << ",";
os << "yaw" << ",";
os << "insVX" << ",";
os << "insVY" << ",";
os << "insVZ" << ",";
os << "dvlVX" << ",";
os << "dvlVY" << ",";
os << "dvlVZ" << ",";
os << "height" << ",";
os << "thrust" << ",";
os << "light" << ",";
os << "load" << ",";
os << "dvl" << ",";
os << "iridium" << ",";
os << "batteryVol" << ",";
os << "batteryLev" << ",";
os << "batteryTemp" << std::endl;
return true;
}
bool DataManagement::OpenFile(std::ofstream & of,const std::string & sName)
{
of.open(sName.c_str());
if(!of.is_open())
{
return false;
}
return true;
}
void DataManagement::GenerateFileName(std::string &fileDir, std::string &fileName)
{
time_t now = time(0);
tm *gmtm = localtime(&now);
int year = 1900 + gmtm->tm_year;
int monthTemp = gmtm->tm_mon+1;
std::string month;
if (monthTemp < 10)
{
month = "0" + std::to_string(monthTemp);
}
else
{
month = std::to_string(monthTemp);
}
int dayTemp = gmtm->tm_mday;
std::string day;
if (dayTemp < 10)
{
day = "0" + std::to_string(dayTemp);
}
else
{
day = std::to_string(dayTemp);
}
int hourTemp = gmtm->tm_hour;
std::string hour;
if (hourTemp < 10)
{
hour = "0" + std::to_string(hourTemp);
}
else
{
hour = std::to_string(hourTemp);
}
int minuteTemp = gmtm->tm_min;
std::string minute;
if (minuteTemp < 10)
{
minute = "0" + std::to_string(minuteTemp);
}
else
{
minute = std::to_string(minuteTemp);
}
int secondTemp = gmtm->tm_sec;
std::string second;
if (secondTemp < 10)
{
second = "0" + std::to_string(secondTemp);
}
else
{
second = std::to_string(secondTemp);
}
std::stringstream ss;
ss << year << "-" << month << "-" << day;
fileDir = ss.str();
ss.str("");
ss << hour << minute << second;
fileName = ss.str();
}

14
src/pDataManagement/DataManagement.h
View File

@@ -21,12 +21,22 @@ class DataManagement : public CMOOSApp
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
bool DoAsyncLog(MOOSMSG_LIST & NewMail);
protected:
void RegisterVariables();
// double getTimeStamp();
bool OpenFile(std::ofstream & of,const std::string & sName);
bool DoLogBanner(std::ofstream &os);
void GenerateFileName(std::string &fileDir, std::string &fileName);
int nDoublePrecision;
std::ofstream auvDataStream;
std::ofstream missionHistoryStream;
std::ofstream clientCommandStream;
std::ofstream faultLogStream;
std::map<std::string, int> logVarList;
private:
double getTimeStamp();
};
#endif

2
src/pEmulator/Emulator.cpp
View File

@@ -2,7 +2,7 @@
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-08 17:33:41
* @LastEditTime: 2023-11-10 08:42:57
* @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp
* @Description:
*

2
src/pEmulator/Emulator.hpp
View File

@@ -2,7 +2,7 @@
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 15:57:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-08 17:24:35
* @LastEditTime: 2023-11-10 08:34:14
* @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.hpp
* @Description:
*

1
src/pEmulator/_150server.hpp
View File

@@ -91,5 +91,6 @@ public:
AUVCmd _150cmd;
unsigned char embeddedBuffer[59];
unsigned char recvbuf[65535] = {0};
unsigned int _150_recive = 0;
};
#endif

12
src/pFaultHandle/.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,12 @@
{
"files.associations": {
"array": "cpp",
"deque": "cpp",
"list": "cpp",
"string": "cpp",
"unordered_map": "cpp",
"vector": "cpp",
"string_view": "cpp",
"initializer_list": "cpp"
}
}

32
src/pFaultHandle/CMakeLists.txt Normal file
View File

@@ -0,0 +1,32 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pFaultHandle
# Author(s): wade
#--------------------------------------------------------
SET(SRC
FaultHandle.cpp
FaultHandle_Info.cpp
main.cpp
FaultHandle.h
)
# FIND_LIBRARY(/usr/local/lib)
# FIND_PATH(/usr/local/include)
include_directories(/usr/local/include/jsoncpp/)
link_directories(/usr/local/lib/)
ADD_EXECUTABLE(pFaultHandle ${SRC})
TARGET_LINK_LIBRARIES(pFaultHandle
${MOOS_LIBRARIES}
${CMAKE_DL_LIBS}
${SYSTEM_LIBS}
mbutil
pthread
jsoncpp
)

1366
src/pFaultHandle/FaultHandle.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

170
src/pFaultHandle/FaultHandle.h Normal file
View File

@@ -0,0 +1,170 @@
/************************************************************/
/* NAME: wade */
/* ORGN: MIT */
/* FILE: FaultHandle.h */
/* DATE: */
/************************************************************/
#ifndef FaultHandle_HEADER
#define FaultHandle_HEADER
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include "MOOS/libMOOS/MOOSLib.h"
//#include "moos.h"
#include <json/json.h>
#include <string>
#include <vector>
#include <map>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <sstream>
//class FaultHandle : public CMOOSApp
class FaultHandle : public AppCastingMOOSApp
{
public:
FaultHandle();
~FaultHandle();
protected: // Standard MOOSApp functions to overload
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
bool buildReport();
void RegisterVariables();
protected:
void handleMailNewDBClients();
void handleMailStatusUpdate(std::string);
bool handleConfigWatchList(std::string);
bool handleConfigWatchItem(std::string);
bool handleConfigExcludeList(std::string);
bool handleConfigExcludeItem(std::string);
protected:
bool addToWatchList(std::string);
void checkForIndividualUpdates();
void postFullSummary();
bool isAlive(std::string);
void procNotedHere(std::string);
void procNotedGone(std::string);
void procNotedExcused(std::string);
void handlePostMapping(std::string);
std::string postVar(std::string);
void populateAntlerList();
bool processIncluded(const std::string& procname);
bool processExcluded(const std::string& procname);
protected:
std::string convertSecondsToString(double);
void errorMsgInit();
void MoosAppStatusMonitor();
void MoosAppOverloadMonitor();
void MissionFaultMonitor();
void MotionControlMonitor();
void DeviceLeakageMonitor();
void BatteryStatusMonitor();
void EmergencyBatteryMonitor();
void ThrustStatusMonitor();
void CPUTempMonitor();
bool MoosAppMissing();
bool MoosAppOverload();
protected: // State Variables
bool m_proc_watch_summary_changed;
double m_last_posting_time;
// Clients and Summary stored across iterations
std::string m_db_clients;
std::string m_proc_watch_summary;
std::vector<std::string> m_watch_list;
std::vector<std::string> m_excused_list;
// Mapping from proc name to data
std::map<std::string, bool> m_map_alive;
std::map<std::string, bool> m_map_alive_prev;
std::map<std::string, unsigned int> m_map_noted_gone;
std::map<std::string, unsigned int> m_map_noted_here;
std::map<std::string, double> m_map_now_cpuload;
std::map<std::string, double> m_map_max_cpuload;
std::set<std::string> m_set_db_clients;
std::set<std::string> m_set_watch_clients;
std::set<std::string> m_set_antler_clients;
protected: // Configurations Variables
bool m_watch_all_db;
bool m_watch_all_antler;
double m_allow_retractions;
double m_min_wait;
double m_noted_gone_wait;
// Include List
std::vector<std::string> m_include_list;
std::vector<bool> m_include_list_prefix;
// Exclude List
std::vector<std::string> m_exclude_list;
std::vector<bool> m_exclude_list_prefix;
// A dedicated MOOS var for posting when a proc chgs status
std::map<std::string, std::string> m_map_proc_post;
// A map for changing the MOOS variable posted.
std::map<std::string, std::string> m_map_chgpost;
protected:
int m_Mission_fault_fb;
int m_Motion_fault_fb;
double m_CPU_temperature;
uint32_t m_faultBattery;
uint32_t m_faultEmergencyBattery;
uint32_t m_faultLeakSensor;
uint32_t m_faultThrust;
uint32_t m_batteryLevel;
Json::Value Device_monitor;
Json::Value MoosAppStatus;
Json::Value uFH_ErrorMsg_fb;
Json::Value uFH_errorMsg1_fb;
Json::Value uFH_errorMsg2_fb;
Json::Value uFH_errorMsg3_fb;
protected:
//Contingency variable
// bool uSensor_leakage_fb;
// bool uMission_deepOut_fb;
// bool uMission_timeOut_fb;
// bool uDrive_batteryLow_fb;
// double uSoftWare_appMiss_fb; //AppID
//------------------------------------------------
//subscribe variables
private: // Configuration variables
private: // State variables
};
#endif

115
src/pFaultHandle/FaultHandle_Info.cpp Normal file
View File

@@ -0,0 +1,115 @@
/****************************************************************/
/* NAME: wade */
/* ORGN: MIT Cambridge MA */
/* FILE: FaultHandle_Info.cpp */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#include <cstdlib>
#include <iostream>
#include "FaultHandle_Info.h"
#include "ColorParse.h"
#include "ReleaseInfo.h"
using namespace std;
//----------------------------------------------------------------
// Procedure: showSynopsis
void showSynopsis()
{
blk("SYNOPSIS: ");
blk("------------------------------------ ");
blk(" The pFaultHandle application is used for ");
blk(" ");
blk(" ");
blk(" ");
blk(" ");
}
//----------------------------------------------------------------
// Procedure: showHelpAndExit
void showHelpAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("Usage: pFaultHandle file.moos [OPTIONS] ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("Options: ");
mag(" --alias","=<ProcessName> ");
blk(" Launch pFaultHandle with the given process name ");
blk(" rather than pFaultHandle. ");
mag(" --example, -e ");
blk(" Display example MOOS configuration block. ");
mag(" --help, -h ");
blk(" Display this help message. ");
mag(" --interface, -i ");
blk(" Display MOOS publications and subscriptions. ");
mag(" --version,-v ");
blk(" Display the release version of pFaultHandle. ");
blk(" ");
blk("Note: If argv[2] does not otherwise match a known option, ");
blk(" then it will be interpreted as a run alias. This is ");
blk(" to support pAntler launching conventions. ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showExampleConfigAndExit
void showExampleConfigAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pFaultHandle Example MOOS Configuration ");
blu("=============================================================== ");
blk(" ");
blk("ProcessConfig = pFaultHandle ");
blk("{ ");
blk(" AppTick = 4 ");
blk(" CommsTick = 4 ");
blk(" ");
blk("} ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showInterfaceAndExit
void showInterfaceAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pFaultHandle INTERFACE ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("SUBSCRIPTIONS: ");
blk("------------------------------------ ");
blk(" NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO, ");
blk(" string_val=BAR ");
blk(" ");
blk("PUBLICATIONS: ");
blk("------------------------------------ ");
blk(" Publications are determined by the node message content. ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showReleaseInfoAndExit
void showReleaseInfoAndExit()
{
showReleaseInfo("pFaultHandle", "gpl");
exit(0);
}

18
src/pFaultHandle/FaultHandle_Info.h Normal file
View File

@@ -0,0 +1,18 @@
/****************************************************************/
/* NAME: wade */
/* ORGN: MIT Cambridge MA */
/* FILE: FaultHandle_Info.h */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#ifndef FaultHandle_INFO_HEADER
#define FaultHandle_INFO_HEADER
void showSynopsis();
void showHelpAndExit();
void showExampleConfigAndExit();
void showInterfaceAndExit();
void showReleaseInfoAndExit();
#endif

52
src/pFaultHandle/main.cpp Normal file
View File

@@ -0,0 +1,52 @@
/************************************************************/
/* NAME: wade */
/* ORGN: MIT */
/* FILE: main.cpp */
/* DATE: */
/************************************************************/
#include <string>
#include "MBUtils.h"
#include "ColorParse.h"
#include "FaultHandle.h"
#include "FaultHandle_Info.h"
using namespace std;
int main(int argc, char *argv[])
{
string mission_file;
string run_command = argv[0];
for(int i=1; i<argc; i++) {
string argi = argv[i];
if((argi=="-v") || (argi=="--version") || (argi=="-version"))
showReleaseInfoAndExit();
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
showExampleConfigAndExit();
else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
showHelpAndExit();
else if((argi == "-i") || (argi == "--interface"))
showInterfaceAndExit();
else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
mission_file = argv[i];
else if(strBegins(argi, "--alias="))
run_command = argi.substr(8);
else if(i==2)
run_command = argi;
}
if(mission_file == "")
showHelpAndExit();
cout << termColor("green");
cout << "pFaultHandle launching as " << run_command << endl;
cout << termColor() << endl;
FaultHandle FaultHandle;
FaultHandle.Run(run_command.c_str(), mission_file.c_str());
return(0);
}

9
src/pFaultHandle/pFaultHandle.moos Normal file
View File

@@ -0,0 +1,9 @@
//------------------------------------------------
// pFaultHandle config block
ProcessConfig = pFaultHandle
{
AppTick = 4
CommsTick = 4
}

534
src/pSurfaceSupportComm/SurfaceSupportComm.cpp
View File

@@ -19,8 +19,8 @@ using namespace std;
#define TCP_RECEIVE_PORT 8000
// #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 "127.0.0.1"
//#define DEST_IP_ADDRESS "10.25.0.163"
//#define DEST_IP_ADDRESS "10.25.0.160"
//---------------------------------------------------------
@@ -86,11 +86,8 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
std::istringstream iss(estimatedStateString);
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err);
if (!parsingResult) {
std::cerr << "Failed to parse JSON string." << std::endl;
return -1;
}
if (parsingResult)
{
estimatedState.currentLon = estimatedStateData["currentLon"].asFloat();
estimatedState.currentLat = estimatedStateData["currentLat"].asFloat();
estimatedState.currentAltitude = estimatedStateData["currentAltitude"].asFloat();
@@ -117,11 +114,11 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
deviceStatus.throwingLoadEnable = estimatedStateData["throwingLoadEnable"].asUInt();
deviceStatus.dvlStatus = estimatedStateData["dvlStatus"].asUInt();
}
}
if(key == "uMission_task_fb")
{
missionStatusString = msg.GetString();
}
}
return(true);
@@ -155,14 +152,14 @@ bool SurfaceSupportComm::udpSendToServer(DUNE::IMC::Message * msg, std::string a
bool SurfaceSupportComm::Iterate()
{
// AppCastingMOOSApp::Iterate();
#if 1
DUNE::IMC::Announce announceMsg;
announceMsg.setTimeStamp();
announceMsg.setSource(header_src);
announceMsg.setSourceEntity(header_src_ent);
announceMsg.setDestination(header_dst);
announceMsg.setDestinationEntity(header_dst_ent);
announceMsg.sys_name.assign("lauv-150");
announceMsg.sys_name.assign(vehicleName);
announceMsg.sys_type = 2;
// announceMsg.owner = 65535;
announceMsg.lat = estimatedState.currentLat * M_PI / 180;
@@ -171,10 +168,8 @@ bool SurfaceSupportComm::Iterate()
// 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/");
#endif
#if 1
DUNE::IMC::PlanControlState planControlStateMsg;
planControlStateMsg.setTimeStamp();
planControlStateMsg.setSource(header_src);
@@ -196,13 +191,9 @@ bool SurfaceSupportComm::Iterate()
std::istringstream iss(missionStatusString);
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err);
if (!parsingResult)
if (parsingResult)
{
std::cerr << "Failed to parse JSON string." << std::endl;
return -1;
}
int planExecutingStatus = missionStatusObject["state"].asInt();
planControlStateMsg.state = planExecutingStatus;
planControlStateMsg.state = missionStatusObject["state"].asInt();
planControlStateMsg.plan_id = missionStatusObject["taskName"].asString(); //子任务名称，对应PlanDB info的[taskName]
planControlStateMsg.plan_eta = -1; //缺省值
planControlStateMsg.plan_progress = -1; //缺省值
@@ -245,6 +236,7 @@ bool SurfaceSupportComm::Iterate()
vehicleStateMsg.last_error = "";
vehicleStateMsg.last_error_time = -1;
}
}
else
{
planControlStateMsg.state = DUNE::IMC::PlanControlState::StateEnum::PCS_READY;
@@ -268,11 +260,6 @@ bool SurfaceSupportComm::Iterate()
vehicleStateMsg.last_error_time = -1;
}
#endif
#if 1
DUNE::IMC::MsgList equipmentMsgList;
equipmentMsgList.setTimeStamp();
equipmentMsgList.setSource(header_src);
@@ -280,9 +267,7 @@ bool SurfaceSupportComm::Iterate()
equipmentMsgList.setDestination(header_dst);
equipmentMsgList.setDestinationEntity(header_dst_ent);
retrieveEntityStatus(equipmentMsgList);
#endif
#if 1
DUNE::IMC::EstimatedState estimatedStateMsg;
estimatedStateMsg.setTimeStamp();
estimatedStateMsg.setSource(header_src);
@@ -293,34 +278,27 @@ bool SurfaceSupportComm::Iterate()
estimatedStateMsg.lon = estimatedState.currentLon * M_PI / 180;
estimatedStateMsg.depth = estimatedState.depth;
estimatedStateMsg.height = estimatedState.height;
// estimatedStateMsg.phi = estimatedState.roll * M_PI / 180; //x axis
// estimatedStateMsg.theta = estimatedState.pitch * M_PI / 180; //y axis
// estimatedStateMsg.psi = estimatedState.yaw * M_PI / 180; //z axis
estimatedStateMsg.phi = estimatedState.roll; //x axis
estimatedStateMsg.theta = estimatedState.pitch; //y axis
//estimatedStateMsg.psi = estimatedState.yaw; //z axis
estimatedStateMsg.phi = estimatedState.roll; //x axis radian
estimatedStateMsg.theta = estimatedState.pitch; //y axis radian
float yawTemp = estimatedState.yaw;
if ((yawTemp >= 0) && (yawTemp <= M_PI))
{
estimatedStateMsg.psi = yawTemp;
std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI
<< "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl;
// std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI
// << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl;
}
if ((yawTemp > M_PI) && (yawTemp <= 2 * M_PI))
{
estimatedStateMsg.psi = -1 * (2 * M_PI - yawTemp);
std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI
<< "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl;
// std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI
// << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl;
}
estimatedStateMsg.vx = estimatedState.linearVelocityNorth; //x axis
estimatedStateMsg.vy = estimatedState.linearVelocityEast; //y axis
estimatedStateMsg.vz = estimatedState.linearVelocityDown; //z axis
#endif
double TimeScheduled = TimeLast + 0.5;
// double TimeScheduled = TimeLast + 1.0;
@@ -349,58 +327,41 @@ bool SurfaceSupportComm::Iterate()
double TimeNow = MOOS::Time();
TimeLast = TimeNow;
#if 1
tcpReceiveFromClient(recv_buf, sizeof(recv_buf)/sizeof(char), 0.005);
#endif
// AppCastingMOOSApp::PostReport();
return(true);
}
int SurfaceSupportComm::simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value)
{
int result = -1;
// double dfMean = 0;
// double dfVar = 0;
// unsigned int N = 0;
DUNE::IMC::EntityParameter subMsg;
subMsg.name = name;
subMsg.value = value;
entityParameter.params.push_back(subMsg);
// bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * /*pParamCaller*/)
// {
// //lets wait a while
// MOOSPause(2);
result = 0;
return result;
}
// //some stats
// double dfE = TimeNow-TimeScheduled;
// N=N+1;
// dfMean = dfMean*(N-1)/N+(dfE)/N;
double dfMean = 0;
double dfVar = 0;
unsigned int N = 0;
// if(N>1)
// dfVar = dfE/(N-1)+(N-1)/N*dfVar;
bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * /*pParamCaller*/)
{
//lets wait a while
MOOSPause(2);
//some stats
double dfE = TimeNow-TimeScheduled;
N=N+1;
dfMean = dfMean*(N-1)/N+(dfE)/N;
if(N>1)
dfVar = dfE/(N-1)+(N-1)/N*dfVar;
std::cout.setf(std::ios::fixed);
std::cout<<std::setprecision(4);
std::cout<<"Timer Callback \n";
std::cout<<" TimeNow "<<TimeNow<<"\n";
std::cout<<" TimeScheduled "<<TimeScheduled<<"\n";
std::cout<<" TimeLastRun "<<TimeLastRun<<"\n";
std::cout<<" Error "<<(TimeNow-TimeScheduled)*1000.0<<"ms\n";
std::cout<<" Mean Error "<<dfMean*1000.0<<"ms\n";
std::cout<<" Std Error "<<std::sqrt(dfVar)*1000.0<<"ms\n";
return true;
}
// std::cout.setf(std::ios::fixed);
// std::cout<<std::setprecision(4);
// std::cout<<"Timer Callback \n";
// std::cout<<" TimeNow "<<TimeNow<<"\n";
// std::cout<<" TimeScheduled "<<TimeScheduled<<"\n";
// std::cout<<" TimeLastRun "<<TimeLastRun<<"\n";
// std::cout<<" Error "<<(TimeNow-TimeScheduled)*1000.0<<"ms\n";
// std::cout<<" Mean Error "<<dfMean*1000.0<<"ms\n";
// std::cout<<" Std Error "<<std::sqrt(dfVar)*1000.0<<"ms\n";
// return true;
// }
// bool processMessageCallback(DUNE::IMC::Message * message)
// {
@@ -473,48 +434,6 @@ bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, vo
// return true;
// }
#if 0
int SurfaceSupportComm::retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList)
{
int result = -1;
DUNE::IMC::EntityParameters paramBattery;
paramBattery.name = "Battery";
simulateGetEntityStatus(paramBattery, "Voltage", "3");
simulateGetEntityStatus(paramBattery, "FuelLevel", "5");
simulateGetEntityStatus(paramBattery, "Temp", "7");
equipmentMsgList.msgs.push_back(paramBattery);
DUNE::IMC::EntityParameters paramController;
paramController.name = "Controller";
simulateGetEntityStatus(paramController, "Temp", "9");
equipmentMsgList.msgs.push_back(paramController);
DUNE::IMC::EntityParameters paramThrust;
paramThrust.name = "Thrust";
simulateGetEntityStatus(paramThrust, "Rpm", "11");
equipmentMsgList.msgs.push_back(paramThrust);
DUNE::IMC::EntityParameters paramDVL;
paramDVL.name = "DVL";
simulateGetEntityStatus(paramDVL, "Status", "1"); //取值范围{0, 1}
equipmentMsgList.msgs.push_back(paramDVL);
DUNE::IMC::EntityParameters paramIndicatorLight;
paramIndicatorLight.name = "IndicatorLight";
simulateGetEntityStatus(paramIndicatorLight, "Status", "1"); //取值范围{0, 1}
equipmentMsgList.msgs.push_back(paramIndicatorLight);
DUNE::IMC::EntityParameters paramThrowingLoad;
paramThrowingLoad.name = "ThrowingLoad";
simulateGetEntityStatus(paramThrowingLoad, "Status", "1"); //取值范围{0, 1}
equipmentMsgList.msgs.push_back(paramThrowingLoad);
result = 0;
return result;
}
#endif
int SurfaceSupportComm::retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList)
{
int result = -1;
@@ -691,18 +610,15 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
BatchConvertCoordinate(behaviorSpecData);
std::string queryMemberName = behaviorSpecData["taskName"].asString();
std::cout << queryMemberName << std::endl;
struct stat info;
if (stat(planConfigPath.c_str(), &info) != 0)
{
std::ofstream ofs;
ofs.open(planConfigPath, std::ios::out);
Json::StreamWriterBuilder builder;
Json::Value saveJsonValue;
saveJsonValue[queryMemberName] = behaviorSpecData;
const std::string json_file = Json::writeString(builder, saveJsonValue);
std::cout << json_file << std::endl;
ofs << json_file << std::endl;
Json::StreamWriterBuilder builder;
std::ofstream ofs;
ofs.open(planConfigPath, std::ios::out);
ofs << Json::writeString(builder, saveJsonValue) << std::endl;
ofs.close();
}
else
@@ -714,15 +630,19 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
taskConfigureReader.parse(ifs, tempJsonValue);
ifs.close();
std::ofstream ofs;
ofs.open(planConfigPath, std::ios::out);
Json::StreamWriterBuilder builder;
tempJsonValue[queryMemberName] = behaviorSpecData;
const std::string json_file = Json::writeString(builder, tempJsonValue);
std::cout << json_file << std::endl;
ofs << json_file << std::endl;
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());
}
if (msg->type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_GET_STATE)
{
@@ -762,6 +682,11 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
DUNE::Utils::ByteBuffer planDBBuffer;
DUNE::IMC::Packet::serialize(&planDBMsg, planDBBuffer);
tcpSendToClient((char *)(planDBBuffer.getBuffer()), planDBMsg.getSerializationSize());
std::stringstream ss;
ss << std::fixed << std::setprecision(6)
<< "PlanDB Get" << ":"
<< MOOS::Time();
Notify("uClient_plandbGet_log", ss.str());
}
}
if (type == DUNE::IMC::SetEntityParameters::getIdStatic())
@@ -769,6 +694,10 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
DUNE::IMC::SetEntityParameters * msg = dynamic_cast<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)
<< "SetEntityParameters" << ":"
<< MOOS::Time();
for(; iter < msg->params.end(); iter++)
{
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter);
@@ -816,7 +745,9 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
ss >> dataValue;
printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str());
}
ss2 << "," << parentName << "," << childName << "," << dataValueTemp;
}
Notify("uClient_parameterSet_log", ss2.str());
}
if (type == DUNE::IMC::PlanControl::getIdStatic())
{
@@ -839,7 +770,7 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
outputTaskValue["action"] = action;
Json::StreamWriterBuilder builder2;
std::string outputTaskString = Json::writeString(builder2, outputTaskValue);
std::cout << outputTaskString << std::endl;
// std::cout << outputTaskString << std::endl;
if (action == "start")
{
std::ifstream ifs;
@@ -852,9 +783,10 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
float originLat = tempJsonValue[msg->plan_id]["origin"]["lat"].asFloat();
float originLon = tempJsonValue[msg->plan_id]["origin"]["lon"].asFloat();
Json::Value outputOrginValue;
outputOrginValue["altitude"] = originAlt;
outputOrginValue["alt"] = originAlt;
outputOrginValue["lat"] = originLat;
outputOrginValue["lon"] = originLon;
outputOrginValue["task"] = msg->plan_id;
Json::StreamWriterBuilder builder1;
std::string outputOriginString = Json::writeString(builder1, outputOrginValue);
std::cout << outputOriginString << std::endl;
@@ -866,7 +798,13 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
{
Notify("uMission_action_cmd", outputTaskString);
}
std::stringstream ss;
ss << std::fixed << std::setprecision(6)
<< "PlanControl" << ":"
<< MOOS::Time() << ","
<< msg->plan_id << ","
<< action;
Notify("uClient_planControl_log", ss.str());
}
}
if (type == DUNE::IMC::VehicleCommand::getIdStatic())
@@ -884,113 +822,259 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
Notify("uManual_enable_cmd", (double)msg->command);
}
}
std::stringstream ss;
ss << std::fixed << std::setprecision(6)
<< "VehicleCommand" << ":"
<< MOOS::Time() << ","
<< (int)msg->command;
Notify("uClient_manualEnable_log", ss.str());
}
if (type == DUNE::IMC::RemoteActions::getIdStatic())
{
DUNE::IMC::RemoteActions * msg = dynamic_cast<DUNE::IMC::RemoteActions *>(message);
printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->actions);
Notify("uManual_drive_cmd", msg->actions);
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg->actions);
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
std::stringstream ss;
ss << std::fixed << std::setprecision(6)
<< "RemoteActions" << ":"
<< MOOS::Time() << ","
<< "Thrust" << ","
<< recvCommand["Thrust"].asInt() << ","
<< "Heading" << ","
<< recvCommand["Heading"].asFloat();
Notify("uClient_manualDrive_log", ss.str());
}
if (type == DUNE::IMC::LogBookControl::getIdStatic())
{
DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
uint8_t logBookType = 0;
std::string logBookContext = "";
std::string logBookText = "";
DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
for (; iter != msg->msg.end(); ++iter)
{
DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast<DUNE::IMC::LogBookEntry *>(*iter);
logBookType = msgLogBookEntry->type;
logBookContext = msgLogBookEntry->context;
logBookText = msgLogBookEntry->text;
}
printf("server receive %s: %lf, %u, %u, %s\n", msg->getName(), msg->getTimeStamp(),
msg->command, logBookType, logBookContext.c_str());
std::string saveLogDir;
m_MissionReader.GetValue("LogDir", saveLogDir);
char lastChar = saveLogDir.at(saveLogDir.size()-1);
if (std::string(1, lastChar) != std::string("/"))
{
saveLogDir += "/";
}
if (msg->command == DUNE::IMC::LogBookControl::CommandEnum::LBC_GET)
{
if (logBookType == DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO && logBookContext == "LIST")
{
std::vector<std::string> localAUVNamePath;
getAllFiles(saveLogDir.c_str(), localAUVNamePath);
std::vector<std::string> localDayPath;
for (int i=0; i<localAUVNamePath.size(); i++)
{
getAllFiles(localAUVNamePath.at(i).c_str(), localDayPath);
}
std::vector<std::string> localTimePath;
for (int i=0; i<localDayPath.size(); i++)
{
getAllFiles(localDayPath.at(i).c_str(), localTimePath);
}
std::vector<std::string> localFilePath;
for (int i=0; i<localTimePath.size(); i++)
{
getAllFiles(localTimePath.at(i).c_str(), localFilePath);
}
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);
Json::Value outputFileList;
int startPos = saveLogDir.length();
for (int i=0; i<localFilePath.size(); i++)
{
std::string subFilePath = localFilePath.at(i).substr(startPos);
int subFilePos = subFilePath.rfind("/");
std::string subFileDir = subFilePath.substr(0, subFilePos);
std::string subFileName = subFilePath.substr(subFilePos+1);
if (!outputFileList.isMember(subFileDir))
{
Json::Value singleFileList;
singleFileList["auv"] = "";
singleFileList["mission"] = "";
singleFileList["command"] = "";
singleFileList["fault"] = "";
outputFileList[subFileDir] = singleFileList;
}
if (subFileName == auvDataFile)
{
outputFileList[subFileDir]["auv"] = subFileName;
}
if (subFileName == missionHistoryFile)
{
outputFileList[subFileDir]["mission"] = subFileName;
}
if (subFileName == clientCommandFile)
{
outputFileList[subFileDir]["command"] = subFileName;
}
if (subFileName == faultLogFile)
{
outputFileList[subFileDir]["fault"] = subFileName;
}
}
Json::StreamWriterBuilder builder;
std::string outputFileString = Json::writeString(builder, outputFileList);
DUNE::IMC::LogBookControl msgFB;
msgFB.setTimeStamp();
msgFB.setSource(header_src);
msgFB.setSourceEntity(header_src_ent);
msgFB.setDestination(header_dst);
msgFB.setDestinationEntity(header_dst_ent);
msgFB.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_REPLY;
DUNE::IMC::LogBookEntry msgLogBookEntry;
msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
msgLogBookEntry.text = outputFileString;
msgLogBookEntry.context = logBookContext;
msgFB.msg.push_back(msgLogBookEntry);
DUNE::Utils::ByteBuffer logBookControlBuffer;
DUNE::IMC::Packet::serialize(&msgFB, logBookControlBuffer);
tcpSendToClient((char *)(logBookControlBuffer.getBuffer()), msgFB.getSerializationSize());
}
if (logBookType == DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO && logBookContext == "RETR")
{
std::string err;
Json::Value retrFileObject;
std::istringstream iss(logBookText);
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &retrFileObject, &err);
if (parsingResult)
{
Json::Value retrFileArray = retrFileObject["file"];
for (int i = 0; i < retrFileArray.size(); i++)
{
std::string targetFile = saveLogDir + retrFileArray[i].asString();
std::cout << "file: " << targetFile << std::endl;
}
}
}
}
if (msg->command == DUNE::IMC::LogBookControl::CommandEnum::LBC_CLEAR)
{
if (logBookType == DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO && logBookContext == "DELE")
{
std::string err;
Json::Value inputFileObject;
std::istringstream iss(logBookText);
Json::CharReaderBuilder builder1;
bool parsingResult = Json::parseFromStream(builder1, iss, &inputFileObject, &err);
if (parsingResult)
{
Json::Value inputFileArray = inputFileObject["file"];
Json::Value outputFileObject;
for (int i = 0; i < inputFileArray.size(); i++)
{
std::string targetFile = saveLogDir + inputFileArray[i].asString();
Json::Value singleFileObject;
struct stat info;
if (stat(targetFile.c_str(), &info) == 0)
{
remove(targetFile.c_str());
singleFileObject["result"] = "success";
}
else
{
singleFileObject["result"] = "failure";
}
outputFileObject[inputFileArray[i].asString()] = singleFileObject;
}
Json::StreamWriterBuilder builder2;
std::string outputFileString = Json::writeString(builder2, outputFileObject);
DUNE::IMC::LogBookControl msgFB;
msgFB.setTimeStamp();
msgFB.setSource(header_src);
msgFB.setSourceEntity(header_src_ent);
msgFB.setDestination(header_dst);
msgFB.setDestinationEntity(header_dst_ent);
msgFB.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_REPLY;
DUNE::IMC::LogBookEntry msgLogBookEntry;
msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
msgLogBookEntry.text = outputFileString;
msgLogBookEntry.context = logBookContext;
msgFB.msg.push_back(msgLogBookEntry);
DUNE::Utils::ByteBuffer logBookControlBuffer;
DUNE::IMC::Packet::serialize(&msgFB, logBookControlBuffer);
tcpSendToClient((char *)(logBookControlBuffer.getBuffer()), msgFB.getSerializationSize());
}
}
}
}
}
void SurfaceSupportComm::ParsePlanConfig(Json::Value inputJsonValue)
void SurfaceSupportComm::getAllFiles(const char * dir_name, std::vector<std::string>& filePath)
{
int taskCount = inputJsonValue.size();
std::vector<std::string> taskList = inputJsonValue.getMemberNames();
std::cout << "taskCount: " << taskCount << std::endl;
double lastChangeStamp = 0;
std::string lastChangeSourceAddress;
std::string lastChangeSourceName;
for (int i=0; i<taskCount; i++)
if( NULL == dir_name )
{
std::cout << taskList.at(i) << std::endl;
Json::Value subTask = inputJsonValue[taskList.at(i)];
std::string taskName = subTask["taskName"].asString();
std::string taskId = subTask["taskId"].asString();
int priority = subTask["priority"].asInt();
float duration = subTask["duration"].asFloat();
bool closedLoop = subTask["closedLoop"].asBool();
float constSpeed = subTask["constSpeed"].asFloat();
int repeat = subTask["repeat"].asInt();
bool perpetual = subTask["perpetual"].asBool();
float minDepth = subTask["minDepth"].asFloat();
float maxDepth = subTask["maxDepth"].asFloat();
std::string sourceName = subTask["sourceName"].asString();
std::string sourceAddress = subTask["sourceAddress"].asString();
double boardStamp = subTask["boardStamp"].asDouble();
double clientStamp = subTask["clientStamp"].asDouble();
float originLat = subTask["origin"]["lat"].asFloat();
float originLon = subTask["origin"]["lon"].asFloat();
float originAlt = subTask["origin"]["altitude"].asFloat();
Json::StreamWriterBuilder builder;
std::string subTaskString = Json::writeString(builder, subTask);
if (boardStamp > lastChangeStamp)
{Json::Value inputJsonValue;
lastChangeStamp = boardStamp;
lastChangeSourceAddress = sourceAddress;
lastChangeSourceName = sourceName;
return;
}
std::cout << "taskName: " << taskName << std::endl;
std::cout << "taskId: " << taskId << std::endl;
std::cout << "originLon: " << originLon << std::endl;
std::cout << "originLat: " << originLat << std::endl;
std::cout << "originAlt: " << originAlt << std::endl;
std::cout << "sourceName: " << sourceName << std::endl;
std::cout << "sourceAddress: " << sourceAddress << std::endl;
std::cout << "boardStamp: " << boardStamp << std::endl;
std::cout << "clientStamp: " << clientStamp << std::endl;
std::cout << "priority: " << priority << std::endl;
std::cout << "duration: " << duration << std::endl;
std::cout << "closedLoop: " << closedLoop << std::endl;
std::cout << "constSpeed: " << constSpeed << std::endl;
std::cout << "repeat: " << repeat << std::endl;
std::cout << "perpetual: " << perpetual << std::endl;
std::cout << "minDepth: " << minDepth << std::endl;
std::cout << "maxDepth: " << maxDepth << std::endl;
if (subTask["origin"].isArray())
struct stat s;
lstat( dir_name , &s );
if(!S_ISDIR( s.st_mode ))
{
Json::Value array = subTask["origin"];
if (array.size() == 3)
return;
}
struct dirent * filename;
DIR * dir;
dir = opendir( dir_name );
if( NULL == dir )
{
float origin_lon = array[0].asFloat();
float origin_lat = array[1].asFloat();
float origin_alt = array[2].asFloat();
std::cout << "origin: [" << origin_lon << ", "
<< origin_lat << ", " << origin_alt << "] " << std::endl;
}
return;
}
while((filename = readdir(dir)) != NULL)
{
if( strcmp(filename->d_name , "." ) == 0 || strcmp(filename->d_name , "..") == 0 )
continue;
if (subTask["points"].isArray())
char lastChar = dir_name[strlen(dir_name) - 1];
if (std::string(1, lastChar) == std::string("/"))
{
Json::Value array = subTask["points"];
for (int j = 0; j < array.size(); j++)
filePath.push_back(std::string(dir_name) + std::string(filename->d_name));
}
else
{
std::string name = array[j]["name"].asString();
float lon = array[j]["lon"].asFloat();
float lat = array[j]["lat"].asFloat();
float depth = array[j]["depth"].asFloat();
float speed = array[j]["speed"].asFloat();
float north = array[j]["north"].asFloat();
float east = array[j]["east"].asFloat();
std::string type = array[j]["type"].asString();
std::cout << "point " << j << ": "
<< name << ", "
<< lon << ", "
<< lat << ", "
<< north << ", "
<< east << ", "
<< depth << ", "
<< speed << ", "
<< type << ", "
<< std::endl;
}
filePath.push_back(std::string(dir_name) + "/" + std::string(filename->d_name));
}
}
return ;
}
void SurfaceSupportComm::BatchConvertCoordinate(Json::Value &object)
@@ -1019,7 +1103,6 @@ void SurfaceSupportComm::BatchConvertCoordinate(Json::Value &object)
void SurfaceSupportComm::closeConnection(Client& c, std::exception& e)
{
// long unsigned int client_count = m_clients.size() - 1;
m_poll.remove(*c.socket);
delete c.socket;
}
@@ -1027,6 +1110,9 @@ void SurfaceSupportComm::closeConnection(Client& c, std::exception& e)
bool SurfaceSupportComm::OnStartUp()
{
// AppCastingMOOSApp::OnStartUp();
m_MissionReader.GetValue("VehicleName", vehicleName);
planConfigPath = "";
m_MissionReader.GetValue("PlanConfigPath", planConfigPath);

5
src/pSurfaceSupportComm/SurfaceSupportComm.h
View File

@@ -122,12 +122,12 @@ private:
std::vector<double> &point_lla);
void BatchConvertCoordinate(Json::Value &object);
void ParsePlanConfig(Json::Value inputJsonValue);
int simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value);
int retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList);
template <typename Type>
inline int getEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, Type& value);
void getAllFiles(const char * dir_name, std::vector<std::string>& filePath);
int testFlag = 0;
double TimeLast;
int sendCount = 0;
@@ -135,6 +135,7 @@ private:
PeriodicUDPEvent udpEvent;
// PeriodicTCPEvent tcpEvent;
std::string missionStatusString;
std::string vehicleName;
};
#endif

45
src/pTaskManger/TaskManger.cpp
View File

@@ -48,7 +48,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
string msg_str = msg.GetString();
double msg_dval = msg.GetDouble();
bool msg_bval = msg.GetBinaryData();
// cout << msg_name + ": " << msg_str << endl;
// 内部消息
if(msg_name == MSG_ENDFLAG)
{
if(msg_str == "true")
@@ -82,7 +82,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
faultNumber = 99;
}
if(msg_name == MSG_SENDSAFTRULES && msg_str == "true")
if(msg_name == MSG_SENDSAFTRULES && msg_str == "true") // 暂时没用配置功能
{
state = CONFIG;
st = 40;
@@ -92,7 +92,7 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
st = 39;
state = FAULT;
}
// 交互消息
if(msg_name == MSG_IN_SSM)
{
Json::Value j;
@@ -120,6 +120,20 @@ bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
}
}
if((msg_name == MSG_FAULT_LEV2) || (msg_name == MSG_FALUT_LEV3))
{
Json::Value j;
Json::Reader a;
a.parse(msg_str,j);
int falut_num = j["FaultLevel"].asInt();
if(falut_num != 0)
{
faultNumber = 20;
state = FAULT;
}
}
if(msg_name == MSG_IN_MAN)
{
if(msg_dval == 1)
@@ -254,7 +268,6 @@ bool TaskManger::Iterate()
start = false;
Notify(MSG_ENDFLAG,"false");
Notify(MSG_WPTFLAG,"false");
// Notify(MSG_START,"false");
Notify(MSG_RUN,"false");
Notify("MOOS_MANUAL_OVERRIDE","true");
cout << "Wating task start......" << endl;
@@ -283,6 +296,8 @@ bool TaskManger::Iterate()
st = 31;
else if(faultNumber>10 && faultNumber<20)
st = 32;
else if(faultNumber==20)
st = 33;
else
st = 38;
break;
@@ -299,6 +314,20 @@ bool TaskManger::Iterate()
state = UNRUN;
break;
}
case 20: //接收到来自其他组件的故障
{
RepList["FALT"] = "Reresive fault from others";
current_node_complete = false;
current_pol_complete = false;
current_node="";
start = false;
Notify(MSG_ENDFLAG,"false");
Notify(MSG_WPTFLAG,"false");
Notify(MSG_RUN,"false");
Notify("MOOS_MANUAL_OVERRIDE","true");
cout << "Wating task start......" << endl;
st = 21;
}
case 38: //未知故障
{
postFaultNumber(99);
@@ -573,14 +602,17 @@ bool TaskManger::setWayConfig(string lead, string lead_damper, string capture_li
void TaskManger::RegisterVariables()
{
AppCastingMOOSApp::RegisterVariables();
//内部消息订阅
Register(MSG_ENDFLAG, 0);
// Register(MSG_START, 0);
Register(MSG_WPTFLAG,0);
Register(MSG_SENDSAFTRULES,0);
Register(MSG_FALUT,0);
Register(MSG_CLEARFAULT,0);
//交互消息订阅
Register(MSG_IN_SSM,0);
Register(MSG_IN_MAN,0);
Register(MSG_FAULT_LEV2,1);
Register(MSG_FALUT_LEV3,1);
}
/**
* @description: read task File to nodeList
@@ -941,7 +973,7 @@ bool TaskManger::buildReport()
}
m_msgs << "=========================================================" << endl;
RepList["Current Node"] = current_node;
RepList["remaining number of nodes"] = nodeList.size()+1;
RepList["remaining number of nodes"] = (int)nodeList.size()+1;
string rep = Json::writeString(RepJsBuilder, RepList);
m_msgs << rep << endl;
return true;
@@ -992,6 +1024,7 @@ void TaskManger::postReportToSSM()
//3.超出区域
//4.低于最小高度
//11:任务文本
//20:接收到其余程序传递的故障
//99.未知故障
void TaskManger::postFaultNumber(int n)
{

11
src/pTaskManger/TaskManger.h
View File

@@ -2,7 +2,7 @@
* @Author: 1553836110 1553836110@qq.com
* @Date: 2023-09-28 15:45:17
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-08 14:53:05
* @LastEditTime: 2023-11-15 09:31:32
* @FilePath: /moos-ivp-pi/src/pTaskManger/TaskManger.h
* @Description:
*
@@ -74,7 +74,7 @@ class TaskManger : public AppCastingMOOSApp
inline void taskFinish();
void FaultFlagClear();
inline void InitConfig();
//内部消息发布
const string UPDATE_WPT = "WPT_UPDATE";
const string UPDATE_SPD = "SPEED_UPDATE";
const string UPDATE_DEP = "DEPTH_UPDATE";
@@ -82,7 +82,7 @@ class TaskManger : public AppCastingMOOSApp
const string UPDATE_MAXDEP = "MAXDEEP_UPDATES";
const string UPDATE_OPREGION = "OPREGION_UPDATES";
const string UPDATE_RESETFAULT = "OPREGION_RESET";
//内部消息订阅
const string MSG_WPTFLAG = "CurrentPointComplete";
const string MSG_ENDFLAG = "END_WayPoint";
const string MSG_START = "START";
@@ -90,12 +90,13 @@ class TaskManger : public AppCastingMOOSApp
const string MSG_FALUT = "TaskFault";
const string MSG_RUN = "RUN";
const string MSG_CLEARFAULT = "ClearFalut";
//交互消息
const string MSG_IN_SSM = "uMission_action_cmd";
const string MSG_TO_SSM = "uMission_task_fb";
const string MSG_TO_FH = "uMission_fault_fb";
const string MSG_IN_FH = "uDrive_fault_fb";
const string MSG_IN_MAN = "uManual_enable_cmd"; //TODO: 增加手操状态
const string MSG_FAULT_LEV2 = "uFH_errorMsg2_fb";
const string MSG_FALUT_LEV3 = "uFH_errorMsg3_fb";
protected:
// 任务参数