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:
jhl
2023-11-29 17:47:03 +08:00
parent c71064fcb4
commit 9db47019cc
7 changed files with 57 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
launch/launch_local.moos
View File

@@ -18,16 +18,14 @@ AltOrigin = 0
VehicleName = lauv-150 VehicleName = lauv-150
LogEnable = false LogEnable = false
//LogDir = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/log/ LogDir = /home/jhl/project/moos-ivp-pi/log/
LogDir = /home/ben/project/moos-ivp-pi/log/
AuvDataLog = auvData.mdat AuvDataLog = auvData.mdat
MissionHistoryLog = missionHistory.txt MissionHistoryLog = missionHistory.txt
ClientCommandLog = clientCommand.txt ClientCommandLog = clientCommand.txt
FaultLog = faultLog.txt FaultLog = faultLog.txt
MotionControlLog = motionControl.txt MotionControlLog = motionControl.txt
//llaOriginPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/Origin.json llaOriginPath = /home/jhl/project/moos-ivp-pi/setting/Origin.json
llaOriginPath = /home/ben/project/moos-ivp-pi/setting/Origin.json
//------------------------------------------ //------------------------------------------
// Antler configuration block // Antler configuration block
@@ -40,20 +38,20 @@ ProcessConfig = ANTLER
Run = pNodeReporter @ NewConsole = false Run = pNodeReporter @ NewConsole = false
Run = uProcessWatch @ NewConsole = false Run = uProcessWatch @ NewConsole = false
Run = pRealm @ NewConsole = false Run = pRealm @ NewConsole = false
//Run = pShare @ NewConsole = false Run = pShare @ NewConsole = false
Run = pMarineViewer @ NewConsole = false //Run = pMarineViewer @ NewConsole = false
//===========Our define process==================== //===========Our define process====================
Run = pBoardSupportComm @ NewConsole = false Run = pBoardSupportComm @ NewConsole = false
Run = pTaskManagement @ NewConsole = false Run = pTaskManagement @ NewConsole = false
//Run = pMotionControler @ NewConsole = false Run = pMotionControler @ NewConsole = false
Run = pSurfaceSupportComm @ NewConsole = false Run = pSurfaceSupportComm @ NewConsole = false
Run = pDataManagement @ NewConsole = false Run = pDataManagement @ NewConsole = false
Run = pFaultHandle @ NewConsole = false Run = pFaultHandle @ NewConsole = false
Run = pStateManagement @ NewConsole = false Run = pStateManagement @ NewConsole = false
//===============For test process=================== //===============For test process===================
//Run = pEmulator @ NewConsole = false Run = pEmulator @ NewConsole = false
Run = uSimMarine @ NewConsole = false //Run = uSimMarine @ NewConsole = false
Run = pMarinePID @ NewConsole = false //Run = pMarinePID @ NewConsole = false
} }
@@ -211,8 +209,7 @@ ProcessConfig = pTaskManagement
{ {
AppTick = 8 AppTick = 8
CommsTick = 8 CommsTick = 8
//planConfigPath = /home/zjk/Desktop/project/moos-ivp-pi/setting/PlanConfigure.json planConfigPath = /home/jhl/project/moos-ivp-pi/setting/PlanConfigure.json
planConfigPath = /home/ben/project/moos-ivp-pi/setting/PlanConfigure.json
} }
@@ -275,8 +272,7 @@ ProcessConfig = pMotionControler
cheak_stalensee = true cheak_stalensee = true
delta_freqency = 5 delta_freqency = 5
//config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/setting/ControlParam.json config_file = /home/jhl/project/moos-ivp-pi/setting/ControlParam.json
config_file = /home/ben/project/moos-ivp-pi/setting/ControlParam.json
} }
ProcessConfig = pEmulator ProcessConfig = pEmulator

2
setting/Origin.json
View File

