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", "taskId" : "1",
"taskName" : "east_waypt_survey" "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" : "west_waypt_survey" :
{ {
"boardStamp" : 1698135268.3958621, "boardStamp" : 1698135268.3958621,

34
launch/launch.moos
View File

@@ -15,6 +15,19 @@ LatOrigin = 43.825300
LongOrigin = -70.330400 LongOrigin = -70.330400
AltOrigin = 0 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 // Antler configuration block
ProcessConfig = ANTLER ProcessConfig = ANTLER
@@ -27,18 +40,18 @@ ProcessConfig = ANTLER
Run = uProcessWatch @ NewConsole = false Run = uProcessWatch @ NewConsole = false
Run = pRealm @ NewConsole = false Run = pRealm @ NewConsole = false
Run = pShare @ NewConsole = false Run = pShare @ NewConsole = false
//Run = pMarineViewer @ NewConsole = false Run = pMarineViewer @ NewConsole = false
//Run = pLogger @ NewConsole = false
//===========Our define process==================== //===========Our define process====================
Run = pBoardSupportComm @ NewConsole = false Run = pBoardSupportComm @ NewConsole = false
Run = pTaskManger @ NewConsole = false Run = pTaskManger @ NewConsole = false
Run = pMotionControler @ NewConsole = false Run = pMotionControler @ NewConsole = false
//Run = pSurfaceSupportComm @ NewConsole = false Run = pSurfaceSupportComm @ NewConsole = false
Run = pDataManagement @ NewConsole = false Run = pDataManagement @ NewConsole = false
//===============For test process=================== //===============For test process===================
Run = pEmulator @ NewConsole = false Run = pEmulator @ NewConsole = false
//Run = uSimMarine @ NewConsole = false //Run = uSimMarine @ NewConsole = false
//Run = pMarinePID @ NewConsole = false //Run = pMarinePID @ NewConsole = false
Run = pFaultHandle @ NewConsole = false
} }
ProcessConfig = pMarineViewer ProcessConfig = pMarineViewer
@@ -195,9 +208,6 @@ ProcessConfig = pTaskManger
{ {
AppTick = 8 AppTick = 8
CommsTick = 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 ProcessConfig = pBoardSupportComm
@@ -259,13 +269,13 @@ ProcessConfig = pMotionControler
cheak_stalensee = true cheak_stalensee = true
delta_freqency = 5 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 ProcessConfig = pEmulator
{ {
AppTick = 100 AppTick = 5
CommsTick = 5 CommsTick = 5
matlab_host = 10.25.0.137 matlab_host = 10.25.0.137
matlab_port = 8085 matlab_port = 8085
@@ -304,3 +314,9 @@ ProcessConfig = pLogger
WildCardOmitPattern = DB_RWSUMMARY WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true 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(pTaskSend)
ADD_SUBDIRECTORY(pMotionControler) ADD_SUBDIRECTORY(pMotionControler)
ADD_SUBDIRECTORY(pEmulator) ADD_SUBDIRECTORY(pEmulator)
ADD_SUBDIRECTORY(pFaultHandle)
############################################################################## ##############################################################################
# END of CMakeLists.txt # END of CMakeLists.txt
############################################################################## ##############################################################################

150
src/pBoardSupportComm/BoardSupportComm.cpp
View File

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

19
src/pBoardSupportComm/BoardSupportComm.h
View File

@@ -77,7 +77,6 @@ struct AUVExecuteCommand
}; };
struct EstimatedState { struct EstimatedState {
std::string info;
float referenceLon; float referenceLon;
float referenceLat; float referenceLat;
float referenceAltitude; float referenceAltitude;
@@ -113,18 +112,6 @@ protected: // Standard MOOSApp functions to overload
// bool buildReport(); // bool buildReport();
private: 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; struct EstimatedState estimatedState;
@@ -146,12 +133,8 @@ private:
std::string convertEmbeddedFormat(Json::Value &embeddedState); std::string convertEmbeddedFormat(Json::Value &embeddedState);
int tcpSockFD; int tcpSockFD;
int tcpConnectRet; int tcpConnectRet;
// unsigned char tcpReceiveBuffer[65535];
char tcpReceiveBuffer[65535]; char tcpReceiveBuffer[65535];
unsigned char tcpSendBuffer[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 parseMessage(unsigned char* buffer, int size);
int serializeMessage(unsigned char* buffer); int serializeMessage(unsigned char* buffer);
@@ -165,6 +148,8 @@ private:
bool testCount = false; bool testCount = false;
int sockfd = -1; int sockfd = -1;
int connectFlg = -1; int connectFlg = -1;
std::string llaOriginPath;
}; };

331
src/pClientViewer/ClientViewer.cpp
View File

@@ -71,7 +71,7 @@ bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail)
double mtime = msg.GetTime(); double mtime = msg.GetTime();
bool mdbl = msg.IsDouble(); bool mdbl = msg.IsDouble();
bool mstr = msg.IsString(); bool mstr = msg.IsString();
std::cout << key << " : " << sval << std::endl; std::cout << key << " : " << sval << std::endl;
if(key == "Command") if(key == "Command")
{ {
if (sval == "SetPlan1") //PlanDB if (sval == "SetPlan1") //PlanDB
@@ -204,7 +204,55 @@ bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail)
msg.type = DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST; msg.type = DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST;
msg.command = 1; msg.command = 1;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT); 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() bool ClientViewer::Iterate()
{ {
#if 1
DUNE::IMC::Message * receiveUDPMessage; DUNE::IMC::Message * receiveUDPMessage;
while ((receiveUDPMessage = imcPollUDP()) != NULL) while ((receiveUDPMessage = imcPollUDP()) != NULL)
@@ -230,90 +277,91 @@ bool ClientViewer::Iterate()
processMessage(receiveUDPMessage); processMessage(receiveUDPMessage);
free(receiveUDPMessage); free(receiveUDPMessage);
} }
#endif
# if 1
DUNE::IMC::Message * receiveTCPMessage; DUNE::IMC::Message * receiveTCPMessage;
while ((receiveTCPMessage = imcPollTCP()) != NULL) while ((receiveTCPMessage = imcPollTCP()) != NULL)
{ {
processMessage(receiveTCPMessage); processMessage(receiveTCPMessage);
free(receiveTCPMessage); free(receiveTCPMessage);
} }
#endif
return(true); return(true);
} }
bool processMessageCallback(DUNE::IMC::Message * message) // bool processMessageCallback(DUNE::IMC::Message * message)
{ // {
int type = message->getId(); // int type = message->getId();
#if 1 // if (type == DUNE::IMC::Announce::getIdStatic())
if (type == DUNE::IMC::Announce::getIdStatic()) // {
{ // DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message); // // printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// printf("server receive %s: %lf, %lf, %lf, %lf\n", \ // // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
// msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height); // printf("server receive %s: %lf, %lf, %lf, %lf\n", \
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);
msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height); // }
} // if (type == DUNE::IMC::PlanDB::getIdStatic())
#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);
DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message); // printf("%s\n", msg->info.c_str());
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())
} // {
#if 1 // DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
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", \ // 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); // msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
} // }
if (type == DUNE::IMC::MsgList::getIdStatic()) // if (type == DUNE::IMC::MsgList::getIdStatic())
{ // {
DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message); // DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); // printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin(); // DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
for (; iter1 != msgList->msgs.end(); ++iter1) // for (; iter1 != msgList->msgs.end(); ++iter1)
{ // {
DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1); // DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin(); // DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
for (; iter2 != entityParameter->params.end(); ++iter2) // for (; iter2 != entityParameter->params.end(); ++iter2)
{ // {
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2); // DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; // std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;
} // }
} // }
} // }
if (type == DUNE::IMC::EstimatedState::getIdStatic()) // if (type == DUNE::IMC::EstimatedState::getIdStatic())
{ // {
DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message); // DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
// printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", // // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// msg->getName(), msg->getTimeStamp(), // // msg->getName(), msg->getTimeStamp(),
// msg->lat, msg->lon, msg->depth, // // msg->lat, msg->lon, msg->depth,
// msg->phi, msg->theta, msg->psi); // // msg->phi, msg->theta, msg->psi);
printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n", // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
msg->getName(), msg->getTimeStamp(), // msg->getName(), msg->getTimeStamp(),
msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI, // 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); // msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
} // }
if (type == DUNE::IMC::VehicleState::getIdStatic()) // if (type == DUNE::IMC::VehicleState::getIdStatic())
{ // {
DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message); // DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
printf("server receive %s: %lf, %u\n", // printf("server receive %s: %lf, %u\n",
msg->getName(), msg->getTimeStamp(), msg->op_mode); // msg->getName(), msg->getTimeStamp(), msg->op_mode);
} // }
if (type == DUNE::IMC::PlanControl::getIdStatic()) // if (type == DUNE::IMC::LogBookControl::getIdStatic())
{ // {
DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message); // DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
printf("server receive %s: %lf, %u,\n", // printf("server receive %s: %lf, %u\n",
msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str()); // msg->getName(), msg->getTimeStamp(), msg->command);
}
#endif // DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
return true; // 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() // Procedure: OnStartUp()
@@ -333,12 +381,9 @@ bool ClientViewer::OnStartUp()
// tcpCommEvent.SetCallback(processMessageCallback,NULL); // tcpCommEvent.SetCallback(processMessageCallback,NULL);
// tcpCommEvent.Start(); // tcpCommEvent.Start();
#if 1
sock_udp_receive.bind(UDP_RECEIVE_PORT, DUNE::Network::Address::Any, false); sock_udp_receive.bind(UDP_RECEIVE_PORT, DUNE::Network::Address::Any, false);
m_poll_0.add(sock_udp_receive); m_poll_0.add(sock_udp_receive);
#endif
#if 1
try try
{ {
// sock_tcp_send.setSendTimeout(5); // sock_tcp_send.setSendTimeout(5);
@@ -351,16 +396,11 @@ bool ClientViewer::OnStartUp()
{ {
std::cerr << e.what() << '\n'; std::cerr << e.what() << '\n';
} }
#endif
#if 1
char local_ip2[INET_ADDRSTRLEN] = {0}; char local_ip2[INET_ADDRSTRLEN] = {0};
get_local_ip_using_create_socket(local_ip2); get_local_ip_using_create_socket(local_ip2);
ethernetIP = local_ip2; ethernetIP = local_ip2;
#endif
RegisterVariables(); RegisterVariables();
return(true); return(true);
@@ -668,71 +708,74 @@ bool ClientViewer::tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, i
void ClientViewer::processMessage(DUNE::IMC::Message * message) { void ClientViewer::processMessage(DUNE::IMC::Message * message) {
int type = message->getId(); int type = message->getId();
if (type == DUNE::IMC::Announce::getIdStatic()) // if (type == DUNE::IMC::Announce::getIdStatic())
{ // {
DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message); // DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
// printf("server receive %s: %lf, %lf, %lf, %lf\n", \ // // printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height); // // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
printf("server receive %s: %lf, %lf, %lf, %lf\n", \ // 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); // msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
} // }
else if (type == DUNE::IMC::PlanDB::getIdStatic()) // if (type == DUNE::IMC::PlanDB::getIdStatic())
{ // {
DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message); // DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op); // printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
printf("%s\n", msg->info.c_str()); // printf("%s\n", msg->info.c_str());
} // }
else if (type == DUNE::IMC::PlanControlState::getIdStatic()) // if (type == DUNE::IMC::PlanControlState::getIdStatic())
{ // {
DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message); // DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
printf("server receive %s: %lf, %s, %s, %u\n", \ // 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); // msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
} // }
else if (type == DUNE::IMC::MsgList::getIdStatic()) // 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::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message); DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); printf("server receive %s: %lf, %u\n",
DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin(); msg->getName(), msg->getTimeStamp(), msg->command);
for (; iter1 != msgList->msgs.end(); ++iter1)
DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
for (; iter != msg->msg.end(); ++iter)
{ {
DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1); DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast<DUNE::IMC::LogBookEntry *>(*iter);
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin(); std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->context << ", " << msgLogBookEntry->text << std::endl;
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, (%f, %f, %f), (%f, %f, %f)\n",
// msg->getName(), msg->getTimeStamp(),
// msg->lat, msg->lon, msg->depth,
// msg->phi, msg->theta, msg->psi);
printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
msg->getName(), msg->getTimeStamp(),
msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
}
else if (type == DUNE::IMC::VehicleState::getIdStatic())
{
DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(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
{
}
} }
//--------------------------------------------------------- //---------------------------------------------------------

415
src/pDataManagement/DataManagement.cpp
View File

@@ -6,9 +6,9 @@
/************************************************************/ /************************************************************/
#include <iterator> #include <iterator>
#include <sys/stat.h>
#include "MBUtils.h" #include "MBUtils.h"
#include "DataManagement.h" #include "DataManagement.h"
//#include "NavigationInfo.pb.h"
#include <json/json.h> #include <json/json.h>
using namespace std; using namespace std;
@@ -17,18 +17,50 @@ using namespace std;
// Constructor // Constructor
DataManagement::DataManagement() 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 // Destructor
DataManagement::~DataManagement() 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 // Procedure: OnNewMail
bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail) bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
DoAsyncLog(NewMail);
MOOSMSG_LIST::iterator p; MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++) for(p=NewMail.begin(); p!=NewMail.end(); p++)
@@ -44,89 +76,124 @@ bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
bool mdbl = msg.IsDouble(); bool mdbl = msg.IsDouble();
bool mstr = msg.IsString(); bool mstr = msg.IsString();
std::string estimatedStateString; if(key == "uDevice_monitor_fb")
if(key == "uMotion_pose_fb")
{ {
estimatedStateString = msg.GetString();
std::string err; std::string err;
Json::Value estimatedStateData; Json::Value estimatedStateData;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
const std::unique_ptr<Json::CharReader> reader(builder.newCharReader()); bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err);
if (!reader->parse(estimatedStateString.c_str(), if (!parsingResult)
estimatedStateString.c_str() + static_cast<int>(estimatedStateString.length()), {
&estimatedStateData, std::cerr << "Failed to parse JSON string." << std::endl;
&err)) return false;
{ }
std::cout << "error" << std::endl;
return EXIT_FAILURE;
}
std::string info = estimatedStateData["info"].asString();
float currentLon = estimatedStateData["currentLon"].asFloat();
float currentLat = estimatedStateData["currentLat"].asFloat();
float currentAltitude = estimatedStateData["currentAltitude"].asFloat();
float referenceLon = estimatedStateData["referenceLon"].asFloat();
float referenceLat = estimatedStateData["referenceLat"].asFloat();
float referenceAltitude = estimatedStateData["referenceAltitude"].asFloat();
float offsetNorth = estimatedStateData["offsetNorth"].asFloat();
float offsetEast = estimatedStateData["offsetEast"].asFloat();
float offsetDown = estimatedStateData["offsetDown"].asFloat();
float roll = estimatedStateData["roll"].asFloat();
float pitch = estimatedStateData["pitch"].asFloat();
float yaw = estimatedStateData["yaw"].asFloat();
float linearVelocityNorth = estimatedStateData["linearVelocityNorth"].asFloat();
float linearVelocityEast = estimatedStateData["linearVelocityEast"].asFloat();
float linearVelocityDown = estimatedStateData["linearVelocityDown"].asFloat();
float angularVelocityNorth = estimatedStateData["angularVelocityNorth"].asFloat();
float angularVelocityEast = estimatedStateData["angularVelocityEast"].asFloat();
float angularVelocityDown = estimatedStateData["angularVelocityDown"].asFloat();
float height = estimatedStateData["height"].asFloat();
float depth = estimatedStateData["depth"].asFloat();
std::cout << "info: " << info << std::endl;
std::cout << "referenceLon: " << referenceLon << std::endl;
std::cout << "referenceLat: " << referenceLat << std::endl;
std::cout << "referenceAltitude: " << referenceAltitude << std::endl;
std::cout << "currentLon: " << currentLon << std::endl;
std::cout << "currentLat: " << currentLat << std::endl;
std::cout << "altitude: " << currentAltitude << std::endl;
std::cout << "offsetNorth: " << offsetNorth << std::endl;
std::cout << "offsetEast: " << offsetEast << std::endl;
std::cout << "offsetDown: " << offsetDown << std::endl;
std::cout << "roll: " << roll << std::endl;
std::cout << "pitch: " << pitch << std::endl;
std::cout << "yaw: " << yaw << std::endl;
std::cout << "linearVelocityNorth: " << linearVelocityNorth << std::endl;
std::cout << "linearVelocityEast: " << linearVelocityEast << std::endl;
std::cout << "linearVelocityDown: " << linearVelocityDown << std::endl;
std::cout << "angularVelocityNorth: " << angularVelocityNorth << std::endl;
std::cout << "angularVelocityEast: " << angularVelocityEast << std::endl;
std::cout << "angularVelocityDown: " << angularVelocityDown << std::endl;
std::cout << "height: " << height << std::endl;
std::cout << "depth: " << depth << std::endl;
std::stringstream ss; std::stringstream ss;
ss << std::fixed << std::setprecision(6) << getTimeStamp() << "," ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","
<< info << "," << estimatedStateData["driveMode"].asUInt() << ","
<< referenceLon << "," << estimatedStateData["referenceLon"].asFloat() << ","
<< referenceLat << "," << estimatedStateData["referenceLat"].asFloat() << ","
<< referenceAltitude << "," << estimatedStateData["referenceAltitude"].asFloat() << ","
<< currentLon << "," << estimatedStateData["currentLon"].asFloat() << ","
<< currentLat << "," << estimatedStateData["currentLat"].asFloat() << ","
<< currentAltitude << "," << estimatedStateData["currentAltitude"].asFloat() << ","
<< offsetNorth << "," << estimatedStateData["north"].asFloat() << ","
<< offsetEast << "," << estimatedStateData["east"].asFloat() << ","
<< offsetDown << "," << estimatedStateData["depth"].asFloat() << ","
<< roll << "," << estimatedStateData["roll"].asFloat() << ","
<< pitch << "," << estimatedStateData["pitch"].asFloat() << ","
<< yaw << "," << estimatedStateData["yaw"].asFloat() << ","
<< height << "," << estimatedStateData["insVX"].asFloat() << ","
<< depth << std::endl; << 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()); Notify("uMotion_pose_log", ss.str());
} }
}
if(key == "uMission_task_fb")
{
std::string err;
Json::Value missionStatusObject;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err);
std::stringstream ss;
ss << std::fixed << std::setprecision(6) << MOOS::Time() << ","
<< missionStatusObject["state"].asInt() << ","
<< missionStatusObject["taskName"].asString() << ","
<< missionStatusObject["destName"].asString() << ","
<< missionStatusObject["errorCode"].asUInt();
Notify("uMission_task_log", ss.str());
}
}
return(true); return(true);
}
bool DataManagement::DoAsyncLog(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator q;
std::stringstream sStream;
int saveFileIndex = -1;
for(q = NewMail.begin();q!=NewMail.end();q++)
{
CMOOSMsg & rMsg = *q;
if(logVarList.find(rMsg.m_sKey)!=logVarList.end())
{
//ok we have a name at column nVar...(numerical order retained in vector)
std::map<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;
} }
//--------------------------------------------------------- //---------------------------------------------------------
@@ -144,6 +211,10 @@ bool DataManagement::OnConnectToServer()
bool DataManagement::Iterate() bool DataManagement::Iterate()
{ {
auvDataStream.flush();
missionHistoryStream.flush();
clientCommandStream.flush();
faultLogStream.flush();
return(true); 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(); RegisterVariables();
return(true); return(true);
@@ -180,15 +312,130 @@ bool DataManagement::OnStartUp()
void DataManagement::RegisterVariables() void DataManagement::RegisterVariables()
{ {
// Register("FOOBAR", 0); Register("uDevice_monitor_fb", 0);
Register("uMotion_pose_fb", 0); Register("uMission_task_fb", 0);
//Register("uMotion_pose_log", 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; os << "stamp" << ",";
gettimeofday(&tv,NULL); os << "mode" << ",";
double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000; os << "refLon" << ",";
return stamp; 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();
} }

16
src/pDataManagement/DataManagement.h
View File

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

2
src/pEmulator/Emulator.cpp
View File

@@ -2,7 +2,7 @@
* @Author: zjk 1553836110@qq.com * @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:27 * @Date: 2023-10-12 09:52:27
* @LastEditors: zjk 1553836110@qq.com * @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 * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp
* @Description: * @Description:
* *

2
src/pEmulator/Emulator.hpp
View File

@@ -2,7 +2,7 @@
* @Author: zjk 1553836110@qq.com * @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 15:57:27 * @Date: 2023-10-12 15:57:27
* @LastEditors: zjk 1553836110@qq.com * @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 * @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.hpp
* @Description: * @Description:
* *

1
src/pEmulator/_150server.hpp
View File

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

674
src/pSurfaceSupportComm/SurfaceSupportComm.cpp
View File

@@ -19,8 +19,8 @@ using namespace std;
#define TCP_RECEIVE_PORT 8000 #define TCP_RECEIVE_PORT 8000
// #define DEST_IP_ADDRESS "10.25.0.230" //树莓派 // #define DEST_IP_ADDRESS "10.25.0.230" //树莓派
// #define DEST_IP_ADDRESS "127.0.0.1" #define DEST_IP_ADDRESS "127.0.0.1"
#define DEST_IP_ADDRESS "10.25.0.163" //#define DEST_IP_ADDRESS "10.25.0.163"
//#define DEST_IP_ADDRESS "10.25.0.160" //#define DEST_IP_ADDRESS "10.25.0.160"
//--------------------------------------------------------- //---------------------------------------------------------
@@ -86,42 +86,39 @@ bool SurfaceSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
std::istringstream iss(estimatedStateString); std::istringstream iss(estimatedStateString);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err); bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err);
if (!parsingResult) { if (parsingResult)
std::cerr << "Failed to parse JSON string." << std::endl; {
return -1; estimatedState.currentLon = estimatedStateData["currentLon"].asFloat();
} estimatedState.currentLat = estimatedStateData["currentLat"].asFloat();
estimatedState.currentAltitude = estimatedStateData["currentAltitude"].asFloat();
estimatedState.referenceLon = estimatedStateData["referenceLon"].asFloat();
estimatedState.referenceLat = estimatedStateData["referenceLat"].asFloat();
estimatedState.referenceAltitude = estimatedStateData["referenceAltitude"].asFloat();
estimatedState.offsetNorth = estimatedStateData["north"].asFloat();
estimatedState.offsetEast = estimatedStateData["east"].asFloat();
estimatedState.offsetDown = estimatedStateData["depth"].asFloat();
estimatedState.roll = estimatedStateData["roll"].asFloat();
estimatedState.pitch = estimatedStateData["pitch"].asFloat();
estimatedState.yaw = estimatedStateData["yaw"].asFloat(); //弧度
estimatedState.linearVelocityNorth = estimatedStateData["dvlVX"].asFloat();
estimatedState.linearVelocityEast = estimatedStateData["dvlVY"].asFloat();
estimatedState.linearVelocityDown = estimatedStateData["dvlVZ"].asFloat();
estimatedState.height = estimatedStateData["height"].asFloat();
estimatedState.depth = estimatedStateData["depth"].asFloat();
estimatedState.currentLon = estimatedStateData["currentLon"].asFloat(); deviceStatus.batteryVoltage = estimatedStateData["batteryVoltage"].asUInt();
estimatedState.currentLat = estimatedStateData["currentLat"].asFloat(); deviceStatus.batteryLevel = estimatedStateData["batteryLevel"].asUInt();
estimatedState.currentAltitude = estimatedStateData["currentAltitude"].asFloat(); deviceStatus.batteryTemp = estimatedStateData["batteryTemp"].asFloat();
estimatedState.referenceLon = estimatedStateData["referenceLon"].asFloat(); deviceStatus.thrustRpm = estimatedStateData["rpm"].asInt();
estimatedState.referenceLat = estimatedStateData["referenceLat"].asFloat(); deviceStatus.lightEnable = estimatedStateData["lightEnable"].asUInt();
estimatedState.referenceAltitude = estimatedStateData["referenceAltitude"].asFloat(); deviceStatus.throwingLoadEnable = estimatedStateData["throwingLoadEnable"].asUInt();
estimatedState.offsetNorth = estimatedStateData["north"].asFloat(); deviceStatus.dvlStatus = estimatedStateData["dvlStatus"].asUInt();
estimatedState.offsetEast = estimatedStateData["east"].asFloat(); }
estimatedState.offsetDown = estimatedStateData["depth"].asFloat();
estimatedState.roll = estimatedStateData["roll"].asFloat();
estimatedState.pitch = estimatedStateData["pitch"].asFloat();
estimatedState.yaw = estimatedStateData["yaw"].asFloat(); //弧度
estimatedState.linearVelocityNorth = estimatedStateData["dvlVX"].asFloat();
estimatedState.linearVelocityEast = estimatedStateData["dvlVY"].asFloat();
estimatedState.linearVelocityDown = estimatedStateData["dvlVZ"].asFloat();
estimatedState.height = estimatedStateData["height"].asFloat();
estimatedState.depth = estimatedStateData["depth"].asFloat();
deviceStatus.batteryVoltage = estimatedStateData["batteryVoltage"].asUInt();
deviceStatus.batteryLevel = estimatedStateData["batteryLevel"].asUInt();
deviceStatus.batteryTemp = estimatedStateData["batteryTemp"].asFloat();
deviceStatus.thrustRpm = estimatedStateData["rpm"].asInt();
deviceStatus.lightEnable = estimatedStateData["lightEnable"].asUInt();
deviceStatus.throwingLoadEnable = estimatedStateData["throwingLoadEnable"].asUInt();
deviceStatus.dvlStatus = estimatedStateData["dvlStatus"].asUInt();
} }
if(key == "uMission_task_fb") if(key == "uMission_task_fb")
{ {
missionStatusString = msg.GetString(); missionStatusString = msg.GetString();
} }
} }
return(true); return(true);
@@ -155,14 +152,14 @@ bool SurfaceSupportComm::udpSendToServer(DUNE::IMC::Message * msg, std::string a
bool SurfaceSupportComm::Iterate() bool SurfaceSupportComm::Iterate()
{ {
// AppCastingMOOSApp::Iterate(); // AppCastingMOOSApp::Iterate();
#if 1
DUNE::IMC::Announce announceMsg; DUNE::IMC::Announce announceMsg;
announceMsg.setTimeStamp(); announceMsg.setTimeStamp();
announceMsg.setSource(header_src); announceMsg.setSource(header_src);
announceMsg.setSourceEntity(header_src_ent); announceMsg.setSourceEntity(header_src_ent);
announceMsg.setDestination(header_dst); announceMsg.setDestination(header_dst);
announceMsg.setDestinationEntity(header_dst_ent); announceMsg.setDestinationEntity(header_dst_ent);
announceMsg.sys_name.assign("lauv-150"); announceMsg.sys_name.assign(vehicleName);
announceMsg.sys_type = 2; announceMsg.sys_type = 2;
// announceMsg.owner = 65535; // announceMsg.owner = 65535;
announceMsg.lat = estimatedState.currentLat * M_PI / 180; 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+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+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+udp://10.25.0.160:6001/;imc+tcp://10.25.0.160:8000/");
#endif
#if 1
DUNE::IMC::PlanControlState planControlStateMsg; DUNE::IMC::PlanControlState planControlStateMsg;
planControlStateMsg.setTimeStamp(); planControlStateMsg.setTimeStamp();
planControlStateMsg.setSource(header_src); planControlStateMsg.setSource(header_src);
@@ -196,54 +191,51 @@ bool SurfaceSupportComm::Iterate()
std::istringstream iss(missionStatusString); std::istringstream iss(missionStatusString);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err); bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err);
if (!parsingResult) if (parsingResult)
{ {
std::cerr << "Failed to parse JSON string." << std::endl; planControlStateMsg.state = missionStatusObject["state"].asInt();
return -1; planControlStateMsg.plan_id = missionStatusObject["taskName"].asString(); //子任务名称，对应PlanDB info的[taskName]
} planControlStateMsg.plan_eta = -1; //缺省值
int planExecutingStatus = missionStatusObject["state"].asInt(); planControlStateMsg.plan_progress = -1; //缺省值
planControlStateMsg.state = planExecutingStatus; planControlStateMsg.man_id = missionStatusObject["destName"].asString(); //对应PlanDB info的[points][name]
planControlStateMsg.plan_id = missionStatusObject["taskName"].asString(); //子任务名称，对应PlanDB info的[taskName] planControlStateMsg.man_type = 450; //未知
planControlStateMsg.plan_eta = -1; //缺省值 planControlStateMsg.man_eta = -1; //缺省值
planControlStateMsg.plan_progress = -1; //缺省值 planControlStateMsg.last_outcome = missionStatusObject["errorCode"].asUInt();
planControlStateMsg.man_id = missionStatusObject["destName"].asString(); //对应PlanDB info的[points][name]
planControlStateMsg.man_type = 450; //未知
planControlStateMsg.man_eta = -1; //缺省值
planControlStateMsg.last_outcome = missionStatusObject["errorCode"].asUInt();
if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_BLOCKED) if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_BLOCKED)
{ {
//故障状态 //故障状态
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR;
}
else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_READY)
{
//没有在执行任务，等待执行
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE;
}
else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_INITIALIZING)
{
//手操状态
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_EXTERNAL;
}
else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_EXECUTING)
{
//正在执行任务
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER;
}
else
{
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE;
}
vehicleStateMsg.error_count = 0;
vehicleStateMsg.error_ents = "";
vehicleStateMsg.maneuver_type = 0xffff;
vehicleStateMsg.maneuver_stime = -1;
vehicleStateMsg.maneuver_eta = 65535;
vehicleStateMsg.control_loops = DUNE::IMC::CLoopsMask::CL_NONE;
vehicleStateMsg.flags = 0;
vehicleStateMsg.last_error = "";
vehicleStateMsg.last_error_time = -1;
} }
else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_READY)
{
//没有在执行任务，等待执行
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE;
}
else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_INITIALIZING)
{
//手操状态
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_EXTERNAL;
}
else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_EXECUTING)
{
//正在执行任务
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER;
}
else
{
vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE;
}
vehicleStateMsg.error_count = 0;
vehicleStateMsg.error_ents = "";
vehicleStateMsg.maneuver_type = 0xffff;
vehicleStateMsg.maneuver_stime = -1;
vehicleStateMsg.maneuver_eta = 65535;
vehicleStateMsg.control_loops = DUNE::IMC::CLoopsMask::CL_NONE;
vehicleStateMsg.flags = 0;
vehicleStateMsg.last_error = "";
vehicleStateMsg.last_error_time = -1;
} }
else else
{ {
@@ -268,11 +260,6 @@ bool SurfaceSupportComm::Iterate()
vehicleStateMsg.last_error_time = -1; vehicleStateMsg.last_error_time = -1;
} }
#endif
#if 1
DUNE::IMC::MsgList equipmentMsgList; DUNE::IMC::MsgList equipmentMsgList;
equipmentMsgList.setTimeStamp(); equipmentMsgList.setTimeStamp();
equipmentMsgList.setSource(header_src); equipmentMsgList.setSource(header_src);
@@ -280,9 +267,7 @@ bool SurfaceSupportComm::Iterate()
equipmentMsgList.setDestination(header_dst); equipmentMsgList.setDestination(header_dst);
equipmentMsgList.setDestinationEntity(header_dst_ent); equipmentMsgList.setDestinationEntity(header_dst_ent);
retrieveEntityStatus(equipmentMsgList); retrieveEntityStatus(equipmentMsgList);
#endif
#if 1
DUNE::IMC::EstimatedState estimatedStateMsg; DUNE::IMC::EstimatedState estimatedStateMsg;
estimatedStateMsg.setTimeStamp(); estimatedStateMsg.setTimeStamp();
estimatedStateMsg.setSource(header_src); estimatedStateMsg.setSource(header_src);
@@ -293,34 +278,27 @@ bool SurfaceSupportComm::Iterate()
estimatedStateMsg.lon = estimatedState.currentLon * M_PI / 180; estimatedStateMsg.lon = estimatedState.currentLon * M_PI / 180;
estimatedStateMsg.depth = estimatedState.depth; estimatedStateMsg.depth = estimatedState.depth;
estimatedStateMsg.height = estimatedState.height; estimatedStateMsg.height = estimatedState.height;
// estimatedStateMsg.phi = estimatedState.roll * M_PI / 180; //x axis estimatedStateMsg.phi = estimatedState.roll; //x axis radian
// estimatedStateMsg.theta = estimatedState.pitch * M_PI / 180; //y axis estimatedStateMsg.theta = estimatedState.pitch; //y axis radian
// 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
float yawTemp = estimatedState.yaw; float yawTemp = estimatedState.yaw;
if ((yawTemp >= 0) && (yawTemp <= M_PI)) if ((yawTemp >= 0) && (yawTemp <= M_PI))
{ {
estimatedStateMsg.psi = yawTemp; estimatedStateMsg.psi = yawTemp;
std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI // std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI
<< "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl; // << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl;
} }
if ((yawTemp > M_PI) && (yawTemp <= 2 * M_PI)) if ((yawTemp > M_PI) && (yawTemp <= 2 * M_PI))
{ {
estimatedStateMsg.psi = -1 * (2 * M_PI - yawTemp); estimatedStateMsg.psi = -1 * (2 * M_PI - yawTemp);
std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI // std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI
<< "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl; // << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl;
} }
estimatedStateMsg.vx = estimatedState.linearVelocityNorth; //x axis estimatedStateMsg.vx = estimatedState.linearVelocityNorth; //x axis
estimatedStateMsg.vy = estimatedState.linearVelocityEast; //y axis estimatedStateMsg.vy = estimatedState.linearVelocityEast; //y axis
estimatedStateMsg.vz = estimatedState.linearVelocityDown; //z axis estimatedStateMsg.vz = estimatedState.linearVelocityDown; //z axis
#endif
double TimeScheduled = TimeLast + 0.5; double TimeScheduled = TimeLast + 0.5;
// double TimeScheduled = TimeLast + 1.0; // double TimeScheduled = TimeLast + 1.0;
@@ -349,58 +327,41 @@ bool SurfaceSupportComm::Iterate()
double TimeNow = MOOS::Time(); double TimeNow = MOOS::Time();
TimeLast = TimeNow; TimeLast = TimeNow;
#if 1
tcpReceiveFromClient(recv_buf, sizeof(recv_buf)/sizeof(char), 0.005); tcpReceiveFromClient(recv_buf, sizeof(recv_buf)/sizeof(char), 0.005);
#endif
// AppCastingMOOSApp::PostReport(); // AppCastingMOOSApp::PostReport();
return(true); return(true);
} }
int SurfaceSupportComm::simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value) // double dfMean = 0;
{ // double dfVar = 0;
int result = -1; // unsigned int N = 0;
DUNE::IMC::EntityParameter subMsg; // bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * /*pParamCaller*/)
subMsg.name = name; // {
subMsg.value = value; // //lets wait a while
entityParameter.params.push_back(subMsg); // MOOSPause(2);
result = 0; // //some stats
return result; // double dfE = TimeNow-TimeScheduled;
} // N=N+1;
// dfMean = dfMean*(N-1)/N+(dfE)/N;
double dfMean = 0; // if(N>1)
double dfVar = 0; // dfVar = dfE/(N-1)+(N-1)/N*dfVar;
unsigned int N = 0;
bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * /*pParamCaller*/) // std::cout.setf(std::ios::fixed);
{ // std::cout<<std::setprecision(4);
//lets wait a while // std::cout<<"Timer Callback \n";
MOOSPause(2); // std::cout<<" TimeNow "<<TimeNow<<"\n";
// std::cout<<" TimeScheduled "<<TimeScheduled<<"\n";
//some stats // std::cout<<" TimeLastRun "<<TimeLastRun<<"\n";
double dfE = TimeNow-TimeScheduled; // std::cout<<" Error "<<(TimeNow-TimeScheduled)*1000.0<<"ms\n";
N=N+1; // std::cout<<" Mean Error "<<dfMean*1000.0<<"ms\n";
dfMean = dfMean*(N-1)/N+(dfE)/N; // std::cout<<" Std Error "<<std::sqrt(dfVar)*1000.0<<"ms\n";
// return true;
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;
}
// bool processMessageCallback(DUNE::IMC::Message * message) // bool processMessageCallback(DUNE::IMC::Message * message)
// { // {
@@ -473,48 +434,6 @@ bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, vo
// return true; // 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 SurfaceSupportComm::retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList)
{ {
int result = -1; int result = -1;
@@ -685,24 +604,21 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
if (!parsingResult) { if (!parsingResult) {
std::cerr << "Failed to parse JSON string." << std::endl; std::cerr << "Failed to parse JSON string." << std::endl;
return; return;
} }
behaviorSpecData["boardStamp"] = MOOS::Time(); behaviorSpecData["boardStamp"] = MOOS::Time();
BatchConvertCoordinate(behaviorSpecData); BatchConvertCoordinate(behaviorSpecData);
std::string queryMemberName = behaviorSpecData["taskName"].asString(); std::string queryMemberName = behaviorSpecData["taskName"].asString();
std::cout << queryMemberName << std::endl;
struct stat info; struct stat info;
if (stat(planConfigPath.c_str(), &info) != 0) if (stat(planConfigPath.c_str(), &info) != 0)
{ {
std::ofstream ofs;
ofs.open(planConfigPath, std::ios::out);
Json::StreamWriterBuilder builder;
Json::Value saveJsonValue; Json::Value saveJsonValue;
saveJsonValue[queryMemberName] = behaviorSpecData; saveJsonValue[queryMemberName] = behaviorSpecData;
const std::string json_file = Json::writeString(builder, saveJsonValue); Json::StreamWriterBuilder builder;
std::cout << json_file << std::endl; std::ofstream ofs;
ofs << json_file << std::endl; ofs.open(planConfigPath, std::ios::out);
ofs << Json::writeString(builder, saveJsonValue) << std::endl;
ofs.close(); ofs.close();
} }
else else
@@ -714,15 +630,19 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
taskConfigureReader.parse(ifs, tempJsonValue); taskConfigureReader.parse(ifs, tempJsonValue);
ifs.close(); ifs.close();
std::ofstream ofs;
ofs.open(planConfigPath, std::ios::out);
Json::StreamWriterBuilder builder; Json::StreamWriterBuilder builder;
tempJsonValue[queryMemberName] = behaviorSpecData; tempJsonValue[queryMemberName] = behaviorSpecData;
const std::string json_file = Json::writeString(builder, tempJsonValue); std::ofstream ofs;
std::cout << json_file << std::endl; ofs.open(planConfigPath, std::ios::out);
ofs << json_file << std::endl; ofs << Json::writeString(builder, tempJsonValue) << std::endl;
ofs.close(); 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) 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::Utils::ByteBuffer planDBBuffer;
DUNE::IMC::Packet::serialize(&planDBMsg, planDBBuffer); DUNE::IMC::Packet::serialize(&planDBMsg, planDBBuffer);
tcpSendToClient((char *)(planDBBuffer.getBuffer()), planDBMsg.getSerializationSize()); 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()) 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); DUNE::IMC::SetEntityParameters * msg = dynamic_cast<DUNE::IMC::SetEntityParameters *>(message);
printf("server receive %s: %lf\n", msg->getName(), msg->getTimeStamp()); printf("server receive %s: %lf\n", msg->getName(), msg->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter = msg->params.begin(); DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter = msg->params.begin();
std::stringstream ss2;
ss2 << std::fixed << std::setprecision(6)
<< "SetEntityParameters" << ":"
<< MOOS::Time();
for(; iter < msg->params.end(); iter++) for(; iter < msg->params.end(); iter++)
{ {
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter); DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter);
@@ -815,8 +744,10 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
std::string dataValue; std::string dataValue;
ss >> dataValue; ss >> dataValue;
printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str()); printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str());
} }
ss2 << "," << parentName << "," << childName << "," << dataValueTemp;
} }
Notify("uClient_parameterSet_log", ss2.str());
} }
if (type == DUNE::IMC::PlanControl::getIdStatic()) if (type == DUNE::IMC::PlanControl::getIdStatic())
{ {
@@ -839,7 +770,7 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
outputTaskValue["action"] = action; outputTaskValue["action"] = action;
Json::StreamWriterBuilder builder2; Json::StreamWriterBuilder builder2;
std::string outputTaskString = Json::writeString(builder2, outputTaskValue); std::string outputTaskString = Json::writeString(builder2, outputTaskValue);
std::cout << outputTaskString << std::endl; // std::cout << outputTaskString << std::endl;
if (action == "start") if (action == "start")
{ {
std::ifstream ifs; std::ifstream ifs;
@@ -852,9 +783,10 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
float originLat = tempJsonValue[msg->plan_id]["origin"]["lat"].asFloat(); float originLat = tempJsonValue[msg->plan_id]["origin"]["lat"].asFloat();
float originLon = tempJsonValue[msg->plan_id]["origin"]["lon"].asFloat(); float originLon = tempJsonValue[msg->plan_id]["origin"]["lon"].asFloat();
Json::Value outputOrginValue; Json::Value outputOrginValue;
outputOrginValue["altitude"] = originAlt; outputOrginValue["alt"] = originAlt;
outputOrginValue["lat"] = originLat; outputOrginValue["lat"] = originLat;
outputOrginValue["lon"] = originLon; outputOrginValue["lon"] = originLon;
outputOrginValue["task"] = msg->plan_id;
Json::StreamWriterBuilder builder1; Json::StreamWriterBuilder builder1;
std::string outputOriginString = Json::writeString(builder1, outputOrginValue); std::string outputOriginString = Json::writeString(builder1, outputOrginValue);
std::cout << outputOriginString << std::endl; std::cout << outputOriginString << std::endl;
@@ -866,7 +798,13 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
{ {
Notify("uMission_action_cmd", outputTaskString); 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()) if (type == DUNE::IMC::VehicleCommand::getIdStatic())
@@ -884,113 +822,259 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
Notify("uManual_enable_cmd", (double)msg->command); 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()) if (type == DUNE::IMC::RemoteActions::getIdStatic())
{ {
DUNE::IMC::RemoteActions * msg = dynamic_cast<DUNE::IMC::RemoteActions *>(message); DUNE::IMC::RemoteActions * msg = dynamic_cast<DUNE::IMC::RemoteActions *>(message);
printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->actions); printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->actions);
Notify("uManual_drive_cmd", 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(); if( NULL == dir_name )
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++)
{ {
std::cout << taskList.at(i) << std::endl; return;
Json::Value subTask = inputJsonValue[taskList.at(i)]; }
struct stat s;
lstat( dir_name , &s );
if(!S_ISDIR( s.st_mode ))
{
return;
}
struct dirent * filename;
DIR * dir;
dir = opendir( dir_name );
if( NULL == dir )
{
return;
}
while((filename = readdir(dir)) != NULL)
{
if( strcmp(filename->d_name , "." ) == 0 || strcmp(filename->d_name , "..") == 0 )
continue;
std::string taskName = subTask["taskName"].asString(); char lastChar = dir_name[strlen(dir_name) - 1];
std::string taskId = subTask["taskId"].asString(); if (std::string(1, lastChar) == std::string("/"))
int priority = subTask["priority"].asInt();
float duration = subTask["duration"].asFloat();
bool closedLoop = subTask["closedLoop"].asBool();
float constSpeed = subTask["constSpeed"].asFloat();
int repeat = subTask["repeat"].asInt();
bool perpetual = subTask["perpetual"].asBool();
float minDepth = subTask["minDepth"].asFloat();
float maxDepth = subTask["maxDepth"].asFloat();
std::string sourceName = subTask["sourceName"].asString();
std::string sourceAddress = subTask["sourceAddress"].asString();
double boardStamp = subTask["boardStamp"].asDouble();
double clientStamp = subTask["clientStamp"].asDouble();
float originLat = subTask["origin"]["lat"].asFloat();
float originLon = subTask["origin"]["lon"].asFloat();
float originAlt = subTask["origin"]["altitude"].asFloat();
Json::StreamWriterBuilder builder;
std::string subTaskString = Json::writeString(builder, subTask);
if (boardStamp > lastChangeStamp)
{Json::Value inputJsonValue;
lastChangeStamp = boardStamp;
lastChangeSourceAddress = sourceAddress;
lastChangeSourceName = sourceName;
}
std::cout << "taskName: " << taskName << std::endl;
std::cout << "taskId: " << taskId << std::endl;
std::cout << "originLon: " << originLon << std::endl;
std::cout << "originLat: " << originLat << std::endl;
std::cout << "originAlt: " << originAlt << std::endl;
std::cout << "sourceName: " << sourceName << std::endl;
std::cout << "sourceAddress: " << sourceAddress << std::endl;
std::cout << "boardStamp: " << boardStamp << std::endl;
std::cout << "clientStamp: " << clientStamp << std::endl;
std::cout << "priority: " << priority << std::endl;
std::cout << "duration: " << duration << std::endl;
std::cout << "closedLoop: " << closedLoop << std::endl;
std::cout << "constSpeed: " << constSpeed << std::endl;
std::cout << "repeat: " << repeat << std::endl;
std::cout << "perpetual: " << perpetual << std::endl;
std::cout << "minDepth: " << minDepth << std::endl;
std::cout << "maxDepth: " << maxDepth << std::endl;
if (subTask["origin"].isArray())
{ {
Json::Value array = subTask["origin"]; filePath.push_back(std::string(dir_name) + std::string(filename->d_name));
if (array.size() == 3)
{
float origin_lon = array[0].asFloat();
float origin_lat = array[1].asFloat();
float origin_alt = array[2].asFloat();
std::cout << "origin: [" << origin_lon << ", "
<< origin_lat << ", " << origin_alt << "] " << std::endl;
}
} }
else
if (subTask["points"].isArray())
{ {
Json::Value array = subTask["points"]; filePath.push_back(std::string(dir_name) + "/" + std::string(filename->d_name));
for (int j = 0; j < array.size(); j++)
{
std::string name = array[j]["name"].asString();
float lon = array[j]["lon"].asFloat();
float lat = array[j]["lat"].asFloat();
float depth = array[j]["depth"].asFloat();
float speed = array[j]["speed"].asFloat();
float north = array[j]["north"].asFloat();
float east = array[j]["east"].asFloat();
std::string type = array[j]["type"].asString();
std::cout << "point " << j << ": "
<< name << ", "
<< lon << ", "
<< lat << ", "
<< north << ", "
<< east << ", "
<< depth << ", "
<< speed << ", "
<< type << ", "
<< std::endl;
}
} }
} }
return ;
} }
void SurfaceSupportComm::BatchConvertCoordinate(Json::Value &object) void SurfaceSupportComm::BatchConvertCoordinate(Json::Value &object)
@@ -1019,14 +1103,16 @@ void SurfaceSupportComm::BatchConvertCoordinate(Json::Value &object)
void SurfaceSupportComm::closeConnection(Client& c, std::exception& e) void SurfaceSupportComm::closeConnection(Client& c, std::exception& e)
{ {
// long unsigned int client_count = m_clients.size() - 1;
m_poll.remove(*c.socket); m_poll.remove(*c.socket);
delete c.socket; delete c.socket;
} }
bool SurfaceSupportComm::OnStartUp() bool SurfaceSupportComm::OnStartUp()
{ {
// AppCastingMOOSApp::OnStartUp(); // AppCastingMOOSApp::OnStartUp();
m_MissionReader.GetValue("VehicleName", vehicleName);
planConfigPath = ""; planConfigPath = "";
m_MissionReader.GetValue("PlanConfigPath", planConfigPath); m_MissionReader.GetValue("PlanConfigPath", planConfigPath);

7
src/pSurfaceSupportComm/SurfaceSupportComm.h
View File

@@ -121,13 +121,13 @@ private:
std::vector<double> point_enu, std::vector<double> point_enu,
std::vector<double> &point_lla); std::vector<double> &point_lla);
void BatchConvertCoordinate(Json::Value &object); 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); int retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList);
template <typename Type> template <typename Type>
inline int getEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, Type& value); 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; int testFlag = 0;
double TimeLast; double TimeLast;
int sendCount = 0; int sendCount = 0;
@@ -135,6 +135,7 @@ private:
PeriodicUDPEvent udpEvent; PeriodicUDPEvent udpEvent;
// PeriodicTCPEvent tcpEvent; // PeriodicTCPEvent tcpEvent;
std::string missionStatusString; std::string missionStatusString;
std::string vehicleName;
}; };
#endif #endif

45
src/pTaskManger/TaskManger.cpp
View File

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

11
src/pTaskManger/TaskManger.h
View File

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