874 lines
32 KiB
C++
Executable File
874 lines
32 KiB
C++
Executable File
/************************************************************/
|
|
/* 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;
|
|
}
|