@@ -1,5 +1,5 @@
{ {
"AltOrigin" : 0, "AltOrigin" : 0.0,
"LatOrigin" : 50.825298309326172, "LatOrigin" : 50.825298309326172,
"LongOrigin" : -90.330398559570312, "LongOrigin" : -90.330398559570312,
"TaskName" : "east_waypt_survey" "TaskName" : "east_waypt_survey"

40
src/pBoardSupportComm/BoardSupportComm.cpp
View File

@@ -13,9 +13,9 @@
#define TCP_RECEIVE_PORT 8001 #define TCP_RECEIVE_PORT 8001
#define TCP_SERVER_ADDRESS "127.0.0.1" #define TCP_SERVER_ADDRESS "127.0.0.1"
// #define MOOS_AUV_SIM //#define MOOS_AUV_SIM
//#define MATLAB_AUV_SIM #define MATLAB_AUV_SIM
#define TRUE_AUV //#define TRUE_AUV
using namespace std; using namespace std;
@@ -57,7 +57,7 @@ BoardSupportComm::BoardSupportComm()
executeCommand.header = 0xEBA2; //1:[0,1] executeCommand.header = 0xEBA2; //1:[0,1]
executeCommand.count = 16; //2:[2,3] executeCommand.count = 16; //2:[2,3]
executeCommand.size = 21; //3:[4] executeCommand.size = 21; //3:[4]
executeCommand.drive_mode = 0xFF; //4:[5] executeCommand.drive_mode = 0x02; //4:[5]
executeCommand.thrust = 0; //5:[6] executeCommand.thrust = 0; //5:[6]
executeCommand.yaw = 0; //6:[7,8] executeCommand.yaw = 0; //6:[7,8]
executeCommand.depth = 0; //7:[9,10] executeCommand.depth = 0; //7:[9,10]
@@ -180,15 +180,13 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
if (sval == "true") if (sval == "true")
{ {
executeCommand.drive_mode = 0x02;
executeCommand.manual_mode = true; executeCommand.manual_mode = true;
} }
else else
{ {
executeCommand.drive_mode = 0xFF;
executeCommand.manual_mode = false; executeCommand.manual_mode = false;
} }
} }
if(key == "uManual_drive_cmd") if(key == "uManual_drive_cmd")
@@ -215,26 +213,21 @@ bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
return false; return false;
} }
executeCommand.thrust = convertIntToUchar(recvCommand["Thrust"].asInt(), -100, 100); executeCommand.thrust = convertIntToUchar(recvCommand["Thrust"].asInt(), -100, 100);
float heading = recvCommand["Heading"].asFloat(); uint8_t helm_top_bottom_angle = convertIntToUchar((int)(recvCommand["Heading"].asFloat()), -30, 30);
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;
}
executeCommand.depth = 0; executeCommand.depth = 0;
executeCommand.helm_top_angle = 0; executeCommand.helm_top_angle = helm_top_bottom_angle;
executeCommand.helm_bottom_angle = 0; executeCommand.helm_bottom_angle = helm_top_bottom_angle;
executeCommand.helm_left_angle = 0; executeCommand.helm_left_angle = 0;
executeCommand.helm_right_angle = 0; executeCommand.helm_right_angle = 0;
executeCommand.yaw = 0;
int serializeResult = serializeMessage(tcpSendBuffer); int serializeResult = serializeMessage(tcpSendBuffer);
std::stringstream ss;
ss << tcpSockFD;
ss << ", ";
ss << serializeResult;
castLogStream = ss.str();
if ((serializeResult == 0) && (tcpSockFD != -1)) if ((serializeResult == 0) && (tcpSockFD != -1))
{ {
try 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() << ", recvLen:" << recvLen << endl;
m_msgs << std::fixed << std::setprecision(6) << MOOS::Time() << ", recvContent:" << recvContent << 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; return true;
} }

2
src/pEmulator/Emulator.cpp
View File

@@ -384,7 +384,7 @@ void Emulator::set150Info()
p_150server_1.embeddedInfoSrc.header = 0xEBA1; //0:[0,1] 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.count = (uint16_t)m_iteration; //2:[2,3]
p_150server_1.embeddedInfoSrc.size = (uint8_t)51; //3:[4] 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.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.depth = (uint16_t)remus100.z; //6:[8,9]
p_150server_1.embeddedInfoSrc.yaw = (uint16_t)remus100.yaw; //7:[10,11] p_150server_1.embeddedInfoSrc.yaw = (uint16_t)remus100.yaw; //7:[10,11]

