From 9db47019cc8baefaf3bc14371a19b1989e526df3 Mon Sep 17 00:00:00 2001 From: jhl Date: Wed, 29 Nov 2023 17:47:03 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=8B=E6=93=8D=E6=8E=A7=E5=88=B6=E6=B5=8B?= =?UTF-8?q?=E8=AF=95=E5=AE=8C=E6=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/launch_local.moos | 24 +++++------ setting/Origin.json | 2 +- src/pBoardSupportComm/BoardSupportComm.cpp | 40 ++++++++----------- src/pEmulator/Emulator.cpp | 2 +- src/pStateManagement/StateManagement.cpp | 12 +++--- .../SurfaceSupportComm.cpp | 37 +++++++++-------- src/pSurfaceSupportComm/SurfaceSupportComm.h | 2 + 7 files changed, 57 insertions(+), 62 deletions(-) diff --git a/launch/launch_local.moos b/launch/launch_local.moos index ec833e9..4a05c25 100644 --- a/launch/launch_local.moos +++ b/launch/launch_local.moos @@ -18,16 +18,14 @@ AltOrigin = 0 VehicleName = lauv-150 LogEnable = false -//LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/ -LogDir = /home/ben/project/moos-ivp-pi/log/ +LogDir = /home/jhl/project/moos-ivp-pi/log/ AuvDataLog = auvData.mdat MissionHistoryLog = missionHistory.txt ClientCommandLog = clientCommand.txt FaultLog = faultLog.txt MotionControlLog = motionControl.txt -//llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json -llaOriginPath = /home/ben/project/moos-ivp-pi/setting/Origin.json +llaOriginPath = /home/jhl/project/moos-ivp-pi/setting/Origin.json //------------------------------------------ // Antler configuration block @@ -40,20 +38,20 @@ ProcessConfig = ANTLER Run = pNodeReporter @ NewConsole = false Run = uProcessWatch @ NewConsole = false Run = pRealm @ NewConsole = false - //Run = pShare @ NewConsole = false - Run = pMarineViewer @ NewConsole = false + Run = pShare @ NewConsole = false + //Run = pMarineViewer @ NewConsole = false //===========Our define process==================== Run = pBoardSupportComm @ NewConsole = false Run = pTaskManagement @ NewConsole = false - //Run = pMotionControler @ NewConsole = false + Run = pMotionControler @ NewConsole = false Run = pSurfaceSupportComm @ NewConsole = false Run = pDataManagement @ NewConsole = false Run = pFaultHandle @ NewConsole = false Run = pStateManagement @ NewConsole = false //===============For test process=================== - //Run = pEmulator @ NewConsole = false - Run = uSimMarine @ NewConsole = false - Run = pMarinePID @ NewConsole = false + Run = pEmulator @ NewConsole = false + //Run = uSimMarine @ NewConsole = false + //Run = pMarinePID @ NewConsole = false } @@ -211,8 +209,7 @@ ProcessConfig = pTaskManagement { AppTick = 8 CommsTick = 8 - //planConfigPath = /home/zjk/Desktop/project/moos-ivp-pi/setting/PlanConfigure.json - planConfigPath = /home/ben/project/moos-ivp-pi/setting/PlanConfigure.json + planConfigPath = /home/jhl/project/moos-ivp-pi/setting/PlanConfigure.json } @@ -275,8 +272,7 @@ ProcessConfig = pMotionControler cheak_stalensee = true delta_freqency = 5 - //config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/ControlParam.json - config_file = /home/ben/project/moos-ivp-pi/setting/ControlParam.json + config_file = /home/jhl/project/moos-ivp-pi/setting/ControlParam.json } ProcessConfig = pEmulator diff --git a/setting/Origin.json b/setting/Origin.json index 5822f5c..1cf6e0d 100644 --- a/setting/Origin.json +++ b/setting/Origin.json @@ -1,5 +1,5 @@ { - "AltOrigin" : 0, + "AltOrigin" : 0.0, "LatOrigin" : 50.825298309326172, "LongOrigin" : -90.330398559570312, "TaskName" : "east_waypt_survey" diff --git a/src/pBoardSupportComm/BoardSupportComm.cpp b/src/pBoardSupportComm/BoardSupportComm.cpp index 8dc768a..acaefcd 100644 --- a/src/pBoardSupportComm/BoardSupportComm.cpp +++ b/src/pBoardSupportComm/BoardSupportComm.cpp @@ -13,9 +13,9 @@ #define TCP_RECEIVE_PORT 8001 #define TCP_SERVER_ADDRESS "127.0.0.1" -// #define MOOS_AUV_SIM -//#define MATLAB_AUV_SIM -#define TRUE_AUV +//#define MOOS_AUV_SIM +#define MATLAB_AUV_SIM +//#define TRUE_AUV using namespace std; @@ -57,7 +57,7 @@ BoardSupportComm::BoardSupportComm() executeCommand.header = 0xEBA2; //1:[0,1] executeCommand.count = 16; //2:[2,3] executeCommand.size = 21; //3:[4] - executeCommand.drive_mode = 0xFF; //4:[5] + executeCommand.drive_mode = 0x02; //4:[5] executeCommand.thrust = 0; //5:[6] executeCommand.yaw = 0; //6:[7,8] executeCommand.depth = 0; //7:[9,10] @@ -180,15 +180,13 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) { if (sval == "true") { - executeCommand.drive_mode = 0x02; executeCommand.manual_mode = true; } else { - executeCommand.drive_mode = 0xFF; executeCommand.manual_mode = false; - } + } } if(key == "uManual_drive_cmd") @@ -215,26 +213,21 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail) return false; } executeCommand.thrust = convertIntToUchar(recvCommand["Thrust"].asInt(), -100, 100); - float heading = recvCommand["Heading"].asFloat(); - if ((heading >= 0) && (heading <= 180)) - { - executeCommand.yaw = heading * 10; - } - - if ((heading < 0) && (heading >= -180)) - { - executeCommand.yaw = (360 + heading) * 10; - } - if ((heading > 180) or (heading < -180)) - { - executeCommand.yaw = 180 * 10; - } + uint8_t helm_top_bottom_angle = convertIntToUchar((int)(recvCommand["Heading"].asFloat()), -30, 30); executeCommand.depth = 0; - executeCommand.helm_top_angle = 0; - executeCommand.helm_bottom_angle = 0; + executeCommand.helm_top_angle = helm_top_bottom_angle; + executeCommand.helm_bottom_angle = helm_top_bottom_angle; executeCommand.helm_left_angle = 0; executeCommand.helm_right_angle = 0; + executeCommand.yaw = 0; + int serializeResult = serializeMessage(tcpSendBuffer); + std::stringstream ss; + ss << tcpSockFD; + ss << ", "; + ss << serializeResult; + castLogStream = ss.str(); + if ((serializeResult == 0) && (tcpSockFD != -1)) { try @@ -540,6 +533,7 @@ bool BoardSupportComm::buildReport() { m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvLen:" << recvLen << endl; m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvContent:" << recvContent << endl; + m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", sendContent:" << castLogStream << endl; return true; } diff --git a/src/pEmulator/Emulator.cpp b/src/pEmulator/Emulator.cpp index 7ae7d78..451243c 100644 --- a/src/pEmulator/Emulator.cpp +++ b/src/pEmulator/Emulator.cpp @@ -384,7 +384,7 @@ void Emulator::set150Info() p_150server_1.embeddedInfoSrc.header = 0xEBA1; //0:[0,1] p_150server_1.embeddedInfoSrc.count = (uint16_t)m_iteration; //2:[2,3] p_150server_1.embeddedInfoSrc.size = (uint8_t)51; //3:[4] - p_150server_1.embeddedInfoSrc.drive_mode = (uint8_t)0xFF; //4:[5] + p_150server_1.embeddedInfoSrc.drive_mode = (uint8_t)0x02; //4:[5] p_150server_1.embeddedInfoSrc.height = (uint16_t)(150-remus100.z); //5:[6,7] p_150server_1.embeddedInfoSrc.depth = (uint16_t)remus100.z; //6:[8,9] p_150server_1.embeddedInfoSrc.yaw = (uint16_t)remus100.yaw; //7:[10,11] diff --git a/src/pStateManagement/StateManagement.cpp b/src/pStateManagement/StateManagement.cpp index 315f326..1acff87 100644 --- a/src/pStateManagement/StateManagement.cpp +++ b/src/pStateManagement/StateManagement.cpp @@ -36,7 +36,6 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) for(p=NewMail.begin(); p!=NewMail.end(); p++) { CMOOSMsg &msg = *p; -#if 1 // Keep these around just for template string key = msg.GetKey(); string comm = msg.GetCommunity(); double dval = msg.GetDouble(); @@ -45,11 +44,10 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) double mtime = msg.GetTime(); bool mdbl = msg.IsDouble(); bool mstr = msg.IsString(); -#endif - - Json::Value deviceState; + + Json::Value deviceState; std::string manualState; - double missionState; + int missionState; if(key == "uManual_enable_cmd") { @@ -74,7 +72,7 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) { deviceState["opMode"] = opModeLists.external; } - else if (manualState == "faluse") + else if (manualState == "false") { if(missionState == 0) { @@ -84,7 +82,7 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail) { deviceState["opMode"] = opModeLists.service; } - else if((missionState == 3) ) + if(missionState == 3) { deviceState["opMode"] = opModeLists.maneuver; } diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.cpp b/src/pSurfaceSupportComm/SurfaceSupportComm.cpp index b0b3c65..e236fee 100644 --- a/src/pSurfaceSupportComm/SurfaceSupportComm.cpp +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.cpp @@ -21,9 +21,9 @@ using namespace std; #define TCP_SEND_FILE_PORT 8002 #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" +#define DEST_IP_ADDRESS "10.25.0.163" //--------------------------------------------------------- // Constructor @@ -217,23 +217,23 @@ bool SurfaceSupportComm::Iterate() { if (missionStatusObject["state"].asInt() == FAULT) { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; } else if (missionStatusObject["state"].asInt() == UNRUN) { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE; } else if (missionStatusObject["state"].asInt() == MANUAL) { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_EXTERNAL; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_EXTERNAL; } else if (missionStatusObject["state"].asInt() == RUN) { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER; } else { - planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; + planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; } planControlStateMsg.plan_id = missionStatusObject["taskName"].asString(); //子任务名称，对应PlanDB info的[taskName] planControlStateMsg.plan_eta = -1; //缺省值 @@ -243,7 +243,6 @@ bool SurfaceSupportComm::Iterate() planControlStateMsg.man_eta = -1; //缺省值 planControlStateMsg.last_outcome = missionStatusObject["errorCode"].asUInt(); - vehicleStateMsg.op_mode = planControlStateMsg.state; vehicleStateMsg.error_count = 0; vehicleStateMsg.error_ents = ""; @@ -551,8 +550,9 @@ void SurfaceSupportComm::acceptNewClient(void) c.socket = sock_tcp_receive.accept(&c.address, &c.port); c.socket->setKeepAlive(true); c.socket->setNoDelay(true); - c.socket->setReceiveTimeout(5); - c.socket->setSendTimeout(5); + // c.socket->setReceiveTimeout(5); + c.socket->setReceiveTimeout(1); + c.socket->setSendTimeout(5); m_poll.add(*c.socket); m_clients.push_back(c); } @@ -590,10 +590,13 @@ void SurfaceSupportComm::handleClients(char* buf, unsigned int cap) if (n > 0) { + mtx.lock(); DUNE::IMC::Message * m = DUNE::IMC::Packet::deserialize((uint8_t *)buf, cap); processMessage(m); free(m); + mtx.unlock(); } + ++itr; } } @@ -739,15 +742,17 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) Json::Value parameterValue; std::istringstream iss(subEntityParameter->value); Json::CharReaderBuilder builder; - bool parsingResult = Json::parseFromStream(builder, iss, ¶meterValue, &err); + bool parsingResult = Json::parseFromStream(builder, iss, ¶meterValue, &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 0 if (dataType == "float") { std::stringstream ss2(dataValueTemp); @@ -776,6 +781,7 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) ss2 >> dataValue; printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str()); } +#endif ss1 << "," << parentName << "," << childName << "," << dataValueTemp; } std::stringstream appCastStream; @@ -872,15 +878,14 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) << (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) + if (msg->command == 1) { Notify("uManual_enable_cmd", "true"); } - if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_STOP_MANEUVER) - { + if (msg->command == 0) + { Notify("uManual_enable_cmd", "false"); } } @@ -889,7 +894,7 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) << "VehicleCommand" << ":" << MOOS::Time() << "," << (int)msg->command; - Notify("uClient_manualEnable_log", ss.str()); + Notify("uClient_manualEnable_log", ss.str()); } if (type == DUNE::IMC::RemoteActions::getIdStatic()) { diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.h b/src/pSurfaceSupportComm/SurfaceSupportComm.h index 88d3d6f..69623fe 100644 --- a/src/pSurfaceSupportComm/SurfaceSupportComm.h +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.h @@ -18,6 +18,7 @@ #include #include #include +#include #include "MOOS/libMOOS/MOOSLib.h" #include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" @@ -147,6 +148,7 @@ private: unsigned int c_block_size; std::string appCastContent; + std::mutex mtx; };