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

545
src/pTaskSend/TaskSend.cpp Normal file
View File

@@ -0,0 +1,545 @@
/*
* @Author: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
* @Date: 2023-09-21 11:04:00
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-10-07 10:37:30
* @FilePath: /moos-ivp-extend/src/pTaskSend/TaskSend.cpp
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
*/
/************************************************************/
/* NAME: zjk */
/* ORGN: MIT */
/* FILE: TaskSend.cpp */
/* DATE: */
/************************************************************/
#include <list>
#include <iterator>
#include "MBUtils.h"
#include "TaskSend.h"
#include <iostream>
#include <json/json.h>
using namespace std;
#define UDP_RECEIVE_PORT 8000
#define TCP_SEND_PORT 6000
#define TCP_SERVER_ADDRESS "127.0.0.1"
using namespace std;
//---------------------------------------------------------
// Constructor
TaskSend::TaskSend()
{
}
//---------------------------------------------------------
// Destructor
TaskSend::~TaskSend()
{
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool TaskSend::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
p->Trace();
// current_msg = msg;
if(p->GetName()== "SendTask")
if(p->GetString() == "true")
{
editView=true;
}
if(p->GetName()=="BHV_WARNING")
if(p->GetString() != "")
{
error = true;
}
}
return(true);
}
//---------------------------------------------------------
// Procedure: OnConnectToServer
bool TaskSend::OnConnectToServer()
{
Notify("SendTask","false");
RegisterVariables();
return(true);
}
//---------------------------------------------------------
// Procedure: Iterate()
bool TaskSend::Iterate()
{
// happens AppTick times per second
if(editView)
{
error=false;
view = taskEditView();
view->show();
// delete view;
// Fl::grab();
Fl::run(); /* 6. 运行FLTK主循环 */
cout << "editView = true" << endl;
editView=false;
}
if(error)
{
error=true;
disp1->value("parameter incorrect");
}
cout << "error=" << error << "editView" << editView << endl;
return(true);
}
//---------------------------------------------------------
// Procedure: OnStartUp()
// happens before connection is open
bool TaskSend::OnStartUp()
{
list<string> sParams;
if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
list<string>::iterator p;
for(p=sParams.begin(); p!=sParams.end(); p++) {
string original_line = *p;
string line = *p;
string param = stripBlankEnds(toupper(biteString(line, '=')));
string value = stripBlankEnds(line);
if(param == "FOO") {
//handled
}
else if(param == "BAR") {
//handled
}
}
}
RegisterVariables();
#if 1
sock_tcp_send.connect(TCP_SERVER_ADDRESS, TCP_SEND_PORT);
sock_tcp_send.setKeepAlive(true);
#endif
return(true);
}
std::string TaskSend::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 = {-70.328891,43.824429, 10, 3};
struct Landmark station_2 = {-70.327885,43.824676, 8, 5};
struct Landmark station_3 = {-70.327867,43.823622, 6, 7};
struct Landmark station_4 = {-70.328765,43.823622, 4, 9};
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.repeate = -1;
behavior.closedLoop = true;
behavior.perpetual = true;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["name"] = behavior.name;
behaviorConfig["source"] = behavior.source;
behaviorConfig["client stamp"] = stamp;
behaviorConfig["board stamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeate"] = behavior.repeate;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
Json::Value station;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
behaviorConfig["points"].append(station);
behaviorConfig["client stamp"] = stamp;
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
std::string TaskSend::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 = {-70.331532,43.824194, 9, 4};
struct Landmark station_2 = {-70.330328,43.824299, 7, 6};
struct Landmark station_3 = {-70.330346,43.823518, 5, 8};
struct Landmark station_4 = {-70.331406,43.823206, 3, 10};
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.repeate = -1;
behavior.closedLoop = true;
behavior.perpetual = true;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["name"] = behavior.name;
behaviorConfig["source"] = behavior.source;
behaviorConfig["client stamp"] = stamp;
behaviorConfig["board stamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeate"] = behavior.repeate;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
Json::Value station;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
behaviorConfig["points"].append(station);
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
std::string TaskSend::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 = {-70.328891,43.824429, 9, 2};
struct Landmark station_2 = {-70.327885,43.824676, 7, 4};
struct Landmark station_3 = {-70.327867,43.823622, 5, 6};
struct Landmark station_4 = {-70.328765,43.823622, 3, 8};
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.repeate = -1;
behavior.closedLoop = true;
behavior.perpetual = true;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["name"] = behavior.name;
behaviorConfig["source"] = behavior.source;
behaviorConfig["client stamp"] = stamp;
behaviorConfig["board stamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeate"] = behavior.repeate;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
Json::Value station;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
behaviorConfig["points"].append(station);
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
behaviorConfig["points"].append(station);
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
//---------------------------------------------------------
// Procedure: RegisterVariables
void TaskSend::RegisterVariables()
{
Register("SendTask",0);
Register("BHV_WARNING",0);
// Register("FOOBAR", 0);
}
Fl_Window * TaskSend::taskEditView()
{
Fl_Window *window = new Fl_Window(405, 600, "task config");
// Fl_Double_Window *window(450, 350, "Simple Table"); /* 1. 创建一个窗口 */
Fl_Group* pGroup = new Fl_Group(0, 0, 400, 70); /* 2. 创建一个分组 */
pGroup->box(FL_GTK_UP_BOX);
Fl_Button* Button_1 = new Fl_Button(5, 5, 90, 30, "send task 1"); //发送任务
Button_1->callback(st_sendTask1Callback, (void*) this);
Fl_Button* Button_2 = new Fl_Button(105, 5, 90, 30, "send task 2"); //发送任务
Button_2->callback(st_sendTask2Callback, (void*) this);
Fl_Button* Button_3 = new Fl_Button(205, 5, 90, 30, "modify task 1"); //发送任务
Button_3->callback(st_ModifyTask1Callback, (void*) this);
Fl_Button* Button_4 = new Fl_Button(295, 5, 90, 30, "load task");
Button_4->callback(st_FeedbackTaskCallback, (void*) this);
Fl_Output* TaskMsgDisp = new Fl_Output(5,35,390,30);
TaskMsgDisp->box(FL_FLAT_BOX);
disp1 = TaskMsgDisp;
pGroup->end(); /* 4. 结束上个容器的创建 */
Fl_Group* pGroup1 = new Fl_Group(0, 90, 400, 70,"task parameter"); /* 2. 创建一个分组 */
pGroup1->box(FL_GTK_UP_BOX);
Fl_Menu_Button* taskType =new Fl_Menu_Button(5,95,100,25,"task type");
taskType->add("path tracking");
taskType->add("fixed yaw");
taskType->add("fixed depth");
taskType->add("fixed speed");
// taskType->add("");
Fl_Input* pwt = new Fl_Input(200,90,100,30,"task priority");
pwt->value("100");
Fl_Input* duration = new Fl_Input(200,35+90,100,30,"task duration");
duration->value("no-time-limit");
// pGroup1->add(taskType);
pGroup1->end();
Fl_Group* pGroup2 = new Fl_Group(0, 180, 400, 300,"path tracking parameter");
pGroup2->box(FL_GTK_UP_BOX);
pGroup2->add(capture_radius);
capture_radius->value("5");
pGroup2->add(capture_line);
capture_line->value("true");
pGroup2->add(slip_radius);
slip_radius->value("15.0");
pGroup2->add(lead);
lead->value("8");
pGroup2->add(lead_damper);
lead_damper->value("1");
pGroup2->add(speed);
speed->value("10.0");
pGroup2->add(repeat);
repeat->value("3");
pGroup2->add(polygon);
pGroup2->end();
window->end(); /* 4. 结束上个容器的创建 */
// window->show(); /* 5. 显示窗口 */
return window;
// delete window;
}
inline int TaskSend::sendTaskCallback(Fl_Widget *w)
{
disp1->value("Task Sending...");
Notify("TaskNum","t1");
string taskMsg = "WPT_UPDATE";
string taskMsgconit="";
// if(polygon->value()=="")
// disp1->value(polygon->value());
taskMsgconit = "capture_radius=";
taskMsgconit += capture_radius->value();
Notify(taskMsg,taskMsgconit);
taskMsgconit = "capture_line=";
taskMsgconit += capture_line->value();
Notify(taskMsg,taskMsgconit);
taskMsgconit = "slip_radius=";
taskMsgconit += slip_radius->value();
Notify(taskMsg,taskMsgconit);
taskMsgconit = "lead=";
taskMsgconit += lead->value();
Notify(taskMsg,taskMsgconit);
taskMsgconit = "lead_damper=";
taskMsgconit += lead_damper->value();
Notify(taskMsg,taskMsgconit);
taskMsgconit = "speed=";
taskMsgconit += speed->value();
Notify(taskMsg,taskMsgconit);
taskMsgconit = "repeat=";
taskMsgconit += repeat->value();
Notify(taskMsg,taskMsgconit);
taskMsgconit = "polygon=";
taskMsgconit += polygon->value();
Notify(taskMsg,taskMsgconit);
if(error)
disp1->value("configure incorrect");
else
disp1->value("Task Send complete");
// disp1->value("Task Send complete");
}
inline int TaskSend::sendTask1Callback(Fl_Widget *w)
{
std::string systemName = "neptus-client-1";
std::string plan_1_Spec = SetPlan1(systemName, getTimeStamp());
std::cout << plan_1_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_1_Spec;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
inline int TaskSend::sendTask2Callback(Fl_Widget *w)
{
std::string systemName = "neptus-client-1";
std::string plan_2_Spec = SetPlan2(systemName, getTimeStamp());
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);
}
inline int TaskSend::ModifyTask1Callback(Fl_Widget *w)
{
std::string systemName = "neptus-client-1";
std::string plan_1_Spec = ModifyPlan1(systemName, getTimeStamp());
std::cout << plan_1_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_1_Spec;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
inline int TaskSend::FeedbackTaskCallback(Fl_Widget *w)
{
DUNE::IMC::PlanDB msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_GET_STATE;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
bool TaskSend::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;
}
double TaskSend::getTimeStamp()
{
struct timeval tv;
gettimeofday(&tv,NULL);
double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000;
return stamp;
}