12
src/pStateManagement/StateManagement.cpp
View File

@@ -36,7 +36,6 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
for(p=NewMail.begin(); p!=NewMail.end(); p++) { for(p=NewMail.begin(); p!=NewMail.end(); p++) {
CMOOSMsg &msg = *p; CMOOSMsg &msg = *p;
#if 1 // Keep these around just for template
string key = msg.GetKey(); string key = msg.GetKey();
string comm = msg.GetCommunity(); string comm = msg.GetCommunity();
double dval = msg.GetDouble(); double dval = msg.GetDouble();
@@ -45,11 +44,10 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
double mtime = msg.GetTime(); double mtime = msg.GetTime();
bool mdbl = msg.IsDouble(); bool mdbl = msg.IsDouble();
bool mstr = msg.IsString(); bool mstr = msg.IsString();
#endif
Json::Value deviceState;
Json::Value deviceState;
std::string manualState; std::string manualState;
double missionState; int missionState;
if(key == "uManual_enable_cmd") if(key == "uManual_enable_cmd")
{ {
@@ -74,7 +72,7 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
deviceState["opMode"] = opModeLists.external; deviceState["opMode"] = opModeLists.external;
} }
else if (manualState == "faluse") else if (manualState == "false")
{ {
if(missionState == 0) if(missionState == 0)
{ {
@@ -84,7 +82,7 @@ bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
{ {
deviceState["opMode"] = opModeLists.service; deviceState["opMode"] = opModeLists.service;
} }
else if((missionState == 3) ) if(missionState == 3)
{ {
deviceState["opMode"] = opModeLists.maneuver; deviceState["opMode"] = opModeLists.maneuver;
} }

37
src/pSurfaceSupportComm/SurfaceSupportComm.cpp
View File

@@ -21,9 +21,9 @@ using namespace std;
#define TCP_SEND_FILE_PORT 8002 #define TCP_SEND_FILE_PORT 8002
#define SRC_IP_ADDRESS "127.0.0.1" #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 "127.0.0.1"
// #define DEST_IP_ADDRESS "10.25.0.163" #define DEST_IP_ADDRESS "10.25.0.163"
//--------------------------------------------------------- //---------------------------------------------------------
// Constructor // Constructor
@@ -217,23 +217,23 @@ bool SurfaceSupportComm::Iterate()
{ {
if (missionStatusObject["state"].asInt() == FAULT) 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) 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) 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) else if (missionStatusObject["state"].asInt() == RUN)
{ {
planControlStateMsg.state == DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER; planControlStateMsg.state = DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER;
} }
else 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_id = missionStatusObject["taskName"].asString(); //子任务名称，对应PlanDB info的[taskName]
planControlStateMsg.plan_eta = -1; //缺省值 planControlStateMsg.plan_eta = -1; //缺省值
@@ -243,7 +243,6 @@ bool SurfaceSupportComm::Iterate()
planControlStateMsg.man_eta = -1; //缺省值 planControlStateMsg.man_eta = -1; //缺省值
planControlStateMsg.last_outcome = missionStatusObject["errorCode"].asUInt(); planControlStateMsg.last_outcome = missionStatusObject["errorCode"].asUInt();
vehicleStateMsg.op_mode = planControlStateMsg.state; vehicleStateMsg.op_mode = planControlStateMsg.state;
vehicleStateMsg.error_count = 0; vehicleStateMsg.error_count = 0;
vehicleStateMsg.error_ents = ""; vehicleStateMsg.error_ents = "";
@@ -551,8 +550,9 @@ void SurfaceSupportComm::acceptNewClient(void)
c.socket = sock_tcp_receive.accept(&c.address, &c.port); c.socket = sock_tcp_receive.accept(&c.address, &c.port);
c.socket->setKeepAlive(true); c.socket->setKeepAlive(true);
c.socket->setNoDelay(true); c.socket->setNoDelay(true);
c.socket->setReceiveTimeout(5); // c.socket->setReceiveTimeout(5);
c.socket->setSendTimeout(5); c.socket->setReceiveTimeout(1);
c.socket->setSendTimeout(5);
m_poll.add(*c.socket); m_poll.add(*c.socket);
m_clients.push_back(c); m_clients.push_back(c);
} }
@@ -590,10 +590,13 @@ void SurfaceSupportComm::handleClients(char* buf, unsigned int cap)
if (n > 0) if (n > 0)
{ {
mtx.lock();
DUNE::IMC::Message * m = DUNE::IMC::Packet::deserialize((uint8_t *)buf, cap); DUNE::IMC::Message * m = DUNE::IMC::Packet::deserialize((uint8_t *)buf, cap);
processMessage(m); processMessage(m);
free(m); free(m);
mtx.unlock();
} }
++itr; ++itr;
} }
} }
@@ -739,15 +742,17 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
Json::Value parameterValue; Json::Value parameterValue;
std::istringstream iss(subEntityParameter->value); std::istringstream iss(subEntityParameter->value);
Json::CharReaderBuilder builder; Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &parameterValue, &err); bool parsingResult = Json::parseFromStream(builder, iss, &parameterValue, &err);
if (!parsingResult) if (!parsingResult)
{ {
throw ("SetEntityParameters parse error"); throw ("SetEntityParameters parse error");
} }
std::string parentName = msg->name; std::string parentName = msg->name;
std::string childName = subEntityParameter->name; std::string childName = subEntityParameter->name;
std::string dataType = parameterValue["type"].asString(); std::string dataType = parameterValue["type"].asString();
std::string dataValueTemp = parameterValue["value"].asString(); std::string dataValueTemp = parameterValue["value"].asString();
#if 0
if (dataType == "float") if (dataType == "float")
{ {
std::stringstream ss2(dataValueTemp); std::stringstream ss2(dataValueTemp);
@@ -776,6 +781,7 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
ss2 >> dataValue; ss2 >> dataValue;
printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str()); printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str());
} }
#endif
ss1 << "," << parentName << "," << childName << "," << dataValueTemp; ss1 << "," << parentName << "," << childName << "," << dataValueTemp;
} }
std::stringstream appCastStream; std::stringstream appCastStream;
@@ -872,15 +878,14 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
<< (int)msg->type << "," << (int)msg->type << ","
<< (int)msg->command; << (int)msg->command;
appCastContent = appCastStream.str(); appCastContent = appCastStream.str();
if (msg->type == DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST) 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"); Notify("uManual_enable_cmd", "true");
} }
if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_STOP_MANEUVER) if (msg->command == 0)
{ {
Notify("uManual_enable_cmd", "false"); Notify("uManual_enable_cmd", "false");
} }
} }
@@ -889,7 +894,7 @@ void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message)
<< "VehicleCommand" << ":" << "VehicleCommand" << ":"
<< MOOS::Time() << "," << MOOS::Time() << ","
<< (int)msg->command; << (int)msg->command;
Notify("uClient_manualEnable_log", ss.str()); Notify("uClient_manualEnable_log", ss.str());
} }
if (type == DUNE::IMC::RemoteActions::getIdStatic()) if (type == DUNE::IMC::RemoteActions::getIdStatic())
{ {

2
src/pSurfaceSupportComm/SurfaceSupportComm.h
View File

@@ -18,6 +18,7 @@
#include <stdlib.h> #include <stdlib.h>
#include <arpa/inet.h> #include <arpa/inet.h>
#include <thread> #include <thread>
#include <mutex>
#include "MOOS/libMOOS/MOOSLib.h" #include "MOOS/libMOOS/MOOSLib.h"
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" #include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
@@ -147,6 +148,7 @@ private:
unsigned int c_block_size; unsigned int c_block_size;
std::string appCastContent; std::string appCastContent;
std::mutex mtx;
}; };