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
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

2
setting/Origin.json
View File

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

40
src/pBoardSupportComm/BoardSupportComm.cpp
View File

@@ -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;
}

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.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]

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++) {
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;
}

37
src/pSurfaceSupportComm/SurfaceSupportComm.cpp
View File

@@ -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, &parameterValue, &err);
bool parsingResult = Json::parseFromStream(builder, iss, &parameterValue, &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())
{

2
src/pSurfaceSupportComm/SurfaceSupportComm.h
View File

@@ -18,6 +18,7 @@
#include <stdlib.h>
#include <arpa/inet.h>
#include <thread>
#include <mutex>
#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;
};