迁移分支
This commit is contained in:
zengxiaobin
35
src/pStateManagement/CMakeLists.txt
Normal file
35
src/pStateManagement/CMakeLists.txt
Normal file
@@ -0,0 +1,35 @@
|
||||
#--------------------------------------------------------
|
||||
# The CMakeLists.txt for: pStateManagement
|
||||
# Author(s): chenlizhi
|
||||
#--------------------------------------------------------
|
||||
|
||||
SET(SRC
|
||||
StateManagement.cpp
|
||||
StateManagement_Info.cpp
|
||||
main.cpp
|
||||
)
|
||||
|
||||
FIND_LIBRARY(DUNE_LIB dune-core /usr/local/lib /usr/local/lib/DUNE)
|
||||
FIND_PATH(DUNE_INCLUDE DUNE/IMC.hpp /usr/local/include /usr/local/include/DUNE)
|
||||
include_directories(${DUNE_INCLUDE})
|
||||
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
#find_package (jsoncpp NO_MODULE REQUIRED)
|
||||
|
||||
include_directories(/usr/include/jsoncpp/)
|
||||
link_directories(/usr/local/lib/)
|
||||
|
||||
ADD_EXECUTABLE(pStateManagement ${SRC})
|
||||
|
||||
TARGET_LINK_LIBRARIES(pStateManagement
|
||||
${MOOS_LIBRARIES}
|
||||
${CMAKE_DL_LIBS}
|
||||
${SYSTEM_LIBS}
|
||||
${DUNE_LIB}
|
||||
mbutil
|
||||
m
|
||||
pthread
|
||||
jsoncpp
|
||||
)
|
||||
|
||||
154
src/pStateManagement/StateManagement.cpp
Normal file
154
src/pStateManagement/StateManagement.cpp
Normal file
@@ -0,0 +1,154 @@
|
||||
/************************************************************/
|
||||
/* NAME: chenlizhi */
|
||||
/* ORGN: MIT */
|
||||
/* FILE: StateManagement.cpp */
|
||||
/* DATE: */
|
||||
/************************************************************/
|
||||
|
||||
#include <iterator>
|
||||
#include "MBUtils.h"
|
||||
#include "StateManagement.h"
|
||||
#include <json/json.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Constructor
|
||||
|
||||
StateManagement::StateManagement()
|
||||
{
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Destructor
|
||||
|
||||
StateManagement::~StateManagement()
|
||||
{
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: OnNewMail
|
||||
|
||||
bool StateManagement::OnNewMail(MOOSMSG_LIST &NewMail)
|
||||
{
|
||||
MOOSMSG_LIST::iterator p;
|
||||
|
||||
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();
|
||||
string sval = msg.GetString();
|
||||
string msrc = msg.GetSource();
|
||||
double mtime = msg.GetTime();
|
||||
bool mdbl = msg.IsDouble();
|
||||
bool mstr = msg.IsString();
|
||||
#endif
|
||||
|
||||
Json::Value deviceState;
|
||||
double manualState;
|
||||
double missionState;
|
||||
|
||||
if(key == "uManual_enable_cmd")
|
||||
{
|
||||
manualState = msg.GetDouble();
|
||||
}
|
||||
if(key == "uMission_task_fb")
|
||||
{
|
||||
std::string missionStateString = msg.GetString();
|
||||
std::string errMission;
|
||||
Json::Value missionStateData;
|
||||
std::istringstream issm(missionStateString);
|
||||
Json::CharReaderBuilder builderMission;
|
||||
bool parsingResultMission = Json::parseFromStream(builderMission, issm, &missionStateData, &errMission);
|
||||
if (!parsingResultMission) {
|
||||
std::cerr << "Failed to parse JSON string." << std::endl;
|
||||
return -1;
|
||||
}
|
||||
missionState = missionStateData["state"].asInt();
|
||||
}
|
||||
|
||||
if(fabs(manualState - 1) < 1e-6) //manualState=1
|
||||
{
|
||||
deviceState["opMode"] = opModeLists.external;
|
||||
}
|
||||
else if (fabs(manualState - 0) < 1e-6) //manualState=0
|
||||
{
|
||||
if(missionState == 0)
|
||||
{
|
||||
deviceState["opMode"] = opModeLists.error;
|
||||
}
|
||||
if(missionState == 1)
|
||||
{
|
||||
deviceState["opMode"] = opModeLists.service;
|
||||
}
|
||||
else if((missionState == 3) )
|
||||
{
|
||||
deviceState["opMode"] = opModeLists.maneuver;
|
||||
}
|
||||
}
|
||||
|
||||
Json::StreamWriterBuilder builder;
|
||||
std::string deviceStateString = Json::writeString(builder, deviceState);
|
||||
Notify("uDevice_state_fb", deviceStateString);
|
||||
}
|
||||
|
||||
return(true);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: OnConnectToServer
|
||||
|
||||
bool StateManagement::OnConnectToServer()
|
||||
{
|
||||
RegisterVariables();
|
||||
return(true);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: Iterate()
|
||||
// happens AppTick times per second
|
||||
|
||||
bool StateManagement::Iterate()
|
||||
{
|
||||
return(true);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: OnStartUp()
|
||||
// happens before connection is open
|
||||
|
||||
bool StateManagement::OnStartUp()
|
||||
{
|
||||
list<string> sParams;
|
||||
m_MissionReader.EnableVerbatimQuoting(false);
|
||||
if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) {
|
||||
list<string>::iterator p;
|
||||
for(p=sParams.begin(); p!=sParams.end(); p++) {
|
||||
string line = *p;
|
||||
string param = tolower(biteStringX(line, '='));
|
||||
string value = line;
|
||||
|
||||
if(param == "foo") {
|
||||
//handled
|
||||
}
|
||||
else if(param == "bar") {
|
||||
//handled
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
RegisterVariables();
|
||||
return(true);
|
||||
}
|
||||
|
||||
//---------------------------------------------------------
|
||||
// Procedure: RegisterVariables
|
||||
|
||||
void StateManagement::RegisterVariables()
|
||||
{
|
||||
// Register("FOOBAR", 0);
|
||||
}
|
||||
|
||||
52
src/pStateManagement/StateManagement.h
Normal file
52
src/pStateManagement/StateManagement.h
Normal file
@@ -0,0 +1,52 @@
|
||||
/************************************************************/
|
||||
/* NAME: chenlizhi */
|
||||
/* ORGN: MIT */
|
||||
/* FILE: StateManagement.h */
|
||||
/* DATE: */
|
||||
/************************************************************/
|
||||
|
||||
#ifndef StateManagement_HEADER
|
||||
#define StateManagement_HEADER
|
||||
|
||||
#include "MOOS/libMOOS/MOOSLib.h"
|
||||
|
||||
class StateManagement : public CMOOSApp
|
||||
{
|
||||
public:
|
||||
StateManagement();
|
||||
~StateManagement();
|
||||
|
||||
struct DeviceState
|
||||
{
|
||||
int opMode; // auv当前状态
|
||||
int errorCount; // 处于错误状态的子设备数量
|
||||
std::string errorEnts; // 处于错误状态的子设备列表
|
||||
};
|
||||
|
||||
struct OpModeLists // auv当前状态列表
|
||||
{
|
||||
int service = 0; // 正常
|
||||
int calibration = 1; // 校准状态
|
||||
int error = 2; // 错误状态
|
||||
int maneuver = 3; // 正在执行使命任务
|
||||
int external = 4; // 外部控制状态（手操考虑用这个状态）
|
||||
int boot = 5; // 只能在启动的
|
||||
};
|
||||
|
||||
protected: // Standard MOOSApp functions to overload
|
||||
bool OnNewMail(MOOSMSG_LIST &NewMail);
|
||||
bool Iterate();
|
||||
bool OnConnectToServer();
|
||||
bool OnStartUp();
|
||||
struct DeviceState deviceState;
|
||||
struct OpModeLists opModeLists;
|
||||
|
||||
protected:
|
||||
void RegisterVariables();
|
||||
|
||||
private: // Configuration variables
|
||||
|
||||
private: // State variables
|
||||
};
|
||||
|
||||
#endif
|
||||
115
src/pStateManagement/StateManagement_Info.cpp
Normal file
115
src/pStateManagement/StateManagement_Info.cpp
Normal file
@@ -0,0 +1,115 @@
|
||||
/****************************************************************/
|
||||
/* NAME: chenlizhi */
|
||||
/* ORGN: MIT Cambridge MA */
|
||||
/* FILE: StateManagement_Info.cpp */
|
||||
/* DATE: Dec 29th 1963 */
|
||||
/****************************************************************/
|
||||
|
||||
#include <cstdlib>
|
||||
#include <iostream>
|
||||
#include "StateManagement_Info.h"
|
||||
#include "ColorParse.h"
|
||||
#include "ReleaseInfo.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showSynopsis
|
||||
|
||||
void showSynopsis()
|
||||
{
|
||||
blk("SYNOPSIS: ");
|
||||
blk("------------------------------------ ");
|
||||
blk(" The pStateManagement application is used for ");
|
||||
blk(" ");
|
||||
blk(" ");
|
||||
blk(" ");
|
||||
blk(" ");
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showHelpAndExit
|
||||
|
||||
void showHelpAndExit()
|
||||
{
|
||||
blk(" ");
|
||||
blu("=============================================================== ");
|
||||
blu("Usage: pStateManagement file.moos [OPTIONS] ");
|
||||
blu("=============================================================== ");
|
||||
blk(" ");
|
||||
showSynopsis();
|
||||
blk(" ");
|
||||
blk("Options: ");
|
||||
mag(" --alias","=<ProcessName> ");
|
||||
blk(" Launch pStateManagement with the given process name ");
|
||||
blk(" rather than pStateManagement. ");
|
||||
mag(" --example, -e ");
|
||||
blk(" Display example MOOS configuration block. ");
|
||||
mag(" --help, -h ");
|
||||
blk(" Display this help message. ");
|
||||
mag(" --interface, -i ");
|
||||
blk(" Display MOOS publications and subscriptions. ");
|
||||
mag(" --version,-v ");
|
||||
blk(" Display the release version of pStateManagement. ");
|
||||
blk(" ");
|
||||
blk("Note: If argv[2] does not otherwise match a known option, ");
|
||||
blk(" then it will be interpreted as a run alias. This is ");
|
||||
blk(" to support pAntler launching conventions. ");
|
||||
blk(" ");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showExampleConfigAndExit
|
||||
|
||||
void showExampleConfigAndExit()
|
||||
{
|
||||
blk(" ");
|
||||
blu("=============================================================== ");
|
||||
blu("pStateManagement Example MOOS Configuration ");
|
||||
blu("=============================================================== ");
|
||||
blk(" ");
|
||||
blk("ProcessConfig = pStateManagement ");
|
||||
blk("{ ");
|
||||
blk(" AppTick = 4 ");
|
||||
blk(" CommsTick = 4 ");
|
||||
blk(" ");
|
||||
blk("} ");
|
||||
blk(" ");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showInterfaceAndExit
|
||||
|
||||
void showInterfaceAndExit()
|
||||
{
|
||||
blk(" ");
|
||||
blu("=============================================================== ");
|
||||
blu("pStateManagement INTERFACE ");
|
||||
blu("=============================================================== ");
|
||||
blk(" ");
|
||||
showSynopsis();
|
||||
blk(" ");
|
||||
blk("SUBSCRIPTIONS: ");
|
||||
blk("------------------------------------ ");
|
||||
blk(" NODE_MESSAGE = src_node=alpha,dest_node=bravo,var_name=FOO, ");
|
||||
blk(" string_val=BAR ");
|
||||
blk(" ");
|
||||
blk("PUBLICATIONS: ");
|
||||
blk("------------------------------------ ");
|
||||
blk(" Publications are determined by the node message content. ");
|
||||
blk(" ");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
//----------------------------------------------------------------
|
||||
// Procedure: showReleaseInfoAndExit
|
||||
|
||||
void showReleaseInfoAndExit()
|
||||
{
|
||||
showReleaseInfo("pStateManagement", "gpl");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
18
src/pStateManagement/StateManagement_Info.h
Normal file
18
src/pStateManagement/StateManagement_Info.h
Normal file
@@ -0,0 +1,18 @@
|
||||
/****************************************************************/
|
||||
/* NAME: chenlizhi */
|
||||
/* ORGN: MIT Cambridge MA */
|
||||
/* FILE: StateManagement_Info.h */
|
||||
/* DATE: Dec 29th 1963 */
|
||||
/****************************************************************/
|
||||
|
||||
#ifndef StateManagement_INFO_HEADER
|
||||
#define StateManagement_INFO_HEADER
|
||||
|
||||
void showSynopsis();
|
||||
void showHelpAndExit();
|
||||
void showExampleConfigAndExit();
|
||||
void showInterfaceAndExit();
|
||||
void showReleaseInfoAndExit();
|
||||
|
||||
#endif
|
||||
|
||||
52
src/pStateManagement/main.cpp
Normal file
52
src/pStateManagement/main.cpp
Normal file
@@ -0,0 +1,52 @@
|
||||
/************************************************************/
|
||||
/* NAME: chenlizhi */
|
||||
/* ORGN: MIT */
|
||||
/* FILE: main.cpp */
|
||||
/* DATE: */
|
||||
/************************************************************/
|
||||
|
||||
#include <string>
|
||||
#include "MBUtils.h"
|
||||
#include "ColorParse.h"
|
||||
#include "StateManagement.h"
|
||||
#include "StateManagement_Info.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
string mission_file;
|
||||
string run_command = argv[0];
|
||||
|
||||
for(int i=1; i<argc; i++) {
|
||||
string argi = argv[i];
|
||||
if((argi=="-v") || (argi=="--version") || (argi=="-version"))
|
||||
showReleaseInfoAndExit();
|
||||
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
|
||||
showExampleConfigAndExit();
|
||||
else if((argi == "-h") || (argi == "--help") || (argi=="-help"))
|
||||
showHelpAndExit();
|
||||
else if((argi == "-i") || (argi == "--interface"))
|
||||
showInterfaceAndExit();
|
||||
else if(strEnds(argi, ".moos") || strEnds(argi, ".moos++"))
|
||||
mission_file = argv[i];
|
||||
else if(strBegins(argi, "--alias="))
|
||||
run_command = argi.substr(8);
|
||||
else if(i==2)
|
||||
run_command = argi;
|
||||
}
|
||||
|
||||
if(mission_file == "")
|
||||
showHelpAndExit();
|
||||
|
||||
cout << termColor("green");
|
||||
cout << "pStateManagement launching as " << run_command << endl;
|
||||
cout << termColor() << endl;
|
||||
|
||||
StateManagement StateManagement;
|
||||
|
||||
StateManagement.Run(run_command.c_str(), mission_file.c_str());
|
||||
|
||||
return(0);
|
||||
}
|
||||
|
||||
9
src/pStateManagement/pStateManagement.moos
Normal file
9
src/pStateManagement/pStateManagement.moos
Normal file
@@ -0,0 +1,9 @@
|
||||
//------------------------------------------------
|
||||
// pStateManagement config block
|
||||
|
||||
ProcessConfig = pStateManagement
|
||||
{
|
||||
AppTick = 4
|
||||
CommsTick = 4
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user