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

no comment

Browse Source
This commit is contained in:
zengxiaobin
2023-11-28 09:06:15 +08:00
parent 61c67ff7e2
commit 70008ee3d8
16 changed files with 506 additions and 1319 deletions
Download Patch File Download Diff File Expand all files Collapse all files

335
src/pSurfaceSupportComm/SurfaceSupportComm.cpp
View File

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

6
src/pSurfaceSupportComm/SurfaceSupportComm.h
View File

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