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

迁移分支

Browse Source
This commit is contained in:
zengxiaobin
2023-11-24 17:09:26 +08:00
commit a6919ec672
158 changed files with 30851 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

873
src/pClientViewer/ClientViewer.cpp Normal file
View File

@@ -0,0 +1,873 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: ClientViewer.cpp */
/* DATE: */
/************************************************************/
#include <iterator>
#include "ClientViewer.h"
//#include "Behavior.pb.h"
#include <json/json.h>
using namespace std;
#define UDP_RECEIVE_PORT 6001
#define TCP_SEND_PORT 8000
#define TCP_FILE_RECEIVE_PORT 8001
//#define TCP_SERVER_ADDRESS "10.25.0.230" //树莓派
#define TCP_SERVER_ADDRESS "127.0.0.1"
// #define TCP_SERVER_ADDRESS "10.25.0.163"
// #define TCP_SERVER_ADDRESS "10.25.0.160"
//---------------------------------------------------------
// Constructor
ClientViewer::ClientViewer()
{
udpReceiveBuffer = new uint8_t[65535];
tcpReceiveBuffer = new uint8_t[65535];
}
//---------------------------------------------------------
// Destructor
ClientViewer::~ClientViewer()
{
// udpCommEvent.Stop();
// tcpCommEvent.Stop();
delete udpReceiveBuffer;
delete tcpReceiveBuffer;
// if (sock_tcp_send)
// {
// delete sock_tcp_send;
// sock_tcp_send = NULL;
// }
// if (sock_udp_receive)
// {
// delete sock_udp_receive;
// sock_udp_receive = NULL;
// }
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
string key = msg.GetKey();
string comm = msg.GetCommunity();
double dval = msg.GetDouble();
string sval = msg.GetString();
string msrc = msg.GetSource();
double mtime = msg.GetTime();
bool mdbl = msg.IsDouble();
bool mstr = msg.IsString();
std::cout << key << " : " << sval << std::endl;
if(key == "Command")
{
if (sval == "SetPlan1") //PlanDB
{
std::string systemName = "CCU Neptus 0_163";
std::string plan_1_Spec = SetPlan1(systemName, MOOS::Time());
DUNE::IMC::PlanDB msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
msg.plan_id.assign("BHV_Waypoint");
msg.info = plan_1_Spec;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "SetPlan2") //PlanDB
{
std::string systemName = "CCU Neptus 0_163";
std::string plan_2_Spec = SetPlan2(systemName, MOOS::Time());
//std::cout << plan_2_Spec << std::endl;
DUNE::IMC::PlanDB msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
msg.plan_id.assign("BHV_Waypoint");
msg.info = plan_2_Spec;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "ModifyPlan1") //PlanDB
{
std::string systemName = "CCU Neptus 0_163";
std::string plan_1_Spec = ModifyPlan1(systemName, MOOS::Time());
DUNE::IMC::PlanDB msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_SET;
msg.plan_id.assign("BHV_Waypoint");
msg.info = plan_1_Spec;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "GetPlanList") //PlanDB
{
DUNE::IMC::PlanDB msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_GET_STATE;
msg.plan_id.assign("Test");
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "SetThrust") //SetEntityParameters
{
double timeStamp = MOOS::Time();
DUNE::IMC::SetEntityParameters msg;
msg.setTimeStamp(timeStamp);
msg.name = "Thrust";
DUNE::IMC::EntityParameter subMsg;
subMsg.setTimeStamp(timeStamp);
SetEntityStatus(subMsg, "Pwm", generateEntityValue("30", "int"));
msg.params.push_back(subMsg);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "SetIndicatorLight") //SetEntityParameters
{
double timeStamp = MOOS::Time();
DUNE::IMC::SetEntityParameters msg;
msg.setTimeStamp(timeStamp);
msg.name = "IndicatorLight";
DUNE::IMC::EntityParameter subMsg;
subMsg.setTimeStamp(timeStamp);
SetEntityStatus(subMsg, "Status", generateEntityValue("1", "int"));
msg.params.push_back(subMsg);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "StartPlan1") //PlanControl
{
DUNE::IMC::PlanControl msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;
msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_START;
msg.plan_id = "east_waypt_survey";
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "StartPlan2") //PlanControl
{
DUNE::IMC::PlanControl msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;
msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_START;
msg.plan_id = "west_waypt_survey";
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "StopPlan1") //PlanControl
{
DUNE::IMC::PlanControl msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;
msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_STOP;
msg.plan_id = "east_waypt_survey";
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "StopPlan2") //PlanControl
{
DUNE::IMC::PlanControl msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;
msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_STOP;
msg.plan_id = "west_waypt_survey";
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "RemoteActions") //PlanControl
{
DUNE::IMC::RemoteActions msg;
msg.setTimeStamp();
Json::Value executeCommand;
executeCommand["Thrust"] = -100;
executeCommand["Heading"] = -27.5;
Json::StreamWriterBuilder builder;
msg.actions = Json::writeString(builder, executeCommand);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "VehicleCommand") //PlanControl
{
DUNE::IMC::VehicleCommand msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST;
msg.command = 1;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "GetLogBook") //PlanControl
{
DUNE::IMC::LogBookControl msg;
msg.setTimeStamp();
msg.command = DUNE::IMC::LogBookControl::CommandEnum::LBC_GET;
DUNE::IMC::LogBookEntry msgLogBookEntry;
msgLogBookEntry.type = DUNE::IMC::LogBookEntry::TypeEnum::LBET_INFO;
msgLogBookEntry.context = "LIST";
msg.msg.push_back(msgLogBookEntry);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "RetrLogBook1") //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["lauv-150/2023-11-16/103058"]["auv"]["file"] = "auvData.mdat";
Json::StreamWriterBuilder builder;
msgLogBookEntry.text = Json::writeString(builder, retrFile);
msg.msg.push_back(msgLogBookEntry);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "RetrLogBook2") //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["lauv-150/2023-11-16/103058"]["command"]["file"] = "clientCommand.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 == "RetrLogBook3") //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["lauv-150/2023-11-16/103058"]["mission"]["file"] = "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["lauv-150/2023-11-16/103058"]["auv"]["file"] = "auvData.mdat";
retrFile["lauv-150/2023-11-16/103058"]["command"]["file"] = "clientCommand.txt";
retrFile["lauv-150/2023-11-16/105131"]["mission"]["file"] = "missionHistory.txt";
Json::StreamWriterBuilder builder;
msgLogBookEntry.text = Json::writeString(builder, retrFile);
msg.msg.push_back(msgLogBookEntry);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
}
}
return(true);
}
//---------------------------------------------------------
// Procedure: OnConnectToServer
bool ClientViewer::OnConnectToServer()
{
RegisterVariables();
return(true);
}
bool ClientViewer::Iterate()
{
DUNE::IMC::Message * receiveUDPMessage;
while ((receiveUDPMessage = imcPollUDP()) != NULL)
{
processMessage(receiveUDPMessage);
free(receiveUDPMessage);
}
DUNE::IMC::Message * receiveTCPMessage;
while ((receiveTCPMessage = imcPollTCP()) != NULL)
{
processMessage(receiveTCPMessage);
free(receiveTCPMessage);
}
return(true);
}
// bool processMessageCallback(DUNE::IMC::Message * message)
// {
// int type = message->getId();
// if (type == DUNE::IMC::Announce::getIdStatic())
// {
// DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
// // printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// // msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
// printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
// }
// if (type == DUNE::IMC::PlanDB::getIdStatic())
// {
// DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
// printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
// printf("%s\n", msg->info.c_str());
// }
// if (type == DUNE::IMC::PlanControlState::getIdStatic())
// {
// DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
// printf("server receive %s: %lf, %s, %s, %u\n", \
// msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
// }
// if (type == DUNE::IMC::MsgList::getIdStatic())
// {
// DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
// printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
// DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
// for (; iter1 != msgList->msgs.end(); ++iter1)
// {
// DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
// DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
// for (; iter2 != entityParameter->params.end(); ++iter2)
// {
// DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
// std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;
// }
// }
// }
// if (type == DUNE::IMC::EstimatedState::getIdStatic())
// {
// DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
// // printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// // msg->getName(), msg->getTimeStamp(),
// // msg->lat, msg->lon, msg->depth,
// // msg->phi, msg->theta, msg->psi);
// printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// msg->getName(), msg->getTimeStamp(),
// msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
// msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
// }
// if (type == DUNE::IMC::VehicleState::getIdStatic())
// {
// DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
// printf("server receive %s: %lf, %u\n",
// msg->getName(), msg->getTimeStamp(), msg->op_mode);
// }
// if (type == DUNE::IMC::LogBookControl::getIdStatic())
// {
// DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
// printf("server receive %s: %lf, %u\n",
// msg->getName(), msg->getTimeStamp(), msg->command);
// DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
// for (; iter != msg->msg.end(); ++iter)
// {
// DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast<DUNE::IMC::LogBookEntry *>(*iter);
// std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->text << std::endl;
// }
// }
// return true;
// }
//---------------------------------------------------------
// Procedure: OnStartUp()
// happens before connection is open
bool ClientViewer::OnStartUp()
{
initLon = -70.330400;
initLat = 43.825300;
initAlt = 0;
// udpCommEvent.SetPeriod(1);
// udpCommEvent.SetCallback(processMessageCallback,NULL);
// udpCommEvent.Start();
// tcpCommEvent.SetPeriod(1);
// tcpCommEvent.SetCallback(processMessageCallback,NULL);
// tcpCommEvent.Start();
sock_udp_receive.bind(UDP_RECEIVE_PORT, DUNE::Network::Address::Any, false);
m_poll_0.add(sock_udp_receive);
try
{
sock_tcp_send.connect(TCP_SERVER_ADDRESS, TCP_SEND_PORT);
sock_tcp_send.setKeepAlive(true);
m_poll_1.add(sock_tcp_send);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
char local_ip2[INET_ADDRSTRLEN] = {0};
get_local_ip_using_create_socket(local_ip2);
ethernetIP = local_ip2;
RegisterVariables();
return(true);
}
std::string ClientViewer::SetPlan1(std::string sourceName, double stamp)
{
struct WayPointBehavior behavior;
behavior.source = sourceName;
behavior.priority = -1;
behavior.points.clear();
behavior.name = "east_waypt_survey";
behavior.priority = 10;
struct Landmark station_1 = {"station_1", -70.328891,43.824429, 10, 3, -1, -1, "point"};
struct Landmark station_2 = {"station_2", -70.327885,43.824676, 8, 5, -1, -1, "point"};
struct Landmark station_3 = {"station_3", -70.327867,43.823622, 6, 7, -1, -1, "point"};
struct Landmark station_4 = {"station_4", -70.328765,43.823622, 4, 9, -1, -1, "point"};
behavior.points.push_back(station_1);
behavior.points.push_back(station_2);
behavior.points.push_back(station_3);
behavior.points.push_back(station_4);
behavior.duration = -1;
behavior.constSpeed = -1;
behavior.repeat = 1;
behavior.closedLoop = true;
behavior.perpetual = false;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["taskName"] = behavior.name;
behaviorConfig["taskId"] = "1";
behaviorConfig["sourceName"] = behavior.source;
behaviorConfig["sourceAddress"] = ethernetIP;
behaviorConfig["clientStamp"] = stamp;
behaviorConfig["boardStamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeat"] = behavior.repeat;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
behaviorConfig["origin"]["lon"] = initLon;
behaviorConfig["origin"]["lat"] = initLat;
behaviorConfig["origin"]["altitude"] = initAlt;
Json::Value station;
station["name"] = station_1.name;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
station["north"] = station_1.north;
station["east"] = station_1.east;
station["type"] = station_1.type;
behaviorConfig["points"].append(station);
station["name"] = station_2.name;
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
station["north"] = station_2.north;
station["east"] = station_2.east;
station["type"] = station_2.type;
behaviorConfig["points"].append(station);
station["name"] = station_3.name;
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
station["north"] = station_3.north;
station["east"] = station_3.east;
station["type"] = station_3.type;
behaviorConfig["points"].append(station);
station["name"] = station_4.name;
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
station["north"] = station_4.north;
station["east"] = station_4.east;
station["type"] = station_4.type;
behaviorConfig["points"].append(station);
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
std::string ClientViewer::SetPlan2(std::string sourceName, double stamp)
{
struct WayPointBehavior behavior;
behavior.source = sourceName;
behavior.priority = -1;
behavior.points.clear();
behavior.name = "west_waypt_survey";
behavior.priority = 10;
struct Landmark station_1 = {"station_1", -70.331532,43.824194, 9, 4, -1, -1, "track"};
struct Landmark station_2 = {"station_2", -70.330328,43.824299, 7, 6, -1, -1, "track"};
struct Landmark station_3 = {"station_3", -70.330346,43.823518, 5, 8, -1, -1, "track"};
struct Landmark station_4 = {"station_4", -70.331406,43.823206, 3, 10, -1, -1, "track"};
behavior.points.push_back(station_1);
behavior.points.push_back(station_2);
behavior.points.push_back(station_3);
behavior.points.push_back(station_4);
behavior.duration = -1;
behavior.constSpeed = -1;
behavior.repeat = -1;
behavior.closedLoop = true;
behavior.perpetual = true;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["taskName"] = behavior.name;
behaviorConfig["taskId"] = "2";
behaviorConfig["sourceName"] = behavior.source;
behaviorConfig["sourceAddress"] = ethernetIP;
behaviorConfig["clientStamp"] = stamp;
behaviorConfig["boardStamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeat"] = behavior.repeat;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
behaviorConfig["origin"]["lon"] = initLon;
behaviorConfig["origin"]["lat"] = initLat;
behaviorConfig["origin"]["altitude"] = initAlt;
Json::Value station;
station["name"] = station_1.name;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
station["north"] = station_1.north;
station["east"] = station_1.east;
station["type"] = station_1.type;
behaviorConfig["points"].append(station);
station["name"] = station_2.name;
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
station["north"] = station_2.north;
station["east"] = station_2.east;
station["type"] = station_2.type;
behaviorConfig["points"].append(station);
station["name"] = station_3.name;
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
station["north"] = station_3.north;
station["east"] = station_3.east;
station["type"] = station_3.type;
behaviorConfig["points"].append(station);
station["name"] = station_4.name;
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
station["north"] = station_4.north;
station["east"] = station_4.east;
station["type"] = station_4.type;
behaviorConfig["points"].append(station);
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
std::string ClientViewer::ModifyPlan1(std::string sourceName, double stamp)
{
struct WayPointBehavior behavior;
behavior.source = sourceName;
behavior.priority = -1;
behavior.points.clear();
behavior.name = "east_waypt_survey";
behavior.priority = 10;
struct Landmark station_1 = {"station_1", -70.328891,43.824429, 9, 2, -1, -1, "track"};
struct Landmark station_2 = {"station_2", -70.327885,43.824676, 7, 4, -1, -1, "point"};
struct Landmark station_3 = {"station_3", -70.327867,43.823622, 5, 6, -1, -1, "track"};
struct Landmark station_4 = {"station_4", -70.328765,43.823622, 3, 8, -1, -1, "point"};
behavior.points.push_back(station_1);
behavior.points.push_back(station_2);
behavior.points.push_back(station_3);
behavior.points.push_back(station_4);
behavior.duration = -1;
behavior.constSpeed = -1;
behavior.repeat = 3;
behavior.closedLoop = true;
behavior.perpetual = false;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["taskName"] = behavior.name;
behaviorConfig["taskId"] = "1";
behaviorConfig["sourceName"] = behavior.source;
behaviorConfig["sourceAddress"] = ethernetIP;
behaviorConfig["clientStamp"] = stamp;
behaviorConfig["boardStamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeat"] = behavior.repeat;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
behaviorConfig["origin"]["lon"] = initLon;
behaviorConfig["origin"]["lat"] = initLat;
behaviorConfig["origin"]["altitude"] = initAlt;
Json::Value station;
station["name"] = station_1.name;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
station["north"] = station_1.north;
station["east"] = station_1.east;
station["type"] = station_1.type;
behaviorConfig["points"].append(station);
station["name"] = station_2.name;
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
station["north"] = station_2.north;
station["east"] = station_2.east;
station["type"] = station_2.type;
behaviorConfig["points"].append(station);
station["name"] = station_3.name;
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
station["north"] = station_3.north;
station["east"] = station_3.east;
station["type"] = station_3.type;
behaviorConfig["points"].append(station);
station["name"] = station_4.name;
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
station["north"] = station_4.north;
station["east"] = station_4.east;
station["type"] = station_4.type;
behaviorConfig["points"].append(station);
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
DUNE::IMC::Message * ClientViewer::imcPollUDP()
{
if (m_poll_0.poll(0))
{
DUNE::Network::Address addr;
uint16_t rv = sock_udp_receive.read(udpReceiveBuffer, 65535, &addr);
DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(udpReceiveBuffer, rv);
return msg;
}
return NULL;
}
DUNE::IMC::Message * ClientViewer::imcPollTCP()
{
if (m_poll_1.poll(0))
{
uint16_t rv = sock_tcp_send.read(tcpReceiveBuffer, 65535);
DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(tcpReceiveBuffer, rv);
return msg;
}
return NULL;
}
bool ClientViewer::tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port)
{
DUNE::Utils::ByteBuffer bb;
try {
DUNE::IMC::Packet::serialize(msg, bb);
return sock_tcp_send.write(bb.getBuffer(), msg->getSerializationSize());
}
catch (std::runtime_error& e)
{
MOOSTrace ("ERROR sending %s to %s:%d: %s\n", msg->getName(), addr.c_str(), port, e.what());
return false;
}
return true;
}
void ClientViewer::processMessage(DUNE::IMC::Message * message) {
int type = message->getId();
if (type == DUNE::IMC::Announce::getIdStatic())
{
DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
printf("server receive %s: %lf, %lf, %lf, %lf\n", \
msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
}
if (type == DUNE::IMC::PlanDB::getIdStatic())
{
DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
printf("%s\n", msg->info.c_str());
}
if (type == DUNE::IMC::PlanControlState::getIdStatic())
{
DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
printf("server receive %s: %lf, %s, %s, %u\n", \
msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
}
if (type == DUNE::IMC::MsgList::getIdStatic())
{
DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
for (; iter1 != msgList->msgs.end(); ++iter1)
{
DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
for (; iter2 != entityParameter->params.end(); ++iter2)
{
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;
}
}
}
if (type == DUNE::IMC::EstimatedState::getIdStatic())
{
DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
msg->getName(), msg->getTimeStamp(),
msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
}
if (type == DUNE::IMC::VehicleState::getIdStatic())
{
DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
printf("server receive %s: %lf, %u\n",
msg->getName(), msg->getTimeStamp(), msg->op_mode);
}
if (type == DUNE::IMC::LogBookControl::getIdStatic())
{
DUNE::IMC::LogBookControl * msg = dynamic_cast<DUNE::IMC::LogBookControl *>(message);
printf("server receive %s: %lf, %u\n",
msg->getName(), msg->getTimeStamp(), msg->command);
DUNE::IMC::MessageList<DUNE::IMC::LogBookEntry>::const_iterator iter = msg->msg.begin();
for (; iter != msg->msg.end(); ++iter)
{
DUNE::IMC::LogBookEntry *msgLogBookEntry = static_cast<DUNE::IMC::LogBookEntry *>(*iter);
std::cout << msgLogBookEntry->type << ", " << msgLogBookEntry->context << ", " << msgLogBookEntry->text << std::endl;
}
}
}
//---------------------------------------------------------
// Procedure: RegisterVariables
void ClientViewer::RegisterVariables()
{
Register("Command", 0);
Register("Key", 0);
Register("Value", 0);
}
int ClientViewer::SetEntityStatus(DUNE::IMC::EntityParameter& subMsg, std::string name, std::string value)
{
int result = -1;
subMsg.name = name;
subMsg.value = value;
result = 0;
return result;
}
std::string ClientViewer::generateEntityName(std::string parentName, std::string childName)
{
Json::Value outputJsonValue;
outputJsonValue["parent"] = parentName;
outputJsonValue["child"] = childName;
Json::StreamWriterBuilder builder;
std::string outputJsonString = Json::writeString(builder, outputJsonValue);
return outputJsonString;
}
std::string ClientViewer::generateEntityValue(std::string value, std::string type)
{
Json::Value outputJsonValue;
outputJsonValue["value"] = value;
outputJsonValue["type"] = type;
Json::StreamWriterBuilder builder;
std::string outputJsonString = Json::writeString(builder, outputJsonValue);
return outputJsonString;
}
int ClientViewer::get_local_ip_using_create_socket(char* str_ip)
{
int status = -1;
int af = AF_INET;
int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
struct sockaddr_in remote_addr;
struct sockaddr_in local_addr;
char *local_ip = NULL;
socklen_t len = 0;
remote_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(53);
remote_addr.sin_addr.s_addr = inet_addr("1.1.1.1");
len = sizeof(struct sockaddr_in);
status = connect(sock_fd, (struct sockaddr*)&remote_addr, len);
if(status != 0 ){
printf("connect err \n");
}
len = sizeof(struct sockaddr_in);
getsockname(sock_fd, (struct sockaddr*)&local_addr, &len);
local_ip = inet_ntoa(local_addr.sin_addr);
if(local_ip)
{
strcpy(str_ip, local_ip);
status = 0;
}
return status;
}