zjk/moos-ivp-pi
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

迁移pi上的master分支

Browse Source
This commit is contained in:
zjk
2023-11-20 09:51:04 +08:00
parent 0388d6de89
commit 036fbdbc9f
99 changed files with 16668 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
CMakeLists.txt Normal file
View File

@@ -0,0 +1,104 @@
#=======================================================================
# FILE: moos-ivp-extend/CMakeLists.txt
# DATE: 2012/07/24
# INFO: Top-level CMakeLists.txt file for the moos-ivp-extend project
# NAME: Maintained by Mike Benjamin - Original setup by Christian Convey
# Chris Gagner, and tips borrowed from Dave Billin
#=======================================================================
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
PROJECT( IVP_EXTEND )
set (CMAKE_CXX_STANDARD 11)
#=======================================================================
# Set the output directories for the binary and library files
#=======================================================================
GET_FILENAME_COMPONENT(IVP_EXTEND_BIN_DIR "${CMAKE_SOURCE_DIR}/bin" ABSOLUTE )
GET_FILENAME_COMPONENT(IVP_EXTEND_LIB_DIR "${CMAKE_SOURCE_DIR}/lib" ABSOLUTE )
SET( LIBRARY_OUTPUT_PATH "${IVP_EXTEND_LIB_DIR}" CACHE PATH "" )
SET( ARCHIVE_OUTPUT_DIRECTORY "${IVP_EXTEND_LIB_DIR}" CACHE PATH "" )
SET( LIBRARY_OUTPUT_DIRECTORY "${IVP_EXTEND_LIB_DIR}" CACHE PATH "" )
SET( EXECUTABLE_OUTPUT_PATH "${IVP_EXTEND_BIN_DIR}" CACHE PATH "" )
SET( RUNTIME_OUTPUT_DIRECTORY "${IVP_EXTEND_BIN_DIR}" CACHE PATH "" )
#=======================================================================
# Find MOOS
#=======================================================================
find_package(MOOS 10.0)
INCLUDE_DIRECTORIES(${MOOS_INCLUDE_DIRS})
#=======================================================================
# Find the "moos-ivp" base directory
#=======================================================================
# Search for the moos-ivp folder
find_path( MOOSIVP_SOURCE_TREE_BASE
NAMES build-ivp.sh build-moos.sh configure-ivp.sh
PATHS "../moos-ivp" "../../moos-ivp" "../../moos-ivp/trunk/" "../moos-ivp/trunk/"
DOC "Base directory of the MOOS-IvP source tree"
NO_DEFAULT_PATH
)
if (NOT MOOSIVP_SOURCE_TREE_BASE)
message("Please set MOOSIVP_SOURCE_TREE_BASE to ")
message("the location of the \"moos-ivp\" folder ")
return()
endif()
#======================================================================
# Specify where to find IvP's headers and libraries...
#======================================================================
FILE(GLOB IVP_INCLUDE_DIRS ${MOOSIVP_SOURCE_TREE_BASE}/ivp/src/lib_* )
INCLUDE_DIRECTORIES(${IVP_INCLUDE_DIRS})
FILE(GLOB IVP_LIBRARY_DIRS ${MOOSIVP_SOURCE_TREE_BASE}/lib )
LINK_DIRECTORIES(${IVP_LIBRARY_DIRS})
#======================================================================
# Specify Compiler Flags
#======================================================================
IF( ${WIN32} )
#---------------------------------------------
# Windows Compiler Flags
#---------------------------------------------
IF(MSVC)
# Flags for Microsoft Visual Studio
SET( WALL_ON OFF CACHE BOOL
"tell me about all compiler warnings (-Wall) ")
IF(WALL_ON)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
ENDIF(WALL_ON)
ELSE(MSVC)
# Other Windows compilers go here
ENDIF(MSVC)
ELSE( ${WIN32} )
#---------------------------------------------
# Linux and Apple Compiler Flags
#---------------------------------------------
# Force -fPIC because gcc complains when we don't use it with x86_64 code.
# Note sure why: -fPIC should only be needed for shared objects, and
# AFAIK, CMake gets that right when building shared objects. -CJC
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -g -Wdeprecated-declarations")
IF(CMAKE_COMPILER_IS_GNUCXX)
# Flags for the GNU C++ Compiler
SET( WALL_ON OFF CACHE BOOL
"tell me about all compiler warnings (-Wall) ")
IF(WALL_ON)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall" -C++11)
ENDIF( WALL_ON)
ELSE(CMAKE_COMPILER_IS_GNUCXX)
ENDIF(CMAKE_COMPILER_IS_GNUCXX)
ENDIF( ${WIN32} )
#=======================================================================
# Add Subdirectories
#=======================================================================
ADD_SUBDIRECTORY( src )

110
PlanConfigure.json Normal file
View File

@@ -0,0 +1,110 @@
{
"east_waypt_survey" :
{
"boardStamp" : 1698135269.1985879,
"clientStamp" : 1698135269.1812179,
"closedLoop" : true,
"constSpeed" : -1.0,
"duration" : -1.0,
"maxDepth" : -1.0,
"minDepth" : -1.0,
"origin" :
{
"altitude" : 0.0,
"lat" : 43.825298309326172,
"lon" : -70.330398559570312
},
"perpetual" : false,
"points" :
[
{
"depth" : 9.0,
"east" : 121.51780491942634,
"lat" : 43.824428558349609,
"lon" : -70.328887939453125,
"name" : "station_1",
"north" : -96.635898081838207,
"speed" : 2.0,
"type" : "point"
},
{
"depth" : 7.0,
"east" : 201.91511278899367,
"lat" : 43.824676513671875,
"lon" : -70.327888488769531,
"name" : "station_2",
"north" : -69.083922179977421,
"speed" : 2.5,
"type" : "point"
}
],
"priority" : 10,
"repeat" : 3,
"sourceAddress" : "10.25.0.160",
"sourceName" : "CCU Neptus 0_163",
"taskId" : "1",
"taskName" : "east_waypt_survey"
},
"west_waypt_survey" :
{
"boardStamp" : 1698135268.3958621,
"clientStamp" : 1698135268.2057669,
"closedLoop" : true,
"constSpeed" : -1.0,
"duration" : -1.0,
"maxDepth" : -1.0,
"minDepth" : -1.0,
"origin" :
{
"altitude" : 0.0,
"lat" : 43.825298309326172,
"lon" : -70.330398559570312
},
"perpetual" : true,
"points" :
[
{
"depth" : 9.0,
"east" : -91.445572043530944,
"lat" : 43.824195861816406,
"lon" : -70.331535339355469,
"name" : "station_1",
"north" : -122.49101460421512,
"speed" : 4.0
},
{
"depth" : 7.0,
"east" : 5.5235485468483718,
"lat" : 43.824298858642578,
"lon" : -70.330329895019531,
"name" : "station_2",
"north" : -111.04778559533926,
"speed" : 6.0
},
{
"depth" : 5.0,
"east" : 4.2961493948725868,
"lat" : 43.823516845703125,
"lon" : -70.330345153808594,
"name" : "station_3",
"north" : -197.93630920628678,
"speed" : 8.0
},
{
"depth" : 3.0,
"east" : -81.013520711457318,
"lat" : 43.823207855224609,
"lon" : -70.331405639648438,
"name" : "station_4",
"north" : -232.26737690334403,
"speed" : 10.0
}
],
"priority" : 10,
"repeat" : -1,
"sourceAddress" : "10.25.0.160",
"sourceName" : "CCU Neptus 0_163",
"taskId" : "2",
"taskName" : "west_waypt_survey"
}
}

84
README Normal file
View File

@@ -0,0 +1,84 @@
##############################################################################
# FILE: moos-ivp-extend/README
# DATE: 2014/01/02
# DESCRIPTION: Contains important information regarding the moos-ivp-extend
# repository.
##############################################################################
#=============================================================================
# Introduction
#=============================================================================
The moos-ivp-extend repository contains examples for extending the MOOS-IvP
Autonomy system. This includes a MOOS application and an IvP behavior.
#=============================================================================
# Directory Structure
#=============================================================================
The directory structure for the moos-ivp-extend is decribed below:
bin - Directory for generated executable files
build - Directory for build object files
build.sh - Script for building moos-ivp-extend
CMakeLists.txt - CMake configuration file for the project
data - Directory for storing data
lib - Directory for generated library files
missions - Directory for mission files
README - Contains helpful information - (this file).
scripts - Directory for script files
src - Directory for source code
#=============================================================================
# Build Instructions
#=============================================================================
#--------------------
# Linux and Mac Users
#--------------------
To build on Linux and Apple platforms, execute the build script within this
directory:
$ ./build.sh
To build without using the supplied script, execute the following commands
within this directory:
$ mkdir -p build
$ cd build
$ cmake ../
$ make
$ cd ..
#--------------
# Windows Users
#--------------
To build on Windows platform, open CMake using your favorite shortcut. Then
set the source directory to be this directory and set the build directory
to the "build" directory inside this directory.
The source directory is typically next to the question:
"Where is the source code?"
The build directory is typically next to the question:
"Where to build the binaries?"
Alternatively, CMake can be invoked via the command line. However, you must
specify your gernerator. Use "cmake --help" for a list of generators and
additional help.
#=============================================================================
# Environment variables
#=============================================================================
The moos-ivp-extend binaries files should be added to your path to allow them
to be launched from pAntler.
In order for generated IvP Behaviors to be recognized by the IvP Helm, you
should add the library directory to the "IVP_BEHAVIOR_DIRS" environment
variable.
##############################################################################
# END of README
##############################################################################

46
build.sh Executable file
View File

@@ -0,0 +1,46 @@
#!/bin/bash
INVOCATION_ABS_DIR=`pwd`
BUILD_TYPE="None"
CMD_LINE_ARGS=""
#-------------------------------------------------------------------
# Part 1: Check for and handle command-line arguments
#-------------------------------------------------------------------
for ARGI; do
if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then
printf "%s [SWITCHES] \n" $0
printf "Switches: \n"
printf " --help, -h \n"
printf " --debug, -d \n"
printf " --release, -r \n"
printf "Notes: \n"
printf " (1) All other command line args will be passed as args \n"
printf " to \"make\" when it is eventually invoked. \n"
printf " (2) For example -k will continue making when/if a failure \n"
printf " is encountered in building one of the subdirectories. \n"
printf " (3) For example -j2 will utilize a 2nd core in the build \n"
printf " if your machine has two cores. -j4 etc for quad core. \n"
exit 0;
elif [ "${ARGI}" = "--debug" -o "${ARGI}" = "-d" ] ; then
BUILD_TYPE="Debug"
elif [ "${ARGI}" = "--release" -o "${ARGI}" = "-r" ] ; then
BUILD_TYPE="Release"
else
CMD_LINE_ARGS=$CMD_LINE_ARGS" "$ARGI
fi
done
#-------------------------------------------------------------------
# Part 2: Invoke the call to make in the build directory
#-------------------------------------------------------------------
mkdir -p build
cd build
cmake -DCMAKE_BUILD_TYPE=${BUILD_TYPE} ../
make ${CMD_LINE_ARGS}
cd ${INVOCATION_ABS_DIR}

15
clean.sh Executable file
View File

@@ -0,0 +1,15 @@
#!/bin/bash
rm -rf build/*
rm -rf lib/*
rm -rf bin/p*
rm -f .DS_Store
rm -f missions/*/.LastOpenedMOOSLogDirectory
find . -name '.DS_Store' -print -exec rm -rfv {} \;
find . -name '*~' -print -exec rm -rfv {} \;
find . -name '#*' -print -exec rm -rfv {} \;
find . -name '*.moos++' -print -exec rm -rfv {} \;
find . -name 'MOOSLog*' -print -exec rm -rfv {} \;

181
launch/alpha.bhv Normal file
View File

@@ -0,0 +1,181 @@
//-------- FILE: alpha.bhv -------------
initialize RUN = false
initialize TaskNum=t1
initialize SendTask=false
//--------------模式判断------------------------
set MODE = ACTIVE{
RUN=true
} INACTIVE
set MODE = T1{
MODE=ACTIVE
TaskNum = t1
}
//----------路径点任务----------------------------
Behavior = BHV_Waypoint
{
name = waypt_survey
pwt = 100 //优先权重
condition = MODE==T1
//endflag = START=false
endflag = END_WayPoint=true
configflag = CRUISE_SPD = $[SPEED]
//configflag = OSPOS = $[OSX],$[OSY]
activeflag = INFO=$[OWNSHIP]
activeflag = INFO=$[BHVNAME]
activeflag = INFO=$[BHVTYPE]
//cycleflag = CINFO=$[OSX],$[OSY]
wptflag = CurrentPointComplete=true
wptflag = PREV=$(PX),$(PY)
wptflag = NEXT=$(NX),$(NY)
wptflag = TEST=$(X),$(Y)
wptflag = OSPOS=$(OSX),$(OSY)
//wptflag_on_start = true
updates = WPT_UPDATE
//perpetual = true
templating = spawn
// speed_alt = 1.2
//use_alt_speed = true
lead = 8
lead_damper = 1
lead_to_start = false
speed = 1 // meters per second
capture_line = true
capture_radius = 5.0
slip_radius = 15.0
efficiency_measure = all
polygon = 60,-40
order = normal
//repeat = 3
visual_hints = nextpt_color=yellow
visual_hints = nextpt_vertex_size=8
visual_hints = nextpt_lcolor=gray70
visual_hints = vertex_color=dodger_blue, edge_color=white
visual_hints = vertex_size=5, edge_size=1
}
//--------------定深任务------------------
Behavior=BHV_ConstantDepth
{
name = const_depth
pwt = 100
//condition = DEPLOY = true
condition = MODE==T1
duration = no-time-limit
updates = DEPTH_UPDATE
depth = 0
}
//--------------定向任务--------------------
Behavior=BHV_ConstantHeading
{
name = const_heading
pwt = 100
//condition = START_TURN = true
//condition = DEPLOY = true
condition = MODE==T3
perpetual = true
activeflag = TURN = started
//endflag = TURN = done
//endflag = RETURN = true
//endflag = START_TURN = false
endflag = START=false
heading = 225
complete_thresh = 5
duration = no-time-limit
}
//--------------定速任务--------------------
Behavior=BHV_ConstantSpeed
{
name = const_speed
pwt = 1000
condition = MODE==T1
perpetual = true
updates = SPEED_UPDATE
//endflag = START=false
speed = 5
duration = no-time-limit
//peakwidth = 0.5
//basewidth = 0.5
}
//----------------安全模式-----------------------
//----------------计时器---------------------
Behavior = BHV_Timer
{
name = mtime
condition = MODE==T1
pwt = 100
templating = spawn
//duration_status = MSTATUS
//duration = 10
endflag = TIME_OUT=true
updates = TIMER_UPDATES
//perpetual = true
}
//-------------最大深度限制--------------------------
Behavior = BHV_MaxDepth
{
name = maxdepth
pwt = 200
condition = MODE==ACTIVE
updates = MAXDEEP_UPDATES
max_depth = 20
tolerance = 0
duration = no-time-limit
}
//--------------安全区域设置-----------------------
Behavior = BHV_OpRegion
{
// General Behavior Parameters
// ---------------------------
name = op_region // example
pwt = 300 // default
condition = MODE==TN
updates = OPREGION_UPDATES // example
// Parameters specific to this behavior
// ------------------------------------
max_time = 20 // default (seconds)
max_depth = 25 // default (meters)
min_altitude = 0 // default (meters)
reset_var = OPREGION_RESET // example
trigger_entry_time = 1 // default (seconds)
trigger_exit_time = 0.5 // default (seconds)
polygon = pts={-80,-00:-30,-175:150,-100:95,25}
breached_altitude_flag = TaskFault = AltitudeOut
breached_depth_flag = TaskFault = DepthOut
breached_poly_flag = TaskFault = RegionOut
breached_time_flag = TaskFault = TimeOut
visual_hints = vertex_color = brown // default
visual_hints = vertex_size = 3 // default
visual_hints = edge_color = aqua // default
visual_hints = edge_size = 1 // default
}

306
launch/launch.moos Normal file
View File

@@ -0,0 +1,306 @@
//======================================
//1. 在phare配置块里面添加需要的变量和调试端端口配置
//2. Our define process中加入自定义程序
//3. For test process 中加入配合调试的程序
ServerHost = localhost
ServerPort = 9000
Simulator = true
Community = pi
MOOSTimeWarp = 1
LatOrigin = 43.825300
LongOrigin = -70.330400
AltOrigin = 0
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
//============MOOS process=========================
Run = MOOSDB @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pRealm @ NewConsole = false
Run = pShare @ NewConsole = false
//Run = pMarineViewer @ NewConsole = false
//Run = pLogger @ NewConsole = false
//===========Our define process====================
Run = pBoardSupportComm @ NewConsole = false
Run = pTaskManger @ NewConsole = false
Run = pMotionControler @ NewConsole = false
//Run = pSurfaceSupportComm @ NewConsole = false
Run = pDataManagement @ NewConsole = false
//===============For test process===================
Run = pEmulator @ NewConsole = false
//Run = uSimMarine @ NewConsole = false
//Run = pMarinePID @ NewConsole = false
}
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
vehicles_name_mode = names+depth //+shortmode
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = xlarge
procs_font_size = xlarge
appcast_font_size = large
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
//button_one = START # START=true
button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
//button_one = MOOS_MANUAL_OVERRIDE=false
button_two = STOP # uMission_action_cmd={"taskName":"east_waypt_survey","action":"stop"}
//button_two = MOOS_MANUAL_OVERRIDE=true
//button_three = FaultClear # ClearFalut = true
//button_four = SendSecurityZone # SendSaftRules = true
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
cmd = label=DEPLOY, var=DEPLOY, sval=true, receivers=all:$(VNAMES)
cmd = label=DEPLOY, var=MOOS_MANUAL_OVERRIDE, sval=false, receivers=all:$(VNAMES)
cmd = label=DEPLOY, var=AVOID, sval=true, receivers=all:$(VNAMES)
cmd = label=DEPLOY, var=RETURN, sval=false, receivers=all:$(VNAMES)
cmd = label=DEPLOY, var=STATION_KEEP, sval=false, receivers=all:$(VNAMES)
cmd = label=RETURN, var=RETURN, sval=true, receivers=all:$(VNAMES)
cmd = label=RETURN, var=STATION_KEEP, sval=false, receivers=all:$(VNAMES)
cmd = label=PERMUTE, var=UTS_FORWARD, dval=0, receivers=shore
cmd = label=STATION, var=STATION_KEEP, sval=true, receivers=all:$(VNAMES), color=pink
cmd = label=LOITER-FAST, var=UP_LOITER, sval=speed=2.8, receivers=all:$(VNAMES)
cmd = label=LOITER-SLOW, var=UP_LOITER, sval=speed=1.4, receivers=all:$(VNAMES)
cmd = label=LOITER-CLOCKWISE-TRUE, var=UP_LOITER, sval=clockwise=true, receivers=all:$(VNAMES)
cmd = label=LOITER-CLOCKWISE-FALSE, var=UP_LOITER, sval=clockwise=false, receivers=all:$(VNAMES)
cmd = label=LOITER-CLOCKWISE-BEST, var=UP_LOITER, sval=clockwise=false, receivers=all:$(VNAMES)
}
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uQueryDB*
nowatch = uXMS*
nowatch = uMAC*
nowatch = pShare*
nowatch = pRealm*
nowatch = pNodeReporter*
}
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
platform_type = UUV
platform_color = red
platform_length = 4
}
ProcessConfig = pShare
{
AppTick = 2
CommsTick = 2
//UUV 信息
output = src_name = NODE_REPORT*, route = 10.25.0.160:8085
output = src_name = NODE_REPORT*, route = 10.25.0.248:8085
output = src_name = NODE_REPORT*, route = 10.25.0.165:8085
//App 信息
output = src_name = APPCAST*, route = 10.25.0.160:8085
output = src_name = APPCAST*, route = 10.25.0.165:8085
output = src_name = APPCAST*, route = 10.25.0.248:8085
//路径点信息
output = src_name = VIEW*, route = 10.25.0.160:8085
output = src_name = VIEW*, route = 10.25.0.165:8085
output = src_name = VIEW*, route = 10.25.0.248:8085
//调试端输入端口
input = route = localhost:8081
input = route = localhost:8082
input = route = localhost:8083
}
ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
planConfigPath = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/PlanConfigure.json
//planConfigPath = /home/ben/project/moos-ivp-pi/PlanConfigure.json
}
ProcessConfig = pBoardSupportComm
{
AppTick = 5
CommsTick = 5
}
ProcessConfig = uSimMarine
{
AppTick = 10
CommsTick = 10
START_X = 0
START_Y = 0
START_SPEED = 0
START_HEADING = 180
PREFIX = NAV
}
ProcessConfig = pMarinePID
{
AppTick = 20
CommsTick = 20
VERBOSE = true
DEPTH_CONTROL = false
// Yaw PID controller
YAW_PID_KP = 0.5
YAW_PID_KD = 0.0
YAW_PID_KI = 0.0
YAW_PID_INTEGRAL_LIMIT = 0.07
// Speed PID controller
SPEED_PID_KP = 1.0
SPEED_PID_KD = 0.0
SPEED_PID_KI = 0.0
SPEED_PID_INTEGRAL_LIMIT = 0.07
//MAXIMUMS
MAXRUDDER = 100
MAXTHRUST = 100
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
SPEED_FACTOR = 20
}
ProcessConfig = pMotionControler
{
AppTick = 5
CommsTick = 5
tardy_nav_thresh = 2.0
tardy_helm_thresh = 2.0
cheak_stalensee = true
delta_freqency = 5
config_file = /home/jhl/moos-ivp-pi/moos-ivp-pi-word/src/pMotionControler/ControlParam.json
}
ProcessConfig = pEmulator
{
AppTick = 100
CommsTick = 5
matlab_host = 10.25.0.137
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 10
start_y = 9
start_z = 1
start_heading = 30
}
ProcessConfig = pDataManagement
{
AppTick = 4
CommsTick = 4
}
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
FileTimeStamp = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
Log = REPORT @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}

11
missions/alder/README Normal file
View File

@@ -0,0 +1,11 @@
To Run this mission, make sure that the following two bin
directories are in your path:
moos-ivp/ivp/bin/
moos-ivp-extend/bin/
Then launch the mission by:
pAntler alder.moos

33
missions/alder/alder.bhv Normal file
View File

@@ -0,0 +1,33 @@
//-------- FILE: alder.bhv -------------
initialize DEPLOY = false
initialize RETURN = false
//----------------------------------------------
Behavior = BHV_SimpleWaypoint
{
name = waypt_to_point
pwt = 100
condition = RETURN = false
condition = DEPLOY = true
endflag = RETURN = true
speed = 2.0 // meters per second
radius = 8.0
ptx = 100
pty = -50
}
//----------------------------------------------
Behavior = BHV_Waypoint
{
name = waypt_return
pwt = 100
condition = (RETURN = true)
condition = (DEPLOY = true)
speed = 2.0
radius = 8.0
point = 0,0
}

166
missions/alder/alder.moos Normal file
View File

@@ -0,0 +1,166 @@
// Level 2 Configuration file: M. Benjamin
ServerHost = localhost
ServerPort = 9000
Simulator = true
Community = alder
MOOSTimeWarp = 1
LatOrigin = 43.825300
LongOrigin = -70.330400
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
Run = uSimMarine @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pMarinePID @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
Run = pOdometry @ NewConsole = false
Run = pBoardSupportComm @ NewConsole = false
}
//------------------------------------------
// uSimMarine config block
ProcessConfig = uSimMarine
{
AppTick = 10
CommsTick = 10
START_X = 0
START_Y = 0
START_SPEED = 0
START_HEADING = 180
PREFIX = NAV
}
//------------------------------------------
// uProcessWatch config block
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
term_report_interval = 0.8
summary_wait = 5
nowatch = uXMS*
nowatch = uPokeDB*
nowatch = uTermCommand*
watch_all = true
}
//------------------------------------------
// pHelmIvP config block
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
Behaviors = alder.bhv
Verbose = quiet
Domain = course:0:359:360
Domain = speed:0:4:21
IVP_BEHAVIOR_DIR = ../../lib
//IVP_BEHAVIOR_DIR = /Users/mikerb/Research/moos-ivp-extend/trunk/lib
ok_skew = any
start_in_drive = false
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMarinePID
{
AppTick = 20
CommsTick = 20
VERBOSE = true
DEPTH_CONTROL = false
// Yaw PID controller
YAW_PID_KP = 0.5
YAW_PID_KD = 0.0
YAW_PID_KI = 0.0
YAW_PID_INTEGRAL_LIMIT = 0.07
// Speed PID controller
SPEED_PID_KP = 1.0
SPEED_PID_KD = 0.0
SPEED_PID_KI = 0.0
SPEED_PID_INTEGRAL_LIMIT = 0.07
//MAXIMUMS
MAXRUDDER = 100
MAXTHRUST = 100
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
SPEED_FACTOR = 20
}
//------------------------------------------
// pMarineViewer config block
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
TIFF_FILE = forrest19.tif
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.4
hash_viewable = true
scope = ODOMETRY_DIST
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = medium
procs_font_size = medium
appcast_font_size = small
BUTTON_ONE = DEPLOY # DEPLOY=true
BUTTON_ONE = MOOS_MANUAL_OVERIDE=false # RETURN=false
BUTTON_TWO = RETURN # RETURN=true
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
VESSEL_TYPE = KAYAK
}
ProcessConfig = pBoardSupportComm
{
AppTick = 4
CommsTick = 4
}

41
missions/alder/alder_orig.bhv Normal file
View File

@@ -0,0 +1,41 @@
//-------- FILE: alder.bhv -------------
initialize DEPLOY = false
initialize RETURN = false
//----------------------------------------------
Behavior = BHV_SimpleWaypoint
{
name = waypt_to_point
pwt = 100
condition = RETURN = false
condition = DEPLOY = true
endflag = RETURN = true
speed = 2.0 // meters per second
radius = 8.0
ptx = 100
pty = -50
}
//----------------------------------------------
Behavior = BHV_SimpleWaypoint
{
name = waypt_return
pwt = 100
condition = (RETURN = true)
condition = (DEPLOY = true)
speed = 2.0
radius = 8.0
ptx = 0
pty = 0
}
//----------------------------------------------
Behavior = BHV_HSLine
{
name = hsline
time_on_leg = 20
}

93
missions/s3_alpha/alpha.bhv Executable file
View File

@@ -0,0 +1,93 @@
//-------- FILE: alpha.bhv -------------
initialize DEPLOY = false
initialize RETURN = false
initialize REGION = east
//----------------------------------------------
Behavior = BHV_Waypoint
{
name = waypt_survey
pwt = 100
condition = RETURN = false
condition = DEPLOY = true
condition = REGION = east
endflag = REGION = west
updates = WPT_UPDATE
perpetual = true
lead = 8
lead_damper = 1
lead_to_start = true
speed = 3 // meters per second
capture_line = true
capture_radius = 5.0
slip_radius = 15.0
efficiency_measure = all
polygon = 60,-40 : 60,-160 : 150,-160 : 180,-100 : 150,-40
order = normal
repeat = forever
visual_hints = nextpt_color=yellow
visual_hints = nextpt_vertex_size=8
visual_hints = nextpt_lcolor=gray70
visual_hints = vertex_color=dodger_blue, edge_color=white
visual_hints = vertex_size=5, edge_size=1
}
//----------------------------------------------
Behavior = BHV_Waypoint
{
name = west_waypt_survey
pwt = 100
condition = RETURN = false
condition = DEPLOY = true
condition = REGION = west
endflag = RETURN = true
updates = WPT_UPDATE
perpetual = true
lead = 8
lead_damper = 1
lead_to_start = true
speed = 3 // meters per second
capture_line = true
capture_radius = 5.0
slip_radius = 15.0
efficiency_measure = all
points = -35,-65 : -35,-125 : 20,-125 : 20, -65
order = normal
repeat = forever
visual_hints = nextpt_color=yellow
visual_hints = nextpt_vertex_size=8
visual_hints = nextpt_lcolor=gray70
visual_hints = vertex_color=dodger_blue, edge_color=white
visual_hints = vertex_size=5, edge_size=1
}
//----------------------------------------------
Behavior=BHV_Waypoint
{
name = waypt_return
pwt = 100
condition = RETURN = true
condition = DEPLOY = true
perpetual = true
updates = RETURN_UPDATE
endflag = RETURN = false
endflag = DEPLOY = false
endflag = MISSION = complete
speed = 2.0
capture_radius = 2.0
slip_radius = 8.0
points = 0,-20
}

310
missions/s3_alpha/alpha.moos Executable file
View File

@@ -0,0 +1,310 @@
/-------------------------------------------------
// NAME: M. Benjamin, MIT CSAIL
// FILE: alpha.moos
//-------------------------------------------------
ServerHost = localhost
ServerPort = 9000
Community = alpha
MOOSTimeWarp = 1
// Forest Lake
LatOrigin = 43.825300
LongOrigin = -70.330400
// MIT Sailing Pavilion (use this one)
// LatOrigin = 42.358456
// LongOrigin = -71.087589
ReferenceLongitude = LongOrigin
ReferenceLatitude = LatOrigin
ReferenceAltitude = 0
//ReferenceLongitude = -70.330400
//ReferenceLatitude = 43.825300
//ReferenceAltitude = 0
PlanConfigPath = "PlanConfigure.json"
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
//Run = pLogger @ NewConsole = false
Run = uSimMarineV22 @ NewConsole = false
Run = pMarinePIDV22 @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pBoardSupportComm @ NewConsole = false
Run = pSurfaceSupportComm @ NewConsole = false
//Run = pDataManagement @ NewConsole = false
//Run = pClientViewer @ NewConsole = false
}
//------------------------------------------
// pLogger config block
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
FileTimeStamp = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
Log = REPORT @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
//------------------------------------------
// uProcessWatch
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uXMS*
}
//------------------------------------------
// uSimMarine config block
ProcessConfig = uSimMarine
{
AppTick = 5
CommsTick = 5
start_x = 0
start_y = -40
start_heading = 180
start_speed = 0
prefix = NAV
turn_rate = 40 Run = pNodeReporter @ NewConsole = false
thrust_map = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5
thrust_reflect = true
}
//------------------------------------------
// uSimMarineV22 config block
ProcessConfig = uSimMarineV22
{
AppTick = 4
CommsTick = 4
start_pos = x=0, y=-20, heading=180, speed=0
prefix = NAV
turn_rate = 40
thrust_map = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5
//thrust_reflect = true
}
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
bhv_dir_not_found_ok = true
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:4:41
}
//------------------------------------------
// pMarinePID config block Run = pNodeReporter @ NewConsole = false
ProcessConfig = pMarinePID
{
AppTick = 20
CommsTick = 20
verbose = true
depth_control = false
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 1.2
yaw_pid_kd = 0.0
yaw_pid_ki = 0.3
yaw_pid_integral_limit = 0.07
// Speed PID controller
speed_pid_kp = 1.0
speed_pid_kd = 0.0
speed_pid_ki = 0.0
speed_pid_integral_limit = 0.07
//MAXIMUMS
maxrudder = 100
maxthrust = 100
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 20
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMarinePIDV22
{
AppTick = 20
CommsTick = 20
verbose = true
depth_control = false
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 1.2
yaw_pid_kd = 0.0
yaw_pid_ki = 0.3
yaw_pid_integral_limit = 0.07
// Speed PID controller
speed_pid_kp = 1.0
speed_pid_kd = 0.0
speed_pid_ki = 0.0
speed_pid_integral_limit = 0.07
//MAXIMUMS
maxrudder = 100
maxthrust = 100
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 20
}
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// vcolor = alpha=dodgerblue
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = medium
procs_font_size = medium
appcast_font_size = small
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
button_one = DEPLOY # DEPLOY=true
button_one = MOOS_MANUAL_OVERRIDE=false # RETURN=false
button_two = RETURN # RETURN=true
button_three = WEST # REGION=west
button_four = EAST # REGION=east
button_five = SLOWER # WPT_UPDATE=speed=1.5
button_six = FASTER # WPT_UPDATE=speed=3.5
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
platform_type = kayak
platform_color = yellow
platform_length = 4
}
ProcessConfig = pBoardSupportComm
{
AppTick = 4
CommsTick = 4
}
ProcessConfig = pSurfaceSupportComm
{
AppTick = 4
CommsTick = 4
}
ProcessConfig = pDataManagement
{
AppTick = 4
CommsTick = 4
}
ProcessConfig = pClientViewer
{
AppTick = 4
CommsTick = 4
}

6
missions/s3_alpha/clean.sh Executable file
View File

@@ -0,0 +1,6 @@
#!/bin/bash
rm -rf MOOSLog_*
rm -f *~
rm -f *.moos++
rm -f .LastOpenedMOOSLogDirectory

34
missions/s3_alpha/launch.sh Executable file
View File

@@ -0,0 +1,34 @@
#!/bin/bash -e
COMMUNITY="alpha"
#-------------------------------------------------------
# Part 1: Check for and handle command-line arguments
#-------------------------------------------------------
TIME_WARP=10
for ARGI; do
if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then
printf "%s [SWITCHES] [time_warp] \n" $0
printf " --help, -h \n"
exit 0;
elif [ "${ARGI//[^0-9]/}" = "$ARGI" -a "$TIME_WARP" = 1 ]; then
TIME_WARP=$ARGI
else
printf "Bad Argument: %s \n" $ARGI
exit 0
fi
done
#-------------------------------------------------------
# Part 2: Launch the processes
#-------------------------------------------------------
printf "Launching the %s MOOS Community (WARP=%s) \n" $COMMUNITY $TIME_WARP
pAntler $COMMUNITY.moos --MOOSTimeWarp=$TIME_WARP >& /dev/null &
uMAC -t $COMMUNITY.moos
printf "Killing all processes ... \n"
kill %1
mykill
printf "Done killing processes. \n"

54
missions/xrelay/xrelay.moos Normal file
View File

@@ -0,0 +1,54 @@
// MOOS file
ServerHost = localhost
ServerPort = 9000
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
Run = pXRelay @ NewConsole = true ~ pXRelay_APPLES
Run = pXRelay @ NewConsole = true ~ pXRelay_PEARS
Run = uXMS @ NewConsole = true
}
//------------------------------------------
// First pXRelay configuration block
ProcessConfig = pXRelay_APPLES
{
AppTick = 4
CommsTick = 4
OUTGOING_VAR = APPLES
INCOMING_VAR = PEARS
}
//------------------------------------------
// Second pXRelay configuration block
ProcessConfig = pXRelay_PEARS
{
AppTick = 4
CommsTick = 4
OUTGOING_VAR = PEARS
INCOMING_VAR = APPLES
}
//------------------------------------------
// uXMS configuration block
ProcessConfig = uXMS
{
AppTick = 4
CommsTick = 4
VAR = PEARS, PEARS_ITER_HZ, PEARS_POST_HZ
VAR = APPLES, APPLES_ITER_HZ, APPLES_POST_HZ
}

5
scripts/README Normal file
View File

@@ -0,0 +1,5 @@
This directory may contain developer utility scripts.
The MyGenMOOSApp script is similar to that found in the moos-ivp/trunk/scripts/
directory. Users are encouraged to tailor this one to their own needs.

29
src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,29 @@
##############################################################################
# FILE: moos-ivp-extend/src/CMakeLists.txt
# DATE: 2010/09/07
# 2020/05/09 minor mods
# DESCRIPTION: CMakeLists.txt file for the moos-ivp-extend source directory
##############################################################################
#============================================================================
# Add the libraries in the current directory to the include path
#============================================================================
FILE(GLOB LOCAL_LIBRARY_DIRS ./lib_*)
INCLUDE_DIRECTORIES(${LOCAL_LIBRARY_DIRS}/* ${LOCAL_LIBRARY_DIRS}/DUNE/)
#============================================================================
# List the subdirectories to build...
#============================================================================
ADD_SUBDIRECTORY(pBoardSupportComm)
ADD_SUBDIRECTORY(pSurfaceSupportComm)
ADD_SUBDIRECTORY(pClientViewer)
ADD_SUBDIRECTORY(pDataManagement)
ADD_SUBDIRECTORY(pTaskManger)
ADD_SUBDIRECTORY(pTaskSend)
ADD_SUBDIRECTORY(pMotionControler)
ADD_SUBDIRECTORY(pEmulator)
##############################################################################
# END of CMakeLists.txt
##############################################################################

785
src/pBoardSupportComm/BoardSupportComm.cpp Normal file
View File

@@ -0,0 +1,785 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: BoardSupportComm.cpp */
/* DATE: */
/************************************************************/
#include <iterator>
#include "BoardSupportComm.h"
#include <cmath>
#define TCP_RECEIVE_PORT 8001
#define TCP_SERVER_ADDRESS "127.0.0.1"
// #define MOOS_AUV_SIM
#define MATLAB_AUV_SIM
// #ifdef TRUE_AUV
using namespace std;
BoardSupportComm::BoardSupportComm()
{
// uMotion_posX_fb = 0;
// uMotion_posY_fb = 0;
// uMotion_posZ_fb = 0;
// uMotion_roll_fb = 0;
// uMotion_pitch_fb = 0;
// uMotion_yaw_fb = 0;
// uMotion_longitude_fb = 0;
// uMotion_latitude_fb = 0;
// uMotion_velocityX_fb = 0;
// uMotion_velocityY_fb = 0;
// uMotion_velocityZ_fb = 0;
// uMotion_deep_fb = 0;
estimatedState.info = "lauv-150";
estimatedState.currentLon = 0;
estimatedState.currentLat = 0;
estimatedState.currentAltitude = 0;
estimatedState.referenceLon = 0;
estimatedState.referenceLat = 0;
estimatedState.referenceAltitude = 0;
estimatedState.offsetNorth = 0;
estimatedState.offsetEast = 0;
estimatedState.offsetDown = 0;
estimatedState.roll = 0;
estimatedState.pitch = 0;
estimatedState.yaw = 0;
estimatedState.linearVelocityNorth = 0;
estimatedState.linearVelocityEast = 0;
estimatedState.linearVelocityDown = 0;
estimatedState.height = 0;
estimatedState.depth = 0;
executeCommand.header = 0xEBA2; //1:[0,1]
executeCommand.count = 16; //2:[2,3]
executeCommand.size = 21; //3:[4]
executeCommand.drive_mode = 0xFF; //4:[5]
executeCommand.thrust = 0; //5:[6]
executeCommand.yaw = 0; //6:[7,8]
executeCommand.depth = 0; //7:[9,10]
executeCommand.helm_top_angle = 0; //8:[11]
executeCommand.helm_bottom_angle = 0; //9:[12]
executeCommand.helm_left_angle = 0; //10:[13]
executeCommand.helm_right_angle = 0; //11:[14]
executeCommand.light_enable = 0; //12:[15]
executeCommand.dvl_enable = 0; //13:[16]
executeCommand.throwing_load_enable = 0; //14:[17]
executeCommand.crc = 0; //15:[18]
executeCommand.footer = 0xEE2A; //16:[19,20]
executeCommand.manual_mode = false;
tcpSockFD = -1;
tcpConnectRet = -1;
}
//---------------------------------------------------------
// Destructor
BoardSupportComm::~BoardSupportComm()
{
// delete tcpReceiveBuffer;
close(tcpSockFD);
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool BoardSupportComm::OnNewMail(MOOSMSG_LIST &NewMail)
{
// AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
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();
#ifdef MOOS_AUV_SIM
if(key == "NAV_X")
{
//E->N
estimatedState.offsetNorth = msg.GetDouble();
}
if(key == "NAV_Y")
{
//N->E
estimatedState.offsetEast = msg.GetDouble();
}
if(key == "NAV_Z")
{
//U->D
estimatedState.offsetDown = -msg.GetDouble();
}
if(key == "NAV_YAW")
{
estimatedState.yaw = -msg.GetDouble();
}
if(key == "NAV_PITCH")
{
estimatedState.pitch = msg.GetDouble();
}
if(key == "NAV_LAT")
{
estimatedState.currentLat = msg.GetDouble();
}
if(key == "NAV_LONG")
{
estimatedState.currentLon = msg.GetDouble();
}
if(key == "NAV_SPEED")
{
estimatedState.linearVelocityNorth = msg.GetDouble() * cos(estimatedState.yaw);
estimatedState.linearVelocityEast = -msg.GetDouble() * sin(estimatedState.yaw);
estimatedState.linearVelocityDown = 0;
}
if(key == "NAV_DEPTH")
{
estimatedState.depth = msg.GetDouble();
}
#endif
if(key == "uManual_enable_cmd")
{
if (msg.GetDouble() == 1.0)
{
executeCommand.drive_mode = 0x02;
executeCommand.manual_mode = true;
}
else
{
executeCommand.drive_mode = 0xFF;
executeCommand.manual_mode = false;
}
}
if(key == "uManual_drive_cmd")
{
if (executeCommand.manual_mode)
{
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
{
std::cerr << "Failed to parse JSON string." << std::endl;
return false;
}
int thrust = recvCommand["Thrust"].asInt();
if ((thrust < -100) or (thrust > 100))
{
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;
}
executeCommand.depth = 0;
executeCommand.helm_top_angle = 0;
executeCommand.helm_bottom_angle = 0;
executeCommand.helm_left_angle = 0;
executeCommand.helm_right_angle = 0;
int serializeResult = serializeMessage(tcpSendBuffer);
if ((serializeResult == 0) && (tcpSockFD != -1))
{
try
{
write(tcpSockFD, tcpSendBuffer, executeCommand.size);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
}
}
}
if(key == "uMotion_control_cmd")
{
if (executeCommand.manual_mode)
{
return false;
}
// if (executeCommand.drive_mode != 0x02)
// {
// return false;
// }
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
{
std::cerr << "Failed to parse JSON string." << std::endl;
return false;
}
executeCommand.thrust = convertIntToUchar(recvCommand["thrust"].asInt(), -100, 100);
executeCommand.helm_top_angle = convertIntToUchar(recvCommand["rudder"].asInt(), -30, 30);
executeCommand.helm_bottom_angle = convertIntToUchar(recvCommand["rudder"].asInt(), -30, 30);
executeCommand.helm_left_angle = convertIntToUchar(recvCommand["elevator"].asInt(), -30, 30);
executeCommand.helm_right_angle = convertIntToUchar(recvCommand["elevator"].asInt(), -30, 30);
int serializeResult = serializeMessage(tcpSendBuffer);
// std::cout << "serializeResult: " << serializeResult << std::endl;
// printf("header: %u, %u\n", tcpSendBuffer[0], tcpSendBuffer[1]);
// printf("count: %u\n", tcpSendBuffer[2], tcpSendBuffer[3]);
// printf("size: %u\n", tcpSendBuffer[4]);
// printf("drive_mode: %u\n", tcpSendBuffer[5]);
// printf("thrust: %u\n", tcpSendBuffer[6]);
// printf("yaw: %u, %u\n", tcpSendBuffer[7], tcpSendBuffer[8]);
// printf("depth: %u, %u\n", tcpSendBuffer[9], tcpSendBuffer[10]);
// printf("helm_top_angle: %u\n", tcpSendBuffer[11]);
// printf("helm_bottom_angle: %u\n", tcpSendBuffer[12]);
// printf("helm_left_angle: %u\n", tcpSendBuffer[13]);
// printf("helm_right_angle: %u\n", tcpSendBuffer[14]);
// printf("light_enable: %u\n", tcpSendBuffer[15]);
// printf("dvl_enable: %u\n", tcpSendBuffer[16]);
// printf("throwing_load_enable: %u\n", tcpSendBuffer[17]);
// printf("crc: %u\n", tcpSendBuffer[18]);
// printf("footer: %u, %u\n", tcpSendBuffer[19], tcpSendBuffer[20]);
if ((serializeResult == 0) && (tcpSockFD != -1))
{
try
{
write(tcpSockFD, tcpSendBuffer, executeCommand.size);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
}
}
if(key == "uMission_origin_cmd")
{
std::string err;
Json::Value recvCommand;
std::istringstream iss(msg.GetString());
Json::CharReaderBuilder builder;
bool parsingResult = Json::parseFromStream(builder, iss, &recvCommand, &err);
if (!parsingResult)
{
std::cerr << "Failed to parse JSON string." << std::endl;
return false;
}
estimatedState.referenceAltitude = recvCommand["altitude"].asFloat();
estimatedState.referenceLat = recvCommand["lat"].asFloat();
estimatedState.referenceLon = recvCommand["lon"].asFloat();
}
}
return(true);
}
uint8_t BoardSupportComm::convertIntToUchar(int src, int min, int max)
{
uint8_t dst;
if (src < 0)
{
if (src < min)
{
dst = std::abs(min) + 0B10000000;
}
else
{
dst = std::abs(src) + 0B10000000;
}
}
else
{
if (src > max)
{
dst = std::abs(max);
}
else
{
dst = std::abs(src);
}
}
return dst;
}
bool BoardSupportComm::OnConnectToServer()
{
RegisterVariables();
return(true);
}
std::string BoardSupportComm::convertEmbeddedFormat(Json::Value &embeddedState)
{
#ifdef MOOS_AUV_SIM
embeddedState["driveMode"] = 0xFF;
embeddedState["height"] = 0;
embeddedState["depth"] = 0;
embeddedState["yaw"] = estimatedState.yaw;
embeddedState["pitch"] = 0;
embeddedState["roll"] = 0;
embeddedState["insVX"] = estimatedState.linearVelocityNorth * 0.01;
embeddedState["insVY"] = estimatedState.linearVelocityEast * 0.01;
embeddedState["insVZ"] = estimatedState.linearVelocityDown * 0.01;
embeddedState["currentLon"] = estimatedState.currentLon * 0.000001;
embeddedState["currentLat"] = estimatedState.currentLat * 0.000001;
embeddedState["currentAltitude"] = 0;
embeddedState["dvlVX"] = 0;
embeddedState["dvlVY"] = 0;
embeddedState["dvlVZ"] = 0;
embeddedState["rpm"] = 0;
embeddedState["lightEnable"] = 0;
embeddedState["batteryVoltage"] = 0;
embeddedState["batteryLevel"] = 0;
embeddedState["batteryTemp"] = 0;
embeddedState["faultLeakSensor"] = 0;
embeddedState["faultBattery"] = 0;
embeddedState["faultEmergencyBattery"] = 0;
embeddedState["faultThrust"] = 0;
embeddedState["iridium"] = 0xFF;
embeddedState["throwingLoadEnable"] = 0xFF;
embeddedState["dvlStatus"] = 0;
#endif
#ifdef MATLAB_AUV_SIM
embeddedState["driveMode"] = embeddedInfo.drive_mode;
embeddedState["height"] = embeddedInfo.height * 0.01;
embeddedState["depth"] = embeddedInfo.depth * 0.01;
embeddedState["roll"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->N
embeddedState["pitch"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->E
embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D
embeddedState["insVX"] = embeddedInfo.ins_vx * 0.01; //N->N
embeddedState["insVY"] = embeddedInfo.ins_vy * 0.01; //E->E
embeddedState["insVZ"] = embeddedInfo.ins_vz * 0.01; //D->D
embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
embeddedState["dvlVX"] = embeddedInfo.dvl_vx * 0.01; //N->N
embeddedState["dvlVY"] = embeddedInfo.dvl_vy * 0.01; //E->E
embeddedState["dvlVZ"] = embeddedInfo.dvl_vz * 0.01; //D->D
embeddedState["rpm"] = embeddedInfo.rpm;
embeddedState["lightEnable"] = embeddedInfo.light_enable;
embeddedState["batteryVoltage"] = embeddedInfo.battery_voltage;
embeddedState["batteryLevel"] = embeddedInfo.battery_level;
embeddedState["batteryTemp"] = embeddedInfo.battery_temp * 0.1;
embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
embeddedState["faultBattery"] = embeddedInfo.fault_battery;
embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
embeddedState["iridium"] = embeddedInfo.iridium;
embeddedState["throwingLoadEnable"] = embeddedInfo.throwing_load;
embeddedState["dvlStatus"] = embeddedInfo.dvl_status;
#endif
#ifdef TRUE_AUV
embeddedState["driveMode"] = embeddedInfo.drive_mode;
embeddedState["height"] = embeddedInfo.height * 0.01;
embeddedState["depth"] = embeddedInfo.depth * 0.01;
embeddedState["roll"] = embeddedInfo.pitch * 0.01 * M_PI / 180; //E->N
embeddedState["pitch"] = embeddedInfo.roll * 0.01 * M_PI / 180; //N->E
embeddedState["yaw"] = embeddedInfo.yaw * 0.01 * M_PI / 180; //D->D
embeddedState["insVX"] = embeddedInfo.ins_vy * 0.01; //E->N
embeddedState["insVY"] = embeddedInfo.ins_vx * 0.01; //N->E
embeddedState["insVZ"] = -embeddedInfo.ins_vz * 0.01; //U->D
embeddedState["currentLon"] = embeddedInfo.lon * 0.000001;
embeddedState["currentLat"] = embeddedInfo.lat * 0.000001;
embeddedState["currentAltitude"] = embeddedInfo.alt * 0.01;
embeddedState["dvlVX"] = embeddedInfo.dvl_vy * 0.01; //E->N
embeddedState["dvlVY"] = embeddedInfo.dvl_vx * 0.01; //N->E
embeddedState["dvlVZ"] = -embeddedInfo.dvl_vz * 0.01; //U->D
embeddedState["rpm"] = embeddedInfo.rpm;
embeddedState["lightEnable"] = embeddedInfo.light_enable;
embeddedState["batteryVoltage"] = embeddedInfo.battery_voltage;
embeddedState["batteryLevel"] = embeddedInfo.battery_level;
embeddedState["batteryTemp"] = embeddedInfo.battery_temp * 0.1;
embeddedState["faultLeakSensor"] = embeddedInfo.fault_leakSensor;
embeddedState["faultBattery"] = embeddedInfo.fault_battery;
embeddedState["faultEmergencyBattery"] = embeddedInfo.fault_emergencyBattery;
embeddedState["faultThrust"] = embeddedInfo.fault_thrust;
embeddedState["iridium"] = embeddedInfo.iridium;
embeddedState["throwingLoadEnable"] = embeddedInfo.throwing_load;
embeddedState["dvlStatus"] = embeddedInfo.dvl_status;
#endif
double currentLon = embeddedInfo.lon * 0.000001;
double currentLat = embeddedInfo.lat * 0.000001;
double currentAlt = embeddedInfo.alt * 0.01;
std::vector<double> reference = {estimatedState.referenceLon, estimatedState.referenceLat, estimatedState.referenceAltitude};
std::vector<double> current = {currentLon, currentLat, currentAlt};
std::vector<double> ned = {0, 0, 0};
ConvertLLAToNED(reference, current, ned);
embeddedState["north"] = ned.at(0);
embeddedState["east"]= ned.at(1);
Json::StreamWriterBuilder builder;
std::string embeddedStateString = Json::writeString(builder, embeddedState);
return embeddedStateString;
}
void BoardSupportComm::tcpProcessThread()
{
while(1)
{
//bzero(tcpReceiveBuffer, 0);
//int lens = read(tcpSockFD, (unsigned char* )tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
int lens = read(tcpSockFD, tcpReceiveBuffer, sizeof(tcpReceiveBuffer));
// std::cout << MOOS::Time() << " recv:" << lens << std::endl;
if(lens>0)
{
parseMessage((unsigned char* )tcpReceiveBuffer, lens);
// parseMessage(tcpReceiveBuffer, lens);
Json::Value embeddedState;
std::string embeddedStateString = convertEmbeddedFormat(embeddedState);
// std::cout << std::fixed << std::setprecision(6) << MOOS::Time() << embeddedStateString << std::endl;
Notify("uDevice_monitor_fb", embeddedStateString);
Notify("NAV_X", embeddedState["north"].asDouble());
Notify("NAV_Y", embeddedState["east"].asDouble());
Notify("NAV_Z", embeddedState["depth"].asDouble());
Notify("NAV_LAT", embeddedState["currentLat"].asDouble());
Notify("NAV_LONG", embeddedState["currentLon"].asDouble());
Notify("NAV_HEADING", embeddedState["yaw"].asDouble() * 180 / M_PI);
double driveSpeed = std::sqrt(std::pow((double)embeddedState["dvlVX"].asDouble(), 2)
+ std::pow((double)embeddedState["dvlVY"].asDouble(), 2)
+ std::pow((double)embeddedState["dvlVZ"].asDouble(), 2));
Notify("NAV_SPEED", driveSpeed);
Notify("NAV_DEPTH", embeddedState["depth"].asDouble());
Notify("NAV_ROLL", embeddedState["roll"].asDouble() * 180 / M_PI);
Notify("NAV_PITCH", embeddedState["pitch"].asDouble() * 180 / M_PI);
Notify("NAV_YAW", embeddedState["yaw"].asDouble() * 180 / M_PI);
}
}
}
// bool BoardSupportComm::buildReport()
// {
// m_msgs << "buildReport:" << embeddedStateString << endl;
// return true;
// }
bool BoardSupportComm::Iterate()
{
// AppCastingMOOSApp::Iterate();
if(tcpSockFD == -1)
{
tcpSockFD = socket(AF_INET, SOCK_STREAM, 0);
if(tcpSockFD == -1)
{
return false;
}
}
if(tcpConnectRet == -1)
{
struct sockaddr_in saddr;
//inet_pton(AF_INET, TCP_SERVER_ADDRESS, &saddr.sin_addr.s_addr);
saddr.sin_addr.s_addr = inet_addr(TCP_SERVER_ADDRESS);
saddr.sin_family = AF_INET;
saddr.sin_port = htons(TCP_RECEIVE_PORT);
tcpConnectRet = connect(tcpSockFD, (struct sockaddr *)&saddr, sizeof(saddr));
if(tcpConnectRet == -1)
{
return false;
}
}
if ((tcpSockFD != -1) && (tcpConnectRet != -1))
{
std::thread t1(&BoardSupportComm::tcpProcessThread, this);
t1.detach();
}
#if 0
printf("header: %hx\n", embeddedInfo.header);
printf("count: %hu\n", embeddedInfo.count);
printf("size: %u\n", embeddedInfo.size);
printf("drive_mode: %u\n", embeddedInfo.drive_mode);
printf("height: %hu\n", embeddedInfo.height);
printf("depth: %hu\n", embeddedInfo.depth);
printf("yaw: %hu\n", embeddedInfo.yaw);
printf("pitch: %hd\n", embeddedInfo.pitch);
printf("roll: %hd\n", embeddedInfo.roll);
printf("ins_vx: %hd\n", embeddedInfo.ins_vx);
printf("ins_vy: %hd\n", embeddedInfo.ins_vy);
printf("ins_vz: %hd\n", embeddedInfo.ins_vz);
printf("lon: %d\n", embeddedInfo.lon);
printf("lat: %d\n", embeddedInfo.lat);
printf("alt: %hd\n", embeddedInfo.alt);
printf("dvl_vx: %hd\n", embeddedInfo.dvl_vx);
printf("dvl_vy: %hd\n", embeddedInfo.dvl_vy);
printf("dvl_vz: %hd\n", embeddedInfo.dvl_vz);
printf("rpm: %hd\n", embeddedInfo.rpm);
printf("light_enable: %u\n", embeddedInfo.light_enable);
printf("battery_voltage: %u\n", embeddedInfo.battery_voltage);
printf("battery_level: %u\n", embeddedInfo.battery_level);
printf("battery_temp: %u\n", embeddedInfo.battery_temp);
printf("fault_leakSensor: %hd\n", embeddedInfo.fault_leakSensor);
printf("fault_battery: %u\n", embeddedInfo.fault_battery);
printf("fault_emergencyBattery: %u\n", embeddedInfo.fault_emergencyBattery);
printf("fault_thrust: %u\n", embeddedInfo.fault_thrust);
printf("iridium: %u\n", embeddedInfo.iridium);
printf("throwing_load: %u\n", embeddedInfo.throwing_load);
printf("dvl_status: %u\n", embeddedInfo.dvl_status);
printf("crc: %u\n", embeddedInfo.crc);
printf("footer: %u\n", embeddedInfo.footer);
#endif
#if 0
executeCommand.header = 0xEBA2; //1:[0,1]
executeCommand.count = 16; //2:[2,3]
executeCommand.size = 21; //3:[4]
executeCommand.drive_mode = 0x02; //4:[5]
executeCommand.thrust = 178; //5:[6],赋值-50
executeCommand.yaw = 45.3 * 10; //6:[7,8],赋值45.3
executeCommand.depth = 298.6 * 10; //7:[9,10],赋值298.6
executeCommand.helm_top_angle = 145; //8:[11],赋值-17
executeCommand.helm_bottom_angle = 154; //9:[12],赋值-26
executeCommand.helm_left_angle = 9; //10:[13],赋值9
executeCommand.helm_right_angle = 158; //11:[14],赋值-30
executeCommand.light_enable = 1; //12:[15],赋值1
executeCommand.dvl_enable = 1; //13:[16],赋值1
executeCommand.throwing_load_enable = 1; //14:[17],赋值1
executeCommand.crc = 0; //15:[18]
executeCommand.footer = 0xEE2A; //16:[19,20]
int serializeResult = serializeMessage(tcpSendBuffer);
if (serializeResult == 0)
{
try
{
write(tcpSockFD, tcpSendBuffer, executeCommand.size);
// sock_tcp_send.write(tcpSendBuffer, executeCommand.size);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
}
#endif
//AppCastingMOOSApp::PostReport();
return true;
}
uint16_t BoardSupportComm::imcPollTCP(char* buffer, int bufferLength)
{
if (m_poll_1.poll(0))
{
bzero(buffer, 0);
uint16_t rv = sock_tcp_send.read(buffer, bufferLength);
return rv;
}
return 0;
}
template <typename Type>
inline uint16_t BoardSupportComm::serialize(const Type t, uint8_t* bfr, uint16_t& sumSize)
{
uint16_t size = sizeof(Type);
std::memcpy(bfr, &t, size);
sumSize += size;
return size;
}
int BoardSupportComm::serializeMessage(unsigned char* buffer)
{
uint8_t* ptr = buffer;
uint16_t sumSize = 0;
ptr += serialize(executeCommand.header, ptr, sumSize);
ptr += serialize(executeCommand.count, ptr, sumSize);
ptr += serialize(executeCommand.size, ptr, sumSize);
ptr += serialize(executeCommand.drive_mode, ptr, sumSize);
ptr += serialize(executeCommand.thrust, ptr, sumSize);
ptr += serialize(executeCommand.yaw, ptr, sumSize);
ptr += serialize(executeCommand.depth, ptr, sumSize);
ptr += serialize(executeCommand.helm_top_angle, ptr, sumSize);
ptr += serialize(executeCommand.helm_bottom_angle, ptr, sumSize);
ptr += serialize(executeCommand.helm_left_angle, ptr, sumSize);
ptr += serialize(executeCommand.helm_right_angle, ptr, sumSize);
ptr += serialize(executeCommand.light_enable, ptr, sumSize);
ptr += serialize(executeCommand.dvl_enable, ptr, sumSize);
ptr += serialize(executeCommand.throwing_load_enable, ptr, sumSize);
DUNE::Algorithms::CRC8 crc(0x07);
crc.putArray(buffer+CRC_COMPUTE_START_POS, CRC_COMPUTE_SUM_SIZE);
executeCommand.crc = crc.get();
ptr += serialize(executeCommand.crc, ptr, sumSize);
ptr += serialize(executeCommand.footer, ptr, sumSize);
if (sumSize != (uint16_t)executeCommand.size)
{
return -1;
}
return 0;
}
template <typename Type>
inline uint16_t BoardSupportComm::deserialize(Type& t, const uint8_t* bfr, uint16_t& length)
{
uint16_t size = sizeof(Type);
std::memcpy(&t, bfr, size);
length -= size;
return size;
}
int BoardSupportComm::parseMessage(unsigned char* buffer, int size)
{
const uint8_t* ptr = buffer;
uint16_t size__ = size;
uint16_t header;
ptr += deserialize(embeddedInfo.header, ptr, size__);
ptr += deserialize(embeddedInfo.count, ptr, size__);
ptr += deserialize(embeddedInfo.size, ptr, size__);
ptr += deserialize(embeddedInfo.drive_mode, ptr, size__);
ptr += deserialize(embeddedInfo.height, ptr, size__);
ptr += deserialize(embeddedInfo.depth, ptr, size__);
ptr += deserialize(embeddedInfo.yaw, ptr, size__);
ptr += deserialize(embeddedInfo.pitch, ptr, size__);
ptr += deserialize(embeddedInfo.roll, ptr, size__);
ptr += deserialize(embeddedInfo.ins_vx, ptr, size__);
ptr += deserialize(embeddedInfo.ins_vy, ptr, size__);
ptr += deserialize(embeddedInfo.ins_vz, ptr, size__);
ptr += deserialize(embeddedInfo.lon, ptr, size__);
ptr += deserialize(embeddedInfo.lat, ptr, size__);
ptr += deserialize(embeddedInfo.alt, ptr, size__);
ptr += deserialize(embeddedInfo.dvl_vx, ptr, size__);
ptr += deserialize(embeddedInfo.dvl_vy, ptr, size__);
ptr += deserialize(embeddedInfo.dvl_vz, ptr, size__);
ptr += deserialize(embeddedInfo.rpm, ptr, size__);
ptr += deserialize(embeddedInfo.light_enable, ptr, size__);
ptr += deserialize(embeddedInfo.battery_voltage, ptr, size__);
ptr += deserialize(embeddedInfo.battery_level, ptr, size__);
ptr += deserialize(embeddedInfo.battery_temp, ptr, size__);
ptr += deserialize(embeddedInfo.fault_leakSensor, ptr, size__);
ptr += deserialize(embeddedInfo.fault_battery, ptr, size__);
ptr += deserialize(embeddedInfo.fault_emergencyBattery, ptr, size__);
ptr += deserialize(embeddedInfo.fault_thrust, ptr, size__);
ptr += deserialize(embeddedInfo.iridium, ptr, size__);
ptr += deserialize(embeddedInfo.throwing_load, ptr, size__);
ptr += deserialize(embeddedInfo.dvl_status, ptr, size__);
ptr += deserialize(embeddedInfo.crc, ptr, size__);
ptr += deserialize(embeddedInfo.footer, ptr, size__);
return 0;
}
bool BoardSupportComm::OnStartUp()
{
// AppCastingMOOSApp::OnStartUp();
m_MissionReader.GetValue("LongOrigin", estimatedState.referenceLon);
m_MissionReader.GetValue("LatOrigin", estimatedState.referenceLat);
m_MissionReader.GetValue("AltOrigin", estimatedState.referenceAltitude);
std::cout << "BoardSupportComm OnStartUp: " << estimatedState.referenceLon << ", "
<< estimatedState.referenceLat << ", "
<< estimatedState.referenceAltitude << std::endl;
RegisterVariables();
return(true);
}
void BoardSupportComm::RegisterVariables()
{
// AppCastingMOOSApp::RegisterVariables();
#ifdef MOOS_AUV_SIM
Register("NAV_X", 0);
Register("NAV_Y", 0);
Register("NAV_Z", 0);
Register("NAV_LAT", 0);
Register("NAV_LONG", 0);
Register("NAV_SPEED", 0);
Register("NAV_DEPTH", 0);
Register("NAV_YAW", 0);
Register("NAV_PITCH", 0);
#endif
Register("uManual_drive_cmd", 0);
Register("uMotion_control_cmd", 0);
Register("uManual_enable_cmd", 0);
Register("uMission_origin_cmd", 0);
}
void BoardSupportComm::ConvertLLAToENU(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_enu)
{
static GeographicLib::LocalCartesian local_cartesian;
local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
local_cartesian.Forward(point_lla.at(1), point_lla.at(0), point_lla.at(2),
point_enu.at(0), point_enu.at(1), point_enu.at(2));
}
void BoardSupportComm::ConvertENUToLLA(std::vector<double> init_lla,
std::vector<double> point_enu,
std::vector<double> &point_lla)
{
static GeographicLib::LocalCartesian local_cartesian;
local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
local_cartesian.Reverse(point_enu.at(0), point_enu.at(1), point_enu.at(2),
point_lla.at(1), point_lla.at(0), point_lla.at(2));
}
void BoardSupportComm::ConvertLLAToNED(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_ned)
{
static GeographicLib::LocalCartesian local_cartesian;
local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
double point_e = 0;
double point_n = 0;
double point_u = 0;
local_cartesian.Forward(point_lla.at(1), point_lla.at(0), point_lla.at(2),
point_e, point_n, point_u);
point_ned.at(0) = point_n;
point_ned.at(1) = point_e;
point_ned.at(2) = -point_u;
}
void BoardSupportComm::ConvertNEDToLLA(std::vector<double> init_lla,
std::vector<double> point_ned,
std::vector<double> &point_lla)
{
static GeographicLib::LocalCartesian local_cartesian;
local_cartesian.Reset(init_lla.at(1), init_lla.at(0), init_lla.at(2));
double point_e = point_ned.at(1);
double point_n = point_ned.at(0);
double point_u = -point_ned.at(2);
local_cartesian.Reverse(point_e, point_n, point_u,
point_lla.at(1), point_lla.at(0), point_lla.at(2));
}

171
src/pBoardSupportComm/BoardSupportComm.h Normal file
View File

@@ -0,0 +1,171 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: BoardSupportComm.h */
/* DATE: */
/************************************************************/
#ifndef BoardSupportComm_HEADER
#define BoardSupportComm_HEADER
#include "MOOS/libMOOS/MOOSLib.h"
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <DUNE/DUNE.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <json/json.h>
#include <thread>
#include <chrono>
#define CRC_COMPUTE_START_POS 5
#define CRC_COMPUTE_SUM_SIZE 13
struct AUVEmbedded
{
uint16_t header; //1:[0,1]
uint16_t count; //2:[2,3]
uint8_t size; //3:[4]
uint8_t drive_mode; //4:[5]
uint16_t height; //5:[6,7]
uint16_t depth; //6:[8,9]
uint16_t yaw; //7:[10,11]
int16_t pitch; //8:[12,13]
int16_t roll; //9:[14,15]
int16_t ins_vx; //10:[16,17]
int16_t ins_vy; //11:[18,19]
int16_t ins_vz; //12:[20,21]
int32_t lon; //13:[22,23,24,25]
int32_t lat; //14:[26,27,28,29]
int16_t alt; //15:[30,31]
int16_t dvl_vx; //16:[32,33]
int16_t dvl_vy; //17:[34,35]
int16_t dvl_vz; //18:[36,37]
int16_t rpm; //19:[38,39]
uint8_t light_enable; //20:[40]
uint16_t battery_voltage; //21:[41,42]
uint8_t battery_level; //22:[43]
uint16_t battery_temp; //23:[44,45]
uint32_t fault_leakSensor; //24:[46,47,48,49]
uint8_t fault_battery; //25:[50]
uint8_t fault_emergencyBattery; //26:[51]
uint8_t fault_thrust; //27:[52]
uint8_t iridium; //28:[53]
uint8_t throwing_load; //29:[54]
uint8_t dvl_status; //30:[55]
uint8_t crc; //31:[56]
uint16_t footer; //32:[57,58]
};
struct AUVExecuteCommand
{
uint16_t header; //1:[0,1]
uint16_t count; //2:[2,3]
uint8_t size; //3:[4]
uint8_t drive_mode; //4:[5]
uint8_t thrust; //5:[6]
uint16_t yaw; //6:[7,8]
uint16_t depth; //7:[9,10]
uint8_t helm_top_angle; //8:[11]
uint8_t helm_bottom_angle;//9:[12]
uint8_t helm_left_angle; //10:[13]
uint8_t helm_right_angle;//11:[14]
uint8_t light_enable; //12:[15]
uint8_t dvl_enable; //13:[16]
uint8_t throwing_load_enable; //14:[17]
uint8_t crc; //15:[18]
uint16_t footer; //16:[19,20]
bool manual_mode;
};
struct EstimatedState {
std::string info;
float referenceLon;
float referenceLat;
float referenceAltitude;
float currentLon;
float currentLat;
float currentAltitude;
float offsetNorth;
float offsetEast;
float offsetDown;
float roll;
float pitch;
float yaw;
float linearVelocityNorth;
float linearVelocityEast;
float linearVelocityDown;
float height;
float depth;
};
class BoardSupportComm : public CMOOSApp
// class BoardSupportComm : public AppCastingMOOSApp
{
public:
BoardSupportComm();
~BoardSupportComm();
protected: // Standard MOOSApp functions to overload
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void RegisterVariables();
// bool buildReport();
private:
// double uMotion_posX_fb;
// double uMotion_posY_fb;
// double uMotion_posZ_fb;
// double uMotion_roll_fb;
// double uMotion_pitch_fb;
// double uMotion_yaw_fb;
// double uMotion_longitude_fb;
// double uMotion_latitude_fb;
// double uMotion_velocityX_fb;
// double uMotion_velocityY_fb;
// double uMotion_velocityZ_fb;
// double uMotion_deep_fb;
struct EstimatedState estimatedState;
void ConvertLLAToENU(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_enu);
void ConvertENUToLLA(std::vector<double> init_lla,
std::vector<double> point_enu,
std::vector<double> &point_lla);
void ConvertLLAToNED(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_ned);
void ConvertNEDToLLA(std::vector<double> init_lla,
std::vector<double> point_ned,
std::vector<double> &point_lla);
void tcpProcessThread();
// std::string convertEmbeddedFormat();
std::string convertEmbeddedFormat(Json::Value &embeddedState);
int tcpSockFD;
int tcpConnectRet;
// unsigned char tcpReceiveBuffer[65535];
char tcpReceiveBuffer[65535];
unsigned char tcpSendBuffer[65535];
DUNE::IO::Poll m_poll_1;
DUNE::Network::TCPSocket sock_tcp_send;
uint16_t imcPollTCP(char* buffer, int bufferLength);
int parseMessage(unsigned char* buffer, int size);
int serializeMessage(unsigned char* buffer);
template <typename Type> inline uint16_t deserialize(Type& t, const uint8_t* bfr, uint16_t& length);
template <typename Type> inline uint16_t serialize(const Type t, uint8_t* bfr, uint16_t& sumSize);
uint8_t convertIntToUchar(int src, int min, int max);
struct AUVEmbedded embeddedInfo;
struct AUVExecuteCommand executeCommand;
int testFlag = 0;
bool testCount = false;
int sockfd = -1;
int connectFlg = -1;
};
#endif

115
src/pBoardSupportComm/BoardSupportComm_Info.cpp Normal file
View File

@@ -0,0 +1,115 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: BoardSupportComm_Info.cpp */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#include <cstdlib>
#include <iostream>
#include "BoardSupportComm_Info.h"
#include "ColorParse.h"
#include "ReleaseInfo.h"
using namespace std;
//----------------------------------------------------------------
// Procedure: showSynopsis
void showSynopsis()
{
blk("SYNOPSIS: ");
blk("------------------------------------ ");
blk(" The pBoardSupportComm application is used for ");
blk(" ");
blk(" ");
blk(" ");
blk(" ");
}
//----------------------------------------------------------------
// Procedure: showHelpAndExit
void showHelpAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("Usage: pBoardSupportComm file.moos [OPTIONS] ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("Options: ");
mag(" --alias","=<ProcessName> ");
blk(" Launch pBoardSupportComm with the given process name ");
blk(" rather than pBoardSupportComm. ");
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 pBoardSupportComm. ");
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("pBoardSupportComm Example MOOS Configuration ");
blu("=============================================================== ");
blk(" ");
blk("ProcessConfig = pBoardSupportComm ");
blk("{ ");
blk(" AppTick = 4 ");
blk(" CommsTick = 4 ");
blk(" ");
blk("} ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showInterfaceAndExit
void showInterfaceAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pBoardSupportComm 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("pBoardSupportComm", "gpl");
exit(0);
}

18
src/pBoardSupportComm/BoardSupportComm_Info.h Normal file
View File

@@ -0,0 +1,18 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: BoardSupportComm_Info.h */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#ifndef BoardSupportComm_INFO_HEADER
#define BoardSupportComm_INFO_HEADER
void showSynopsis();
void showHelpAndExit();
void showExampleConfigAndExit();
void showInterfaceAndExit();
void showReleaseInfoAndExit();
#endif

47
src/pBoardSupportComm/CMakeLists.txt Normal file
View File

@@ -0,0 +1,47 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pBoardSupportComm
# Author(s): Xiaobin Zeng
#--------------------------------------------------------
SET(SRC
BoardSupportComm.cpp
BoardSupportComm_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(FindProtobuf)
# find_package(Protobuf REQUIRED)
# include_directories(${Protobuf_INCLUDE_DIR})
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
#find_package (jsoncpp REQUIRED)
find_package (GeographicLib REQUIRED)
include_directories(${GeographicLib_INCLUDE_DIRS})
ADD_EXECUTABLE(pBoardSupportComm ${SRC})
include_directories(/usr/include/jsoncpp/)
link_directories(/usr/local/lib/)
TARGET_LINK_LIBRARIES(pBoardSupportComm
${MOOS_LIBRARIES}
${DUNE_LIB}
${GeographicLib_LIBRARIES}
mbutil
m
pthread
jsoncpp
# jsoncpp_lib_static
# protobuf
# protoc
# proto
# ${PROTOBUF_LIBRARY}
)

53
src/pBoardSupportComm/main.cpp Normal file
View File

@@ -0,0 +1,53 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: main.cpp */
/* DATE: */
/************************************************************/
#include <string>
#include "MBUtils.h"
#include "ColorParse.h"
#include "BoardSupportComm.h"
#include "BoardSupportComm_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 << "pBoardSupportComm launching as " << run_command << endl;
cout << termColor() << endl;
BoardSupportComm BoardSupportComm;
BoardSupportComm.Run(run_command.c_str(), mission_file.c_str());
return(0);
}

9
src/pBoardSupportComm/pBoardSupportComm.moos Normal file
View File

@@ -0,0 +1,9 @@
//------------------------------------------------
// pBoardSupportComm config block
ProcessConfig = pBoardSupportComm
{
AppTick = 4
CommsTick = 4
}

927
src/pClientViewer/Behavior.pb.h Normal file
View File

@@ -0,0 +1,927 @@
// Generated by the protocol buffer compiler. DO NOT EDIT!
// source: Behavior.proto
#ifndef GOOGLE_PROTOBUF_INCLUDED_Behavior_2eproto
#define GOOGLE_PROTOBUF_INCLUDED_Behavior_2eproto
#include <limits>
#include <string>
#include <google/protobuf/port_def.inc>
#if PROTOBUF_VERSION < 3021000
#error This file was generated by a newer version of protoc which is
#error incompatible with your Protocol Buffer headers. Please update
#error your headers.
#endif
#if 3021012 < PROTOBUF_MIN_PROTOC_VERSION
#error This file was generated by an older version of protoc which is
#error incompatible with your Protocol Buffer headers. Please
#error regenerate this file with a newer version of protoc.
#endif
#include <google/protobuf/port_undef.inc>
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/arena.h>
#include <google/protobuf/arenastring.h>
#include <google/protobuf/generated_message_util.h>
#include <google/protobuf/metadata_lite.h>
#include <google/protobuf/generated_message_reflection.h>
#include <google/protobuf/message.h>
#include <google/protobuf/repeated_field.h> // IWYU pragma: export
#include <google/protobuf/extension_set.h> // IWYU pragma: export
#include <google/protobuf/unknown_field_set.h>
// @@protoc_insertion_point(includes)
#include <google/protobuf/port_def.inc>
#define PROTOBUF_INTERNAL_EXPORT_Behavior_2eproto
PROTOBUF_NAMESPACE_OPEN
namespace internal {
class AnyMetadata;
} // namespace internal
PROTOBUF_NAMESPACE_CLOSE
// Internal implementation detail -- do not use these members.
struct TableStruct_Behavior_2eproto {
static const uint32_t offsets[];
};
extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_Behavior_2eproto;
namespace Behavior {
class Coordinate;
struct CoordinateDefaultTypeInternal;
extern CoordinateDefaultTypeInternal _Coordinate_default_instance_;
class WayPointBehavior;
struct WayPointBehaviorDefaultTypeInternal;
extern WayPointBehaviorDefaultTypeInternal _WayPointBehavior_default_instance_;
} // namespace Behavior
PROTOBUF_NAMESPACE_OPEN
template<> ::Behavior::Coordinate* Arena::CreateMaybeMessage<::Behavior::Coordinate>(Arena*);
template<> ::Behavior::WayPointBehavior* Arena::CreateMaybeMessage<::Behavior::WayPointBehavior>(Arena*);
PROTOBUF_NAMESPACE_CLOSE
namespace Behavior {
// ===================================================================
class Coordinate final :
public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:Behavior.Coordinate) */ {
public:
inline Coordinate() : Coordinate(nullptr) {}
~Coordinate() override;
explicit PROTOBUF_CONSTEXPR Coordinate(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized);
Coordinate(const Coordinate& from);
Coordinate(Coordinate&& from) noexcept
: Coordinate() {
*this = ::std::move(from);
}
inline Coordinate& operator=(const Coordinate& from) {
CopyFrom(from);
return *this;
}
inline Coordinate& operator=(Coordinate&& from) noexcept {
if (this == &from) return *this;
if (GetOwningArena() == from.GetOwningArena()
#ifdef PROTOBUF_FORCE_COPY_IN_MOVE
&& GetOwningArena() != nullptr
#endif // !PROTOBUF_FORCE_COPY_IN_MOVE
) {
InternalSwap(&from);
} else {
CopyFrom(from);
}
return *this;
}
static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
return GetDescriptor();
}
static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
return default_instance().GetMetadata().descriptor;
}
static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
return default_instance().GetMetadata().reflection;
}
static const Coordinate& default_instance() {
return *internal_default_instance();
}
static inline const Coordinate* internal_default_instance() {
return reinterpret_cast<const Coordinate*>(
&_Coordinate_default_instance_);
}
static constexpr int kIndexInFileMessages =
0;
friend void swap(Coordinate& a, Coordinate& b) {
a.Swap(&b);
}
inline void Swap(Coordinate* other) {
if (other == this) return;
#ifdef PROTOBUF_FORCE_COPY_IN_SWAP
if (GetOwningArena() != nullptr &&
GetOwningArena() == other->GetOwningArena()) {
#else // PROTOBUF_FORCE_COPY_IN_SWAP
if (GetOwningArena() == other->GetOwningArena()) {
#endif // !PROTOBUF_FORCE_COPY_IN_SWAP
InternalSwap(other);
} else {
::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
}
}
void UnsafeArenaSwap(Coordinate* other) {
if (other == this) return;
GOOGLE_DCHECK(GetOwningArena() == other->GetOwningArena());
InternalSwap(other);
}
// implements Message ----------------------------------------------
Coordinate* New(::PROTOBUF_NAMESPACE_ID::Arena* arena = nullptr) const final {
return CreateMaybeMessage<Coordinate>(arena);
}
using ::PROTOBUF_NAMESPACE_ID::Message::CopyFrom;
void CopyFrom(const Coordinate& from);
using ::PROTOBUF_NAMESPACE_ID::Message::MergeFrom;
void MergeFrom( const Coordinate& from) {
Coordinate::MergeImpl(*this, from);
}
private:
static void MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTOBUF_NAMESPACE_ID::Message& from_msg);
public:
PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
bool IsInitialized() const final;
size_t ByteSizeLong() const final;
const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
uint8_t* _InternalSerialize(
uint8_t* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
int GetCachedSize() const final { return _impl_._cached_size_.Get(); }
private:
void SharedCtor(::PROTOBUF_NAMESPACE_ID::Arena* arena, bool is_message_owned);
void SharedDtor();
void SetCachedSize(int size) const final;
void InternalSwap(Coordinate* other);
private:
friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
return "Behavior.Coordinate";
}
protected:
explicit Coordinate(::PROTOBUF_NAMESPACE_ID::Arena* arena,
bool is_message_owned = false);
public:
static const ClassData _class_data_;
const ::PROTOBUF_NAMESPACE_ID::Message::ClassData*GetClassData() const final;
::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
// nested types ----------------------------------------------------
// accessors -------------------------------------------------------
enum : int {
kNameFieldNumber = 1,
kLonFieldNumber = 2,
kLatFieldNumber = 3,
kDepthFieldNumber = 4,
kSpeedFieldNumber = 5,
};
// string name = 1;
void clear_name();
const std::string& name() const;
template <typename ArgT0 = const std::string&, typename... ArgT>
void set_name(ArgT0&& arg0, ArgT... args);
std::string* mutable_name();
PROTOBUF_NODISCARD std::string* release_name();
void set_allocated_name(std::string* name);
private:
const std::string& _internal_name() const;
inline PROTOBUF_ALWAYS_INLINE void _internal_set_name(const std::string& value);
std::string* _internal_mutable_name();
public:
// float lon = 2;
void clear_lon();
float lon() const;
void set_lon(float value);
private:
float _internal_lon() const;
void _internal_set_lon(float value);
public:
// float lat = 3;
void clear_lat();
float lat() const;
void set_lat(float value);
private:
float _internal_lat() const;
void _internal_set_lat(float value);
public:
// float depth = 4;
void clear_depth();
float depth() const;
void set_depth(float value);
private:
float _internal_depth() const;
void _internal_set_depth(float value);
public:
// float speed = 5;
void clear_speed();
float speed() const;
void set_speed(float value);
private:
float _internal_speed() const;
void _internal_set_speed(float value);
public:
// @@protoc_insertion_point(class_scope:Behavior.Coordinate)
private:
class _Internal;
template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
typedef void InternalArenaConstructable_;
typedef void DestructorSkippable_;
struct Impl_ {
::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr name_;
float lon_;
float lat_;
float depth_;
float speed_;
mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
};
union { Impl_ _impl_; };
friend struct ::TableStruct_Behavior_2eproto;
};
// -------------------------------------------------------------------
class WayPointBehavior final :
public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:Behavior.WayPointBehavior) */ {
public:
inline WayPointBehavior() : WayPointBehavior(nullptr) {}
~WayPointBehavior() override;
explicit PROTOBUF_CONSTEXPR WayPointBehavior(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized);
WayPointBehavior(const WayPointBehavior& from);
WayPointBehavior(WayPointBehavior&& from) noexcept
: WayPointBehavior() {
*this = ::std::move(from);
}
inline WayPointBehavior& operator=(const WayPointBehavior& from) {
CopyFrom(from);
return *this;
}
inline WayPointBehavior& operator=(WayPointBehavior&& from) noexcept {
if (this == &from) return *this;
if (GetOwningArena() == from.GetOwningArena()
#ifdef PROTOBUF_FORCE_COPY_IN_MOVE
&& GetOwningArena() != nullptr
#endif // !PROTOBUF_FORCE_COPY_IN_MOVE
) {
InternalSwap(&from);
} else {
CopyFrom(from);
}
return *this;
}
static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
return GetDescriptor();
}
static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
return default_instance().GetMetadata().descriptor;
}
static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
return default_instance().GetMetadata().reflection;
}
static const WayPointBehavior& default_instance() {
return *internal_default_instance();
}
static inline const WayPointBehavior* internal_default_instance() {
return reinterpret_cast<const WayPointBehavior*>(
&_WayPointBehavior_default_instance_);
}
static constexpr int kIndexInFileMessages =
1;
friend void swap(WayPointBehavior& a, WayPointBehavior& b) {
a.Swap(&b);
}
inline void Swap(WayPointBehavior* other) {
if (other == this) return;
#ifdef PROTOBUF_FORCE_COPY_IN_SWAP
if (GetOwningArena() != nullptr &&
GetOwningArena() == other->GetOwningArena()) {
#else // PROTOBUF_FORCE_COPY_IN_SWAP
if (GetOwningArena() == other->GetOwningArena()) {
#endif // !PROTOBUF_FORCE_COPY_IN_SWAP
InternalSwap(other);
} else {
::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
}
}
void UnsafeArenaSwap(WayPointBehavior* other) {
if (other == this) return;
GOOGLE_DCHECK(GetOwningArena() == other->GetOwningArena());
InternalSwap(other);
}
// implements Message ----------------------------------------------
WayPointBehavior* New(::PROTOBUF_NAMESPACE_ID::Arena* arena = nullptr) const final {
return CreateMaybeMessage<WayPointBehavior>(arena);
}
using ::PROTOBUF_NAMESPACE_ID::Message::CopyFrom;
void CopyFrom(const WayPointBehavior& from);
using ::PROTOBUF_NAMESPACE_ID::Message::MergeFrom;
void MergeFrom( const WayPointBehavior& from) {
WayPointBehavior::MergeImpl(*this, from);
}
private:
static void MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTOBUF_NAMESPACE_ID::Message& from_msg);
public:
PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
bool IsInitialized() const final;
size_t ByteSizeLong() const final;
const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
uint8_t* _InternalSerialize(
uint8_t* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
int GetCachedSize() const final { return _impl_._cached_size_.Get(); }
private:
void SharedCtor(::PROTOBUF_NAMESPACE_ID::Arena* arena, bool is_message_owned);
void SharedDtor();
void SetCachedSize(int size) const final;
void InternalSwap(WayPointBehavior* other);
private:
friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
return "Behavior.WayPointBehavior";
}
protected:
explicit WayPointBehavior(::PROTOBUF_NAMESPACE_ID::Arena* arena,
bool is_message_owned = false);
public:
static const ClassData _class_data_;
const ::PROTOBUF_NAMESPACE_ID::Message::ClassData*GetClassData() const final;
::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
// nested types ----------------------------------------------------
// accessors -------------------------------------------------------
enum : int {
kPointsFieldNumber = 3,
kNameFieldNumber = 1,
kPriorityFieldNumber = 2,
kDurationFieldNumber = 4,
kConstSpeedFieldNumber = 6,
kClosedLoopFieldNumber = 5,
kPerpetualFieldNumber = 8,
kRepeateFieldNumber = 7,
kMinDepthFieldNumber = 9,
kMaxDepthFieldNumber = 10,
};
// repeated .Behavior.Coordinate points = 3;
int points_size() const;
private:
int _internal_points_size() const;
public:
void clear_points();
::Behavior::Coordinate* mutable_points(int index);
::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate >*
mutable_points();
private:
const ::Behavior::Coordinate& _internal_points(int index) const;
::Behavior::Coordinate* _internal_add_points();
public:
const ::Behavior::Coordinate& points(int index) const;
::Behavior::Coordinate* add_points();
const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate >&
points() const;
// string name = 1;
void clear_name();
const std::string& name() const;
template <typename ArgT0 = const std::string&, typename... ArgT>
void set_name(ArgT0&& arg0, ArgT... args);
std::string* mutable_name();
PROTOBUF_NODISCARD std::string* release_name();
void set_allocated_name(std::string* name);
private:
const std::string& _internal_name() const;
inline PROTOBUF_ALWAYS_INLINE void _internal_set_name(const std::string& value);
std::string* _internal_mutable_name();
public:
// int32 priority = 2;
void clear_priority();
int32_t priority() const;
void set_priority(int32_t value);
private:
int32_t _internal_priority() const;
void _internal_set_priority(int32_t value);
public:
// float duration = 4;
void clear_duration();
float duration() const;
void set_duration(float value);
private:
float _internal_duration() const;
void _internal_set_duration(float value);
public:
// float constSpeed = 6;
void clear_constspeed();
float constspeed() const;
void set_constspeed(float value);
private:
float _internal_constspeed() const;
void _internal_set_constspeed(float value);
public:
// bool closedLoop = 5;
void clear_closedloop();
bool closedloop() const;
void set_closedloop(bool value);
private:
bool _internal_closedloop() const;
void _internal_set_closedloop(bool value);
public:
// bool perpetual = 8;
void clear_perpetual();
bool perpetual() const;
void set_perpetual(bool value);
private:
bool _internal_perpetual() const;
void _internal_set_perpetual(bool value);
public:
// int32 repeate = 7;
void clear_repeate();
int32_t repeate() const;
void set_repeate(int32_t value);
private:
int32_t _internal_repeate() const;
void _internal_set_repeate(int32_t value);
public:
// float minDepth = 9;
void clear_mindepth();
float mindepth() const;
void set_mindepth(float value);
private:
float _internal_mindepth() const;
void _internal_set_mindepth(float value);
public:
// float maxDepth = 10;
void clear_maxdepth();
float maxdepth() const;
void set_maxdepth(float value);
private:
float _internal_maxdepth() const;
void _internal_set_maxdepth(float value);
public:
// @@protoc_insertion_point(class_scope:Behavior.WayPointBehavior)
private:
class _Internal;
template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
typedef void InternalArenaConstructable_;
typedef void DestructorSkippable_;
struct Impl_ {
::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate > points_;
::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr name_;
int32_t priority_;
float duration_;
float constspeed_;
bool closedloop_;
bool perpetual_;
int32_t repeate_;
float mindepth_;
float maxdepth_;
mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
};
union { Impl_ _impl_; };
friend struct ::TableStruct_Behavior_2eproto;
};
// ===================================================================
// ===================================================================
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
#endif // __GNUC__
// Coordinate
// string name = 1;
inline void Coordinate::clear_name() {
_impl_.name_.ClearToEmpty();
}
inline const std::string& Coordinate::name() const {
// @@protoc_insertion_point(field_get:Behavior.Coordinate.name)
return _internal_name();
}
template <typename ArgT0, typename... ArgT>
inline PROTOBUF_ALWAYS_INLINE
void Coordinate::set_name(ArgT0&& arg0, ArgT... args) {
_impl_.name_.Set(static_cast<ArgT0 &&>(arg0), args..., GetArenaForAllocation());
// @@protoc_insertion_point(field_set:Behavior.Coordinate.name)
}
inline std::string* Coordinate::mutable_name() {
std::string* _s = _internal_mutable_name();
// @@protoc_insertion_point(field_mutable:Behavior.Coordinate.name)
return _s;
}
inline const std::string& Coordinate::_internal_name() const {
return _impl_.name_.Get();
}
inline void Coordinate::_internal_set_name(const std::string& value) {
_impl_.name_.Set(value, GetArenaForAllocation());
}
inline std::string* Coordinate::_internal_mutable_name() {
return _impl_.name_.Mutable(GetArenaForAllocation());
}
inline std::string* Coordinate::release_name() {
// @@protoc_insertion_point(field_release:Behavior.Coordinate.name)
return _impl_.name_.Release();
}
inline void Coordinate::set_allocated_name(std::string* name) {
if (name != nullptr) {
} else {
}
_impl_.name_.SetAllocated(name, GetArenaForAllocation());
#ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
if (_impl_.name_.IsDefault()) {
_impl_.name_.Set("", GetArenaForAllocation());
}
#endif // PROTOBUF_FORCE_COPY_DEFAULT_STRING
// @@protoc_insertion_point(field_set_allocated:Behavior.Coordinate.name)
}
// float lon = 2;
inline void Coordinate::clear_lon() {
_impl_.lon_ = 0;
}
inline float Coordinate::_internal_lon() const {
return _impl_.lon_;
}
inline float Coordinate::lon() const {
// @@protoc_insertion_point(field_get:Behavior.Coordinate.lon)
return _internal_lon();
}
inline void Coordinate::_internal_set_lon(float value) {
_impl_.lon_ = value;
}
inline void Coordinate::set_lon(float value) {
_internal_set_lon(value);
// @@protoc_insertion_point(field_set:Behavior.Coordinate.lon)
}
// float lat = 3;
inline void Coordinate::clear_lat() {
_impl_.lat_ = 0;
}
inline float Coordinate::_internal_lat() const {
return _impl_.lat_;
}
inline float Coordinate::lat() const {
// @@protoc_insertion_point(field_get:Behavior.Coordinate.lat)
return _internal_lat();
}
inline void Coordinate::_internal_set_lat(float value) {
_impl_.lat_ = value;
}
inline void Coordinate::set_lat(float value) {
_internal_set_lat(value);
// @@protoc_insertion_point(field_set:Behavior.Coordinate.lat)
}
// float depth = 4;
inline void Coordinate::clear_depth() {
_impl_.depth_ = 0;
}
inline float Coordinate::_internal_depth() const {
return _impl_.depth_;
}
inline float Coordinate::depth() const {
// @@protoc_insertion_point(field_get:Behavior.Coordinate.depth)
return _internal_depth();
}
inline void Coordinate::_internal_set_depth(float value) {
_impl_.depth_ = value;
}
inline void Coordinate::set_depth(float value) {
_internal_set_depth(value);
// @@protoc_insertion_point(field_set:Behavior.Coordinate.depth)
}
// float speed = 5;
inline void Coordinate::clear_speed() {
_impl_.speed_ = 0;
}
inline float Coordinate::_internal_speed() const {
return _impl_.speed_;
}
inline float Coordinate::speed() const {
// @@protoc_insertion_point(field_get:Behavior.Coordinate.speed)
return _internal_speed();
}
inline void Coordinate::_internal_set_speed(float value) {
_impl_.speed_ = value;
}
inline void Coordinate::set_speed(float value) {
_internal_set_speed(value);
// @@protoc_insertion_point(field_set:Behavior.Coordinate.speed)
}
// -------------------------------------------------------------------
// WayPointBehavior
// string name = 1;
inline void WayPointBehavior::clear_name() {
_impl_.name_.ClearToEmpty();
}
inline const std::string& WayPointBehavior::name() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.name)
return _internal_name();
}
template <typename ArgT0, typename... ArgT>
inline PROTOBUF_ALWAYS_INLINE
void WayPointBehavior::set_name(ArgT0&& arg0, ArgT... args) {
_impl_.name_.Set(static_cast<ArgT0 &&>(arg0), args..., GetArenaForAllocation());
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.name)
}
inline std::string* WayPointBehavior::mutable_name() {
std::string* _s = _internal_mutable_name();
// @@protoc_insertion_point(field_mutable:Behavior.WayPointBehavior.name)
return _s;
}
inline const std::string& WayPointBehavior::_internal_name() const {
return _impl_.name_.Get();
}
inline void WayPointBehavior::_internal_set_name(const std::string& value) {
_impl_.name_.Set(value, GetArenaForAllocation());
}
inline std::string* WayPointBehavior::_internal_mutable_name() {
return _impl_.name_.Mutable(GetArenaForAllocation());
}
inline std::string* WayPointBehavior::release_name() {
// @@protoc_insertion_point(field_release:Behavior.WayPointBehavior.name)
return _impl_.name_.Release();
}
inline void WayPointBehavior::set_allocated_name(std::string* name) {
if (name != nullptr) {
} else {
}
_impl_.name_.SetAllocated(name, GetArenaForAllocation());
#ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
if (_impl_.name_.IsDefault()) {
_impl_.name_.Set("", GetArenaForAllocation());
}
#endif // PROTOBUF_FORCE_COPY_DEFAULT_STRING
// @@protoc_insertion_point(field_set_allocated:Behavior.WayPointBehavior.name)
}
// int32 priority = 2;
inline void WayPointBehavior::clear_priority() {
_impl_.priority_ = 0;
}
inline int32_t WayPointBehavior::_internal_priority() const {
return _impl_.priority_;
}
inline int32_t WayPointBehavior::priority() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.priority)
return _internal_priority();
}
inline void WayPointBehavior::_internal_set_priority(int32_t value) {
_impl_.priority_ = value;
}
inline void WayPointBehavior::set_priority(int32_t value) {
_internal_set_priority(value);
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.priority)
}
// repeated .Behavior.Coordinate points = 3;
inline int WayPointBehavior::_internal_points_size() const {
return _impl_.points_.size();
}
inline int WayPointBehavior::points_size() const {
return _internal_points_size();
}
inline void WayPointBehavior::clear_points() {
_impl_.points_.Clear();
}
inline ::Behavior::Coordinate* WayPointBehavior::mutable_points(int index) {
// @@protoc_insertion_point(field_mutable:Behavior.WayPointBehavior.points)
return _impl_.points_.Mutable(index);
}
inline ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate >*
WayPointBehavior::mutable_points() {
// @@protoc_insertion_point(field_mutable_list:Behavior.WayPointBehavior.points)
return &_impl_.points_;
}
inline const ::Behavior::Coordinate& WayPointBehavior::_internal_points(int index) const {
return _impl_.points_.Get(index);
}
inline const ::Behavior::Coordinate& WayPointBehavior::points(int index) const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.points)
return _internal_points(index);
}
inline ::Behavior::Coordinate* WayPointBehavior::_internal_add_points() {
return _impl_.points_.Add();
}
inline ::Behavior::Coordinate* WayPointBehavior::add_points() {
::Behavior::Coordinate* _add = _internal_add_points();
// @@protoc_insertion_point(field_add:Behavior.WayPointBehavior.points)
return _add;
}
inline const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField< ::Behavior::Coordinate >&
WayPointBehavior::points() const {
// @@protoc_insertion_point(field_list:Behavior.WayPointBehavior.points)
return _impl_.points_;
}
// float duration = 4;
inline void WayPointBehavior::clear_duration() {
_impl_.duration_ = 0;
}
inline float WayPointBehavior::_internal_duration() const {
return _impl_.duration_;
}
inline float WayPointBehavior::duration() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.duration)
return _internal_duration();
}
inline void WayPointBehavior::_internal_set_duration(float value) {
_impl_.duration_ = value;
}
inline void WayPointBehavior::set_duration(float value) {
_internal_set_duration(value);
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.duration)
}
// bool closedLoop = 5;
inline void WayPointBehavior::clear_closedloop() {
_impl_.closedloop_ = false;
}
inline bool WayPointBehavior::_internal_closedloop() const {
return _impl_.closedloop_;
}
inline bool WayPointBehavior::closedloop() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.closedLoop)
return _internal_closedloop();
}
inline void WayPointBehavior::_internal_set_closedloop(bool value) {
_impl_.closedloop_ = value;
}
inline void WayPointBehavior::set_closedloop(bool value) {
_internal_set_closedloop(value);
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.closedLoop)
}
// float constSpeed = 6;
inline void WayPointBehavior::clear_constspeed() {
_impl_.constspeed_ = 0;
}
inline float WayPointBehavior::_internal_constspeed() const {
return _impl_.constspeed_;
}
inline float WayPointBehavior::constspeed() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.constSpeed)
return _internal_constspeed();
}
inline void WayPointBehavior::_internal_set_constspeed(float value) {
_impl_.constspeed_ = value;
}
inline void WayPointBehavior::set_constspeed(float value) {
_internal_set_constspeed(value);
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.constSpeed)
}
// int32 repeate = 7;
inline void WayPointBehavior::clear_repeate() {
_impl_.repeate_ = 0;
}
inline int32_t WayPointBehavior::_internal_repeate() const {
return _impl_.repeate_;
}
inline int32_t WayPointBehavior::repeate() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.repeate)
return _internal_repeate();
}
inline void WayPointBehavior::_internal_set_repeate(int32_t value) {
_impl_.repeate_ = value;
}
inline void WayPointBehavior::set_repeate(int32_t value) {
_internal_set_repeate(value);
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.repeate)
}
// bool perpetual = 8;
inline void WayPointBehavior::clear_perpetual() {
_impl_.perpetual_ = false;
}
inline bool WayPointBehavior::_internal_perpetual() const {
return _impl_.perpetual_;
}
inline bool WayPointBehavior::perpetual() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.perpetual)
return _internal_perpetual();
}
inline void WayPointBehavior::_internal_set_perpetual(bool value) {
_impl_.perpetual_ = value;
}
inline void WayPointBehavior::set_perpetual(bool value) {
_internal_set_perpetual(value);
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.perpetual)
}
// float minDepth = 9;
inline void WayPointBehavior::clear_mindepth() {
_impl_.mindepth_ = 0;
}
inline float WayPointBehavior::_internal_mindepth() const {
return _impl_.mindepth_;
}
inline float WayPointBehavior::mindepth() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.minDepth)
return _internal_mindepth();
}
inline void WayPointBehavior::_internal_set_mindepth(float value) {
_impl_.mindepth_ = value;
}
inline void WayPointBehavior::set_mindepth(float value) {
_internal_set_mindepth(value);
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.minDepth)
}
// float maxDepth = 10;
inline void WayPointBehavior::clear_maxdepth() {
_impl_.maxdepth_ = 0;
}
inline float WayPointBehavior::_internal_maxdepth() const {
return _impl_.maxdepth_;
}
inline float WayPointBehavior::maxdepth() const {
// @@protoc_insertion_point(field_get:Behavior.WayPointBehavior.maxDepth)
return _internal_maxdepth();
}
inline void WayPointBehavior::_internal_set_maxdepth(float value) {
_impl_.maxdepth_ = value;
}
inline void WayPointBehavior::set_maxdepth(float value) {
_internal_set_maxdepth(value);
// @@protoc_insertion_point(field_set:Behavior.WayPointBehavior.maxDepth)
}
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif // __GNUC__
// -------------------------------------------------------------------
// @@protoc_insertion_point(namespace_scope)
} // namespace Behavior
// @@protoc_insertion_point(global_scope)
#include <google/protobuf/port_undef.inc>
#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_Behavior_2eproto

52
src/pClientViewer/CMakeLists.txt Normal file
View File

@@ -0,0 +1,52 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pClientViewer
# Author(s): Xiaobin Zeng
#--------------------------------------------------------
SET(SRC
ClientViewer.cpp
ClientViewer_Info.cpp
# UDPCommunicationEvent.cpp
# TCPCommunicationEvent.cpp
main.cpp
)
SET(CMAKE_CXX_STANDARD 11)
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(FindProtobuf)
# find_package(Protobuf REQUIRED)
# include_directories(${Protobuf_INCLUDE_DIR})
# protobuf_generate_cpp(PROTO_SRC PROTO_HEADER Behavior.proto)
# add_library(proto ${PROTO_HEADER} ${PROTO_SRC})
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
#find_package (jsoncpp NO_MODULE REQUIRED)
include_directories(/usr/include/jsoncpp/)
link_directories(/usr/local/lib/)
ADD_EXECUTABLE(pClientViewer ${SRC})
TARGET_LINK_LIBRARIES(pClientViewer
${MOOS_LIBRARIES}
${CMAKE_DL_LIBS}
${SYSTEM_LIBS}
${DUNE_LIB}
mbutil
m
pthread
jsoncpp
# jsoncpp_lib_static
# protobuf
# protoc
# proto
# ${PROTOBUF_LIBRARY}
)

812
src/pClientViewer/ClientViewer.cpp Normal file
View File

@@ -0,0 +1,812 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: ClientViewer.cpp */
/* DATE: */
/************************************************************/
#include <iterator>
#include "ClientViewer.h"
//#include "Behavior.pb.h"
#include <json/json.h>
using namespace std;
#define UDP_RECEIVE_PORT 6001
#define TCP_SEND_PORT 8000
//#define TCP_SERVER_ADDRESS "10.25.0.230" //树莓派
#define TCP_SERVER_ADDRESS "127.0.0.1"
// #define TCP_SERVER_ADDRESS "10.25.0.163"
// #define TCP_SERVER_ADDRESS "10.25.0.160"
//---------------------------------------------------------
// Constructor
ClientViewer::ClientViewer()
{
udpReceiveBuffer = new uint8_t[65535];
tcpReceiveBuffer = new uint8_t[65535];
}
//---------------------------------------------------------
// Destructor
ClientViewer::~ClientViewer()
{
// udpCommEvent.Stop();
// tcpCommEvent.Stop();
delete udpReceiveBuffer;
delete tcpReceiveBuffer;
// if (sock_tcp_send)
// {
// delete sock_tcp_send;
// sock_tcp_send = NULL;
// }
// if (sock_udp_receive)
// {
// delete sock_udp_receive;
// sock_udp_receive = NULL;
// }
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool ClientViewer::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
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();
std::cout << key << " : " << sval << std::endl;
if(key == "Command")
{
if (sval == "SetPlan1") //PlanDB
{
std::string systemName = "CCU Neptus 0_163";
std::string plan_1_Spec = SetPlan1(systemName, MOOS::Time());
std::cout << "send task" << std::endl;
//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);
}
if (sval == "SetPlan2") //PlanDB
{
std::string systemName = "CCU Neptus 0_163";
std::string plan_2_Spec = SetPlan2(systemName, MOOS::Time());
std::cout << "send task" << std::endl;
//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);
}
if (sval == "ModifyPlan1") //PlanDB
{
std::string systemName = "CCU Neptus 0_163";
std::string plan_1_Spec = ModifyPlan1(systemName, MOOS::Time());
std::cout << "modify task" << std::endl;
//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);
}
if (sval == "GetPlanList") //PlanDB
{
DUNE::IMC::PlanDB msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanDB::TypeEnum::DBT_REQUEST;
msg.op = DUNE::IMC::PlanDB::OperationEnum::DBOP_GET_STATE;
msg.plan_id.assign("Test");
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "SetThrust") //SetEntityParameters
{
double timeStamp = MOOS::Time();
DUNE::IMC::SetEntityParameters msg;
msg.setTimeStamp(timeStamp);
msg.name = "Thrust";
DUNE::IMC::EntityParameter subMsg;
subMsg.setTimeStamp(timeStamp);
SetEntityStatus(subMsg, "Pwm", generateEntityValue("30", "int"));
msg.params.push_back(subMsg);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "SetIndicatorLight") //SetEntityParameters
{
double timeStamp = MOOS::Time();
DUNE::IMC::SetEntityParameters msg;
msg.setTimeStamp(timeStamp);
msg.name = "IndicatorLight";
DUNE::IMC::EntityParameter subMsg;
subMsg.setTimeStamp(timeStamp);
SetEntityStatus(subMsg, "Status", generateEntityValue("1", "int"));
msg.params.push_back(subMsg);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "StartPlan1") //PlanControl
{
DUNE::IMC::PlanControl msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;
msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_START;
msg.plan_id = "east_waypt_survey";
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "StartPlan2") //PlanControl
{
DUNE::IMC::PlanControl msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;
msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_START;
msg.plan_id = "west_waypt_survey";
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "StopPlan1") //PlanControl
{
DUNE::IMC::PlanControl msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;
msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_STOP;
msg.plan_id = "east_waypt_survey";
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "StopPlan2") //PlanControl
{
DUNE::IMC::PlanControl msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST;
msg.op = DUNE::IMC::PlanControl::OperationEnum::PC_STOP;
msg.plan_id = "west_waypt_survey";
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "RemoteActions") //PlanControl
{
DUNE::IMC::RemoteActions msg;
msg.setTimeStamp();
Json::Value executeCommand;
executeCommand["Thrust"] = -100;
executeCommand["Heading"] = -27.5;
Json::StreamWriterBuilder builder;
msg.actions = Json::writeString(builder, executeCommand);
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
if (sval == "VehicleCommand") //PlanControl
{
DUNE::IMC::VehicleCommand msg;
msg.setTimeStamp();
msg.type = DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST;
msg.command = 1;
tcpSendToServer(&msg, TCP_SERVER_ADDRESS, TCP_SEND_PORT);
}
}
}
return(true);
}
//---------------------------------------------------------
// Procedure: OnConnectToServer
bool ClientViewer::OnConnectToServer()
{
RegisterVariables();
return(true);
}
bool ClientViewer::Iterate()
{
#if 1
DUNE::IMC::Message * receiveUDPMessage;
while ((receiveUDPMessage = imcPollUDP()) != NULL)
{
processMessage(receiveUDPMessage);
free(receiveUDPMessage);
}
#endif
# if 1
DUNE::IMC::Message * receiveTCPMessage;
while ((receiveTCPMessage = imcPollTCP()) != NULL)
{
processMessage(receiveTCPMessage);
free(receiveTCPMessage);
}
#endif
return(true);
}
bool processMessageCallback(DUNE::IMC::Message * message)
{
int type = message->getId();
#if 1
if (type == DUNE::IMC::Announce::getIdStatic())
{
DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
// printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
printf("server receive %s: %lf, %lf, %lf, %lf\n", \
msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
}
#endif
if (type == DUNE::IMC::PlanDB::getIdStatic())
{
DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
printf("%s\n", msg->info.c_str());
}
#if 1
if (type == DUNE::IMC::PlanControlState::getIdStatic())
{
DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
printf("server receive %s: %lf, %s, %s, %u\n", \
msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
}
if (type == DUNE::IMC::MsgList::getIdStatic())
{
DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
for (; iter1 != msgList->msgs.end(); ++iter1)
{
DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
for (; iter2 != entityParameter->params.end(); ++iter2)
{
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;
}
}
}
if (type == DUNE::IMC::EstimatedState::getIdStatic())
{
DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
// printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// msg->getName(), msg->getTimeStamp(),
// msg->lat, msg->lon, msg->depth,
// msg->phi, msg->theta, msg->psi);
printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
msg->getName(), msg->getTimeStamp(),
msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
}
if (type == DUNE::IMC::VehicleState::getIdStatic())
{
DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
printf("server receive %s: %lf, %u\n",
msg->getName(), msg->getTimeStamp(), msg->op_mode);
}
if (type == DUNE::IMC::PlanControl::getIdStatic())
{
DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message);
printf("server receive %s: %lf, %u,\n",
msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str());
}
#endif
return true;
}
//---------------------------------------------------------
// Procedure: OnStartUp()
// happens before connection is open
bool ClientViewer::OnStartUp()
{
initLon = -70.330400;
initLat = 43.825300;
initAlt = 0;
// udpCommEvent.SetPeriod(1);
// udpCommEvent.SetCallback(processMessageCallback,NULL);
// udpCommEvent.Start();
// tcpCommEvent.SetPeriod(1);
// tcpCommEvent.SetCallback(processMessageCallback,NULL);
// tcpCommEvent.Start();
#if 1
sock_udp_receive.bind(UDP_RECEIVE_PORT, DUNE::Network::Address::Any, false);
m_poll_0.add(sock_udp_receive);
#endif
#if 1
try
{
// sock_tcp_send.setSendTimeout(5);
// sock_tcp_send.setReceiveTimeout(5);
sock_tcp_send.connect(TCP_SERVER_ADDRESS, TCP_SEND_PORT);
sock_tcp_send.setKeepAlive(true);
m_poll_1.add(sock_tcp_send);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
#endif
#if 1
char local_ip2[INET_ADDRSTRLEN] = {0};
get_local_ip_using_create_socket(local_ip2);
ethernetIP = local_ip2;
#endif
RegisterVariables();
return(true);
}
std::string ClientViewer::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 = {"station_1", -70.328891,43.824429, 10, 3, -1, -1, "point"};
struct Landmark station_2 = {"station_2", -70.327885,43.824676, 8, 5, -1, -1, "point"};
struct Landmark station_3 = {"station_3", -70.327867,43.823622, 6, 7, -1, -1, "point"};
struct Landmark station_4 = {"station_4", -70.328765,43.823622, 4, 9, -1, -1, "point"};
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.repeat = 1;
behavior.closedLoop = true;
behavior.perpetual = false;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["taskName"] = behavior.name;
behaviorConfig["taskId"] = "1";
behaviorConfig["sourceName"] = behavior.source;
behaviorConfig["sourceAddress"] = ethernetIP;
behaviorConfig["clientStamp"] = stamp;
behaviorConfig["boardStamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeat"] = behavior.repeat;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
behaviorConfig["origin"]["lon"] = initLon;
behaviorConfig["origin"]["lat"] = initLat;
behaviorConfig["origin"]["altitude"] = initAlt;
Json::Value station;
station["name"] = station_1.name;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
station["north"] = station_1.north;
station["east"] = station_1.east;
station["type"] = station_1.type;
behaviorConfig["points"].append(station);
station["name"] = station_2.name;
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
station["north"] = station_2.north;
station["east"] = station_2.east;
station["type"] = station_2.type;
behaviorConfig["points"].append(station);
station["name"] = station_3.name;
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
station["north"] = station_3.north;
station["east"] = station_3.east;
station["type"] = station_3.type;
behaviorConfig["points"].append(station);
station["name"] = station_4.name;
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
station["north"] = station_4.north;
station["east"] = station_4.east;
station["type"] = station_4.type;
behaviorConfig["points"].append(station);
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
std::string ClientViewer::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 = {"station_1", -70.331532,43.824194, 9, 4, -1, -1, "track"};
struct Landmark station_2 = {"station_2", -70.330328,43.824299, 7, 6, -1, -1, "track"};
struct Landmark station_3 = {"station_3", -70.330346,43.823518, 5, 8, -1, -1, "track"};
struct Landmark station_4 = {"station_4", -70.331406,43.823206, 3, 10, -1, -1, "track"};
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.repeat = -1;
behavior.closedLoop = true;
behavior.perpetual = true;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["taskName"] = behavior.name;
behaviorConfig["taskId"] = "2";
behaviorConfig["sourceName"] = behavior.source;
behaviorConfig["sourceAddress"] = ethernetIP;
behaviorConfig["clientStamp"] = stamp;
behaviorConfig["boardStamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeat"] = behavior.repeat;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
behaviorConfig["origin"]["lon"] = initLon;
behaviorConfig["origin"]["lat"] = initLat;
behaviorConfig["origin"]["altitude"] = initAlt;
Json::Value station;
station["name"] = station_1.name;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
station["north"] = station_1.north;
station["east"] = station_1.east;
station["type"] = station_1.type;
behaviorConfig["points"].append(station);
station["name"] = station_2.name;
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
station["north"] = station_2.north;
station["east"] = station_2.east;
station["type"] = station_2.type;
behaviorConfig["points"].append(station);
station["name"] = station_3.name;
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
station["north"] = station_3.north;
station["east"] = station_3.east;
station["type"] = station_3.type;
behaviorConfig["points"].append(station);
station["name"] = station_4.name;
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
station["north"] = station_4.north;
station["east"] = station_4.east;
station["type"] = station_4.type;
behaviorConfig["points"].append(station);
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
std::string ClientViewer::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 = {"station_1", -70.328891,43.824429, 9, 2, -1, -1, "track"};
struct Landmark station_2 = {"station_2", -70.327885,43.824676, 7, 4, -1, -1, "point"};
struct Landmark station_3 = {"station_3", -70.327867,43.823622, 5, 6, -1, -1, "track"};
struct Landmark station_4 = {"station_4", -70.328765,43.823622, 3, 8, -1, -1, "point"};
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.repeat = 3;
behavior.closedLoop = true;
behavior.perpetual = false;
behavior.minDepth = -1;
behavior.maxDepth = -1;
Json::Value behaviorConfig;
behaviorConfig["taskName"] = behavior.name;
behaviorConfig["taskId"] = "1";
behaviorConfig["sourceName"] = behavior.source;
behaviorConfig["sourceAddress"] = ethernetIP;
behaviorConfig["clientStamp"] = stamp;
behaviorConfig["boardStamp"] = -1;
behaviorConfig["priority"] = behavior.priority;
behaviorConfig["duration"] = behavior.duration;
behaviorConfig["closedLoop"] = behavior.closedLoop;
behaviorConfig["constSpeed"] = behavior.constSpeed;
behaviorConfig["repeat"] = behavior.repeat;
behaviorConfig["perpetual"] = behavior.perpetual;
behaviorConfig["minDepth"] = behavior.minDepth;
behaviorConfig["maxDepth"] = behavior.maxDepth;
behaviorConfig["origin"]["lon"] = initLon;
behaviorConfig["origin"]["lat"] = initLat;
behaviorConfig["origin"]["altitude"] = initAlt;
Json::Value station;
station["name"] = station_1.name;
station["lon"] = station_1.lon;
station["lat"] = station_1.lat;
station["depth"] = station_1.depth;
station["speed"] = station_1.speed;
station["north"] = station_1.north;
station["east"] = station_1.east;
station["type"] = station_1.type;
behaviorConfig["points"].append(station);
station["name"] = station_2.name;
station["lon"] = station_2.lon;
station["lat"] = station_2.lat;
station["depth"] = station_2.depth;
station["speed"] = station_2.speed;
station["north"] = station_2.north;
station["east"] = station_2.east;
station["type"] = station_2.type;
behaviorConfig["points"].append(station);
station["name"] = station_3.name;
station["lon"] = station_3.lon;
station["lat"] = station_3.lat;
station["depth"] = station_3.depth;
station["speed"] = station_3.speed;
station["north"] = station_3.north;
station["east"] = station_3.east;
station["type"] = station_3.type;
behaviorConfig["points"].append(station);
station["name"] = station_4.name;
station["lon"] = station_4.lon;
station["lat"] = station_4.lat;
station["depth"] = station_4.depth;
station["speed"] = station_4.speed;
station["north"] = station_4.north;
station["east"] = station_4.east;
station["type"] = station_4.type;
behaviorConfig["points"].append(station);
Json::StreamWriterBuilder builder;
std::string behaviorSpecString = Json::writeString(builder, behaviorConfig);
return behaviorSpecString;
}
DUNE::IMC::Message * ClientViewer::imcPollUDP()
{
if (m_poll_0.poll(0))
{
DUNE::Network::Address addr;
uint16_t rv = sock_udp_receive.read(udpReceiveBuffer, 65535, &addr);
DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(udpReceiveBuffer, rv);
return msg;
}
return NULL;
}
DUNE::IMC::Message * ClientViewer::imcPollTCP()
{
if (m_poll_1.poll(0))
{
uint16_t rv = sock_tcp_send.read(tcpReceiveBuffer, 65535);
DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(tcpReceiveBuffer, rv);
return msg;
}
return NULL;
}
bool ClientViewer::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;
}
void ClientViewer::processMessage(DUNE::IMC::Message * message) {
int type = message->getId();
if (type == DUNE::IMC::Announce::getIdStatic())
{
DUNE::IMC::Announce * msg = dynamic_cast<DUNE::IMC::Announce *>(message);
// printf("server receive %s: %lf, %lf, %lf, %lf\n", \
// msg->getName(), msg->getTimeStamp(), msg->lat, msg->lon, msg->height);
printf("server receive %s: %lf, %lf, %lf, %lf\n", \
msg->getName(), msg->getTimeStamp(), msg->lat*180/M_PI, msg->lon*180/M_PI, msg->height);
}
else if (type == DUNE::IMC::PlanDB::getIdStatic())
{
DUNE::IMC::PlanDB * msg = dynamic_cast<DUNE::IMC::PlanDB *>(message);
printf("server receive %s: %lf, %d, %d\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->op);
printf("%s\n", msg->info.c_str());
}
else if (type == DUNE::IMC::PlanControlState::getIdStatic())
{
DUNE::IMC::PlanControlState * msg = dynamic_cast<DUNE::IMC::PlanControlState *>(message);
printf("server receive %s: %lf, %s, %s, %u\n", \
msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str(), msg->man_id.c_str(), msg->state);
}
else if (type == DUNE::IMC::MsgList::getIdStatic())
{
DUNE::IMC::MsgList * msgList = dynamic_cast<DUNE::IMC::MsgList *>(message);
printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp());
DUNE::IMC::MessageList<DUNE::IMC::Message>::const_iterator iter1 = msgList->msgs.begin();
for (; iter1 != msgList->msgs.end(); ++iter1)
{
DUNE::IMC::EntityParameters *entityParameter = static_cast<DUNE::IMC::EntityParameters *>(*iter1);
DUNE::IMC::MessageList<DUNE::IMC::EntityParameter>::const_iterator iter2 = entityParameter->params.begin();
for (; iter2 != entityParameter->params.end(); ++iter2)
{
DUNE::IMC::EntityParameter *subEntityParameter = static_cast<DUNE::IMC::EntityParameter *>(*iter2);
std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl;
}
}
}
else if (type == DUNE::IMC::EstimatedState::getIdStatic())
{
DUNE::IMC::EstimatedState * msg = dynamic_cast<DUNE::IMC::EstimatedState *>(message);
// printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
// msg->getName(), msg->getTimeStamp(),
// msg->lat, msg->lon, msg->depth,
// msg->phi, msg->theta, msg->psi);
printf("server receive %s: %lf, (%f, %f, %f), (%f, %f, %f)\n",
msg->getName(), msg->getTimeStamp(),
msg->lat*180/M_PI, msg->lon*180/M_PI, msg->depth*180/M_PI,
msg->phi*180/M_PI, msg->theta*180/M_PI, msg->psi*180/M_PI);
}
else if (type == DUNE::IMC::VehicleState::getIdStatic())
{
DUNE::IMC::VehicleState * msg = dynamic_cast<DUNE::IMC::VehicleState *>(message);
printf("server receive %s: %lf, %u\n",
msg->getName(), msg->getTimeStamp(), msg->op_mode);
}
else if (type == DUNE::IMC::PlanControl::getIdStatic())
{
DUNE::IMC::PlanControl * msg = dynamic_cast<DUNE::IMC::PlanControl *>(message);
printf("server receive %s: %lf, %u,\n",
msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str());
}
else
{
}
}
//---------------------------------------------------------
// Procedure: RegisterVariables
void ClientViewer::RegisterVariables()
{
Register("Command", 0);
Register("Key", 0);
Register("Value", 0);
}
int ClientViewer::SetEntityStatus(DUNE::IMC::EntityParameter& subMsg, std::string name, std::string value)
{
int result = -1;
subMsg.name = name;
subMsg.value = value;
result = 0;
return result;
}
std::string ClientViewer::generateEntityName(std::string parentName, std::string childName)
{
Json::Value outputJsonValue;
outputJsonValue["parent"] = parentName;
outputJsonValue["child"] = childName;
Json::StreamWriterBuilder builder;
std::string outputJsonString = Json::writeString(builder, outputJsonValue);
std::cout << outputJsonString << std::endl;
return outputJsonString;
}
std::string ClientViewer::generateEntityValue(std::string value, std::string type)
{
Json::Value outputJsonValue;
outputJsonValue["value"] = value;
outputJsonValue["type"] = type;
Json::StreamWriterBuilder builder;
std::string outputJsonString = Json::writeString(builder, outputJsonValue);
std::cout << outputJsonString << std::endl;
return outputJsonString;
}
int ClientViewer::get_local_ip_using_create_socket(char* str_ip)
{
int status = -1;
int af = AF_INET;
int sock_fd = socket(AF_INET, SOCK_DGRAM, 0);
struct sockaddr_in remote_addr;
struct sockaddr_in local_addr;
char *local_ip = NULL;
socklen_t len = 0;
remote_addr.sin_family = AF_INET;
remote_addr.sin_port = htons(53);
remote_addr.sin_addr.s_addr = inet_addr("1.1.1.1");
len = sizeof(struct sockaddr_in);
status = connect(sock_fd, (struct sockaddr*)&remote_addr, len);
if(status != 0 ){
printf("connect err \n");
}
len = sizeof(struct sockaddr_in);
getsockname(sock_fd, (struct sockaddr*)&local_addr, &len);
local_ip = inet_ntoa(local_addr.sin_addr);
if(local_ip)
{
strcpy(str_ip, local_ip);
status = 0;
}
return status;
}

100
src/pClientViewer/ClientViewer.h Normal file
View File

@@ -0,0 +1,100 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: ClientViewer.h */
/* DATE: */
/************************************************************/
#ifndef ClientViewer_HEADER
#define ClientViewer_HEADER
#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <arpa/inet.h>
#include "MOOS/libMOOS/MOOSLib.h"
#include "MOOS/libMOOS/Utils/MOOSThread.h"
#include "MOOS/libMOOS/Utils/PeriodicEvent.h"
#include <DUNE/DUNE.hpp>
// #include "UDPCommunicationEvent.h"
// #include "TCPCommunicationEvent.h"
struct Landmark {
std::string name;
float lon;
float lat;
float depth;
float speed;
float north;
float east;
std::string type;
};
struct WayPointBehavior
{
std::string name;
std::string source;
int priority;
std::vector<Landmark> points;
float duration;
bool closedLoop;
float constSpeed;
int repeat;
bool perpetual;
float minDepth;
float maxDepth;
};
class ClientViewer : public CMOOSApp
{
public:
ClientViewer();
~ClientViewer();
protected: // Standard MOOSApp functions to overload
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void RegisterVariables();
void processMessage(DUNE::IMC::Message * message);
private:
DUNE::IMC::Message * imcPollTCP();
DUNE::IMC::Message * imcPollUDP();
DUNE::Network::TCPSocket sock_tcp_send;
DUNE::Network::UDPSocket sock_udp_receive;
bool tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port);
int get_local_ip_using_create_socket(char* str_ip);
std::string SetPlan1(std::string sourceName, double stamp);
std::string SetPlan2(std::string sourceName, double stamp);
std::string ModifyPlan1(std::string sourceName, double stamp);
int SetEntityStatus(DUNE::IMC::EntityParameter& subMsg, std::string name, std::string value);
std::string generateEntityName(std::string parentName, std::string childName);
std::string generateEntityValue(std::string value, std::string type);
DUNE::IO::Poll m_poll_0;
DUNE::IO::Poll m_poll_1;
uint8_t* udpReceiveBuffer;
uint8_t* tcpReceiveBuffer;
std::string SetPlan1();
std::string SetPlan2();
std::string ethernetIP;
float initLon;
float initLat;
float initAlt;
CMOOSThread udpThread;
// UDPCommunicationEvent udpCommEvent;
// TCPCommunicationEvent tcpCommEvent;
};
#endif

115
src/pClientViewer/ClientViewer_Info.cpp Normal file
View File

@@ -0,0 +1,115 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: ClientViewer_Info.cpp */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#include <cstdlib>
#include <iostream>
#include "ClientViewer_Info.h"
#include "ColorParse.h"
#include "ReleaseInfo.h"
using namespace std;
//----------------------------------------------------------------
// Procedure: showSynopsis
void showSynopsis()
{
blk("SYNOPSIS: ");
blk("------------------------------------ ");
blk(" The pClientViewer application is used for ");
blk(" ");
blk(" ");
blk(" ");
blk(" ");
}
//----------------------------------------------------------------
// Procedure: showHelpAndExit
void showHelpAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("Usage: pClientViewer file.moos [OPTIONS] ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("Options: ");
mag(" --alias","=<ProcessName> ");
blk(" Launch pClientViewer with the given process name ");
blk(" rather than pClientViewer. ");
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 pClientViewer. ");
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("pClientViewer Example MOOS Configuration ");
blu("=============================================================== ");
blk(" ");
blk("ProcessConfig = pClientViewer ");
blk("{ ");
blk(" AppTick = 4 ");
blk(" CommsTick = 4 ");
blk(" ");
blk("} ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showInterfaceAndExit
void showInterfaceAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pClientViewer 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("pClientViewer", "gpl");
exit(0);
}

18
src/pClientViewer/ClientViewer_Info.h Normal file
View File

@@ -0,0 +1,18 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: ClientViewer_Info.h */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#ifndef ClientViewer_INFO_HEADER
#define ClientViewer_INFO_HEADER
void showSynopsis();
void showHelpAndExit();
void showExampleConfigAndExit();
void showInterfaceAndExit();
void showReleaseInfoAndExit();
#endif

52
src/pClientViewer/main.cpp Normal file
View File

@@ -0,0 +1,52 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: main.cpp */
/* DATE: */
/************************************************************/
#include <string>
#include "MBUtils.h"
#include "ColorParse.h"
#include "ClientViewer.h"
#include "ClientViewer_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 << "pClientViewer launching as " << run_command << endl;
cout << termColor() << endl;
ClientViewer ClientViewer;
ClientViewer.Run(run_command.c_str(), mission_file.c_str());
return(0);
}

9
src/pClientViewer/pClientViewer.moos Normal file
View File

@@ -0,0 +1,9 @@
//------------------------------------------------
// pClientViewer config block
ProcessConfig = pClientViewer
{
AppTick = 4
CommsTick = 4
}

44
src/pDataManagement/CMakeLists.txt Normal file
View File

@@ -0,0 +1,44 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pDataManagement
# Author(s): Xiaobin Zeng
#--------------------------------------------------------
SET(SRC
DataManagement.cpp
DataManagement_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(FindProtobuf)
# find_package(Protobuf REQUIRED)
# include_directories(${Protobuf_INCLUDE_DIR})
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
#find_package (jsoncpp NO_MODULE REQUIRED)
include_directories(/usr/include/jsoncpp/)
link_directories(/usr/local/lib/)
ADD_EXECUTABLE(pDataManagement ${SRC})
TARGET_LINK_LIBRARIES(pDataManagement
${MOOS_LIBRARIES}
${CMAKE_DL_LIBS}
${SYSTEM_LIBS}
${DUNE_LIB}
mbutil
m
pthread
jsoncpp
# jsoncpp_lib_static
# ${rosbag_LIBRARIES}
# protobuf
# protoc
# proto
# ${PROTOBUF_LIBRARY}
)

194
src/pDataManagement/DataManagement.cpp Normal file
View File

@@ -0,0 +1,194 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: DataManagement.cpp */
/* DATE: */
/************************************************************/
#include <iterator>
#include "MBUtils.h"
#include "DataManagement.h"
//#include "NavigationInfo.pb.h"
#include <json/json.h>
using namespace std;
//---------------------------------------------------------
// Constructor
DataManagement::DataManagement()
{
}
//---------------------------------------------------------
// Destructor
DataManagement::~DataManagement()
{
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool DataManagement::OnNewMail(MOOSMSG_LIST &NewMail)
{
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
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();
std::string estimatedStateString;
if(key == "uMotion_pose_fb")
{
estimatedStateString = msg.GetString();
std::string err;
Json::Value estimatedStateData;
Json::CharReaderBuilder builder;
const std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
if (!reader->parse(estimatedStateString.c_str(),
estimatedStateString.c_str() + static_cast<int>(estimatedStateString.length()),
&estimatedStateData,
&err))
{
std::cout << "error" << std::endl;
return EXIT_FAILURE;
}
std::string info = estimatedStateData["info"].asString();
float currentLon = estimatedStateData["currentLon"].asFloat();
float currentLat = estimatedStateData["currentLat"].asFloat();
float currentAltitude = estimatedStateData["currentAltitude"].asFloat();
float referenceLon = estimatedStateData["referenceLon"].asFloat();
float referenceLat = estimatedStateData["referenceLat"].asFloat();
float referenceAltitude = estimatedStateData["referenceAltitude"].asFloat();
float offsetNorth = estimatedStateData["offsetNorth"].asFloat();
float offsetEast = estimatedStateData["offsetEast"].asFloat();
float offsetDown = estimatedStateData["offsetDown"].asFloat();
float roll = estimatedStateData["roll"].asFloat();
float pitch = estimatedStateData["pitch"].asFloat();
float yaw = estimatedStateData["yaw"].asFloat();
float linearVelocityNorth = estimatedStateData["linearVelocityNorth"].asFloat();
float linearVelocityEast = estimatedStateData["linearVelocityEast"].asFloat();
float linearVelocityDown = estimatedStateData["linearVelocityDown"].asFloat();
float angularVelocityNorth = estimatedStateData["angularVelocityNorth"].asFloat();
float angularVelocityEast = estimatedStateData["angularVelocityEast"].asFloat();
float angularVelocityDown = estimatedStateData["angularVelocityDown"].asFloat();
float height = estimatedStateData["height"].asFloat();
float depth = estimatedStateData["depth"].asFloat();
std::cout << "info: " << info << std::endl;
std::cout << "referenceLon: " << referenceLon << std::endl;
std::cout << "referenceLat: " << referenceLat << std::endl;
std::cout << "referenceAltitude: " << referenceAltitude << std::endl;
std::cout << "currentLon: " << currentLon << std::endl;
std::cout << "currentLat: " << currentLat << std::endl;
std::cout << "altitude: " << currentAltitude << std::endl;
std::cout << "offsetNorth: " << offsetNorth << std::endl;
std::cout << "offsetEast: " << offsetEast << std::endl;
std::cout << "offsetDown: " << offsetDown << std::endl;
std::cout << "roll: " << roll << std::endl;
std::cout << "pitch: " << pitch << std::endl;
std::cout << "yaw: " << yaw << std::endl;
std::cout << "linearVelocityNorth: " << linearVelocityNorth << std::endl;
std::cout << "linearVelocityEast: " << linearVelocityEast << std::endl;
std::cout << "linearVelocityDown: " << linearVelocityDown << std::endl;
std::cout << "angularVelocityNorth: " << angularVelocityNorth << std::endl;
std::cout << "angularVelocityEast: " << angularVelocityEast << std::endl;
std::cout << "angularVelocityDown: " << angularVelocityDown << std::endl;
std::cout << "height: " << height << std::endl;
std::cout << "depth: " << depth << std::endl;
std::stringstream ss;
ss << std::fixed << std::setprecision(6) << getTimeStamp() << ","
<< info << ","
<< referenceLon << ","
<< referenceLat << ","
<< referenceAltitude << ","
<< currentLon << ","
<< currentLat << ","
<< currentAltitude << ","
<< offsetNorth << ","
<< offsetEast << ","
<< offsetDown << ","
<< roll << ","
<< pitch << ","
<< yaw << ","
<< height << ","
<< depth << std::endl;
Notify("uMotion_pose_log", ss.str());
}
}
return(true);
}
//---------------------------------------------------------
// Procedure: OnConnectToServer
bool DataManagement::OnConnectToServer()
{
RegisterVariables();
return(true);
}
//---------------------------------------------------------
// Procedure: Iterate()
// happens AppTick times per second
bool DataManagement::Iterate()
{
return(true);
}
//---------------------------------------------------------
// Procedure: OnStartUp()
// happens before connection is open
bool DataManagement::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 DataManagement::RegisterVariables()
{
// Register("FOOBAR", 0);
Register("uMotion_pose_fb", 0);
//Register("uMotion_pose_log", 0);
}
double DataManagement::getTimeStamp()
{
struct timeval tv;
gettimeofday(&tv,NULL);
double stamp = double(tv.tv_sec*1000000 + tv.tv_usec) / 1000000;
return stamp;
}

32
src/pDataManagement/DataManagement.h Normal file
View File

@@ -0,0 +1,32 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: DataManagement.h */
/* DATE: */
/************************************************************/
#ifndef DataManagement_HEADER
#define DataManagement_HEADER
#include "MOOS/libMOOS/MOOSLib.h"
class DataManagement : public CMOOSApp
{
public:
DataManagement();
~DataManagement();
protected: // Standard MOOSApp functions to overload
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
protected:
void RegisterVariables();
private:
double getTimeStamp();
};
#endif

115
src/pDataManagement/DataManagement_Info.cpp Normal file
View File

@@ -0,0 +1,115 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: DataManagement_Info.cpp */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#include <cstdlib>
#include <iostream>
#include "DataManagement_Info.h"
#include "ColorParse.h"
#include "ReleaseInfo.h"
using namespace std;
//----------------------------------------------------------------
// Procedure: showSynopsis
void showSynopsis()
{
blk("SYNOPSIS: ");
blk("------------------------------------ ");
blk(" The pDataManagement application is used for ");
blk(" ");
blk(" ");
blk(" ");
blk(" ");
}
//----------------------------------------------------------------
// Procedure: showHelpAndExit
void showHelpAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("Usage: pDataManagement file.moos [OPTIONS] ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("Options: ");
mag(" --alias","=<ProcessName> ");
blk(" Launch pDataManagement with the given process name ");
blk(" rather than pDataManagement. ");
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 pDataManagement. ");
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("pDataManagement Example MOOS Configuration ");
blu("=============================================================== ");
blk(" ");
blk("ProcessConfig = pDataManagement ");
blk("{ ");
blk(" AppTick = 4 ");
blk(" CommsTick = 4 ");
blk(" ");
blk("} ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showInterfaceAndExit
void showInterfaceAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pDataManagement 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("pDataManagement", "gpl");
exit(0);
}

18
src/pDataManagement/DataManagement_Info.h Normal file
View File

@@ -0,0 +1,18 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: DataManagement_Info.h */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#ifndef DataManagement_INFO_HEADER
#define DataManagement_INFO_HEADER
void showSynopsis();
void showHelpAndExit();
void showExampleConfigAndExit();
void showInterfaceAndExit();
void showReleaseInfoAndExit();
#endif

809
src/pDataManagement/NavigationInfo.pb.h Normal file
View File

@@ -0,0 +1,809 @@
// Generated by the protocol buffer compiler. DO NOT EDIT!
// source: NavigationInfo.proto
#ifndef GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto
#define GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto
#include <limits>
#include <string>
#include <google/protobuf/port_def.inc>
#if PROTOBUF_VERSION < 3021000
#error This file was generated by a newer version of protoc which is
#error incompatible with your Protocol Buffer headers. Please update
#error your headers.
#endif
#if 3021012 < PROTOBUF_MIN_PROTOC_VERSION
#error This file was generated by an older version of protoc which is
#error incompatible with your Protocol Buffer headers. Please
#error regenerate this file with a newer version of protoc.
#endif
#include <google/protobuf/port_undef.inc>
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/arena.h>
#include <google/protobuf/arenastring.h>
#include <google/protobuf/generated_message_util.h>
#include <google/protobuf/metadata_lite.h>
#include <google/protobuf/generated_message_reflection.h>
#include <google/protobuf/message.h>
#include <google/protobuf/repeated_field.h> // IWYU pragma: export
#include <google/protobuf/extension_set.h> // IWYU pragma: export
#include <google/protobuf/unknown_field_set.h>
// @@protoc_insertion_point(includes)
#include <google/protobuf/port_def.inc>
#define PROTOBUF_INTERNAL_EXPORT_NavigationInfo_2eproto
PROTOBUF_NAMESPACE_OPEN
namespace internal {
class AnyMetadata;
} // namespace internal
PROTOBUF_NAMESPACE_CLOSE
// Internal implementation detail -- do not use these members.
struct TableStruct_NavigationInfo_2eproto {
static const uint32_t offsets[];
};
extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_NavigationInfo_2eproto;
namespace NavigationInfo {
class EstimatedState;
struct EstimatedStateDefaultTypeInternal;
extern EstimatedStateDefaultTypeInternal _EstimatedState_default_instance_;
} // namespace NavigationInfo
PROTOBUF_NAMESPACE_OPEN
template<> ::NavigationInfo::EstimatedState* Arena::CreateMaybeMessage<::NavigationInfo::EstimatedState>(Arena*);
PROTOBUF_NAMESPACE_CLOSE
namespace NavigationInfo {
// ===================================================================
class EstimatedState final :
public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:NavigationInfo.EstimatedState) */ {
public:
inline EstimatedState() : EstimatedState(nullptr) {}
~EstimatedState() override;
explicit PROTOBUF_CONSTEXPR EstimatedState(::PROTOBUF_NAMESPACE_ID::internal::ConstantInitialized);
EstimatedState(const EstimatedState& from);
EstimatedState(EstimatedState&& from) noexcept
: EstimatedState() {
*this = ::std::move(from);
}
inline EstimatedState& operator=(const EstimatedState& from) {
CopyFrom(from);
return *this;
}
inline EstimatedState& operator=(EstimatedState&& from) noexcept {
if (this == &from) return *this;
if (GetOwningArena() == from.GetOwningArena()
#ifdef PROTOBUF_FORCE_COPY_IN_MOVE
&& GetOwningArena() != nullptr
#endif // !PROTOBUF_FORCE_COPY_IN_MOVE
) {
InternalSwap(&from);
} else {
CopyFrom(from);
}
return *this;
}
static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
return GetDescriptor();
}
static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
return default_instance().GetMetadata().descriptor;
}
static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
return default_instance().GetMetadata().reflection;
}
static const EstimatedState& default_instance() {
return *internal_default_instance();
}
static inline const EstimatedState* internal_default_instance() {
return reinterpret_cast<const EstimatedState*>(
&_EstimatedState_default_instance_);
}
static constexpr int kIndexInFileMessages =
0;
friend void swap(EstimatedState& a, EstimatedState& b) {
a.Swap(&b);
}
inline void Swap(EstimatedState* other) {
if (other == this) return;
#ifdef PROTOBUF_FORCE_COPY_IN_SWAP
if (GetOwningArena() != nullptr &&
GetOwningArena() == other->GetOwningArena()) {
#else // PROTOBUF_FORCE_COPY_IN_SWAP
if (GetOwningArena() == other->GetOwningArena()) {
#endif // !PROTOBUF_FORCE_COPY_IN_SWAP
InternalSwap(other);
} else {
::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
}
}
void UnsafeArenaSwap(EstimatedState* other) {
if (other == this) return;
GOOGLE_DCHECK(GetOwningArena() == other->GetOwningArena());
InternalSwap(other);
}
// implements Message ----------------------------------------------
EstimatedState* New(::PROTOBUF_NAMESPACE_ID::Arena* arena = nullptr) const final {
return CreateMaybeMessage<EstimatedState>(arena);
}
using ::PROTOBUF_NAMESPACE_ID::Message::CopyFrom;
void CopyFrom(const EstimatedState& from);
using ::PROTOBUF_NAMESPACE_ID::Message::MergeFrom;
void MergeFrom( const EstimatedState& from) {
EstimatedState::MergeImpl(*this, from);
}
private:
static void MergeImpl(::PROTOBUF_NAMESPACE_ID::Message& to_msg, const ::PROTOBUF_NAMESPACE_ID::Message& from_msg);
public:
PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
bool IsInitialized() const final;
size_t ByteSizeLong() const final;
const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
uint8_t* _InternalSerialize(
uint8_t* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
int GetCachedSize() const final { return _impl_._cached_size_.Get(); }
private:
void SharedCtor(::PROTOBUF_NAMESPACE_ID::Arena* arena, bool is_message_owned);
void SharedDtor();
void SetCachedSize(int size) const final;
void InternalSwap(EstimatedState* other);
private:
friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
return "NavigationInfo.EstimatedState";
}
protected:
explicit EstimatedState(::PROTOBUF_NAMESPACE_ID::Arena* arena,
bool is_message_owned = false);
public:
static const ClassData _class_data_;
const ::PROTOBUF_NAMESPACE_ID::Message::ClassData*GetClassData() const final;
::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
// nested types ----------------------------------------------------
// accessors -------------------------------------------------------
enum : int {
kInfoFieldNumber = 1,
kLonFieldNumber = 2,
kLatFieldNumber = 3,
kHeightFieldNumber = 4,
kOffsetNorthFieldNumber = 5,
kOffsetEastFieldNumber = 6,
kOffsetDownFieldNumber = 7,
kRollFieldNumber = 8,
kPitchFieldNumber = 9,
kYawFieldNumber = 10,
kLinearVelocityNorthFieldNumber = 11,
kLinearVelocityEastFieldNumber = 12,
kLinearVelocityDownFieldNumber = 13,
kAngularVelocityNorthFieldNumber = 14,
kAngularVelocityEastFieldNumber = 15,
kAngularVelocityDownFieldNumber = 16,
kDepthFieldNumber = 17,
kAltitudeFieldNumber = 18,
};
// string info = 1;
void clear_info();
const std::string& info() const;
template <typename ArgT0 = const std::string&, typename... ArgT>
void set_info(ArgT0&& arg0, ArgT... args);
std::string* mutable_info();
PROTOBUF_NODISCARD std::string* release_info();
void set_allocated_info(std::string* info);
private:
const std::string& _internal_info() const;
inline PROTOBUF_ALWAYS_INLINE void _internal_set_info(const std::string& value);
std::string* _internal_mutable_info();
public:
// float lon = 2;
void clear_lon();
float lon() const;
void set_lon(float value);
private:
float _internal_lon() const;
void _internal_set_lon(float value);
public:
// float lat = 3;
void clear_lat();
float lat() const;
void set_lat(float value);
private:
float _internal_lat() const;
void _internal_set_lat(float value);
public:
// float height = 4;
void clear_height();
float height() const;
void set_height(float value);
private:
float _internal_height() const;
void _internal_set_height(float value);
public:
// float offsetNorth = 5;
void clear_offsetnorth();
float offsetnorth() const;
void set_offsetnorth(float value);
private:
float _internal_offsetnorth() const;
void _internal_set_offsetnorth(float value);
public:
// float offsetEast = 6;
void clear_offseteast();
float offseteast() const;
void set_offseteast(float value);
private:
float _internal_offseteast() const;
void _internal_set_offseteast(float value);
public:
// float offsetDown = 7;
void clear_offsetdown();
float offsetdown() const;
void set_offsetdown(float value);
private:
float _internal_offsetdown() const;
void _internal_set_offsetdown(float value);
public:
// float roll = 8;
void clear_roll();
float roll() const;
void set_roll(float value);
private:
float _internal_roll() const;
void _internal_set_roll(float value);
public:
// float pitch = 9;
void clear_pitch();
float pitch() const;
void set_pitch(float value);
private:
float _internal_pitch() const;
void _internal_set_pitch(float value);
public:
// float yaw = 10;
void clear_yaw();
float yaw() const;
void set_yaw(float value);
private:
float _internal_yaw() const;
void _internal_set_yaw(float value);
public:
// float linearVelocityNorth = 11;
void clear_linearvelocitynorth();
float linearvelocitynorth() const;
void set_linearvelocitynorth(float value);
private:
float _internal_linearvelocitynorth() const;
void _internal_set_linearvelocitynorth(float value);
public:
// float linearVelocityEast = 12;
void clear_linearvelocityeast();
float linearvelocityeast() const;
void set_linearvelocityeast(float value);
private:
float _internal_linearvelocityeast() const;
void _internal_set_linearvelocityeast(float value);
public:
// float linearVelocityDown = 13;
void clear_linearvelocitydown();
float linearvelocitydown() const;
void set_linearvelocitydown(float value);
private:
float _internal_linearvelocitydown() const;
void _internal_set_linearvelocitydown(float value);
public:
// float angularVelocityNorth = 14;
void clear_angularvelocitynorth();
float angularvelocitynorth() const;
void set_angularvelocitynorth(float value);
private:
float _internal_angularvelocitynorth() const;
void _internal_set_angularvelocitynorth(float value);
public:
// float angularVelocityEast = 15;
void clear_angularvelocityeast();
float angularvelocityeast() const;
void set_angularvelocityeast(float value);
private:
float _internal_angularvelocityeast() const;
void _internal_set_angularvelocityeast(float value);
public:
// float angularVelocityDown = 16;
void clear_angularvelocitydown();
float angularvelocitydown() const;
void set_angularvelocitydown(float value);
private:
float _internal_angularvelocitydown() const;
void _internal_set_angularvelocitydown(float value);
public:
// float depth = 17;
void clear_depth();
float depth() const;
void set_depth(float value);
private:
float _internal_depth() const;
void _internal_set_depth(float value);
public:
// float altitude = 18;
void clear_altitude();
float altitude() const;
void set_altitude(float value);
private:
float _internal_altitude() const;
void _internal_set_altitude(float value);
public:
// @@protoc_insertion_point(class_scope:NavigationInfo.EstimatedState)
private:
class _Internal;
template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
typedef void InternalArenaConstructable_;
typedef void DestructorSkippable_;
struct Impl_ {
::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr info_;
float lon_;
float lat_;
float height_;
float offsetnorth_;
float offseteast_;
float offsetdown_;
float roll_;
float pitch_;
float yaw_;
float linearvelocitynorth_;
float linearvelocityeast_;
float linearvelocitydown_;
float angularvelocitynorth_;
float angularvelocityeast_;
float angularvelocitydown_;
float depth_;
float altitude_;
mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
};
union { Impl_ _impl_; };
friend struct ::TableStruct_NavigationInfo_2eproto;
};
// ===================================================================
// ===================================================================
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
#endif // __GNUC__
// EstimatedState
// string info = 1;
inline void EstimatedState::clear_info() {
_impl_.info_.ClearToEmpty();
}
inline const std::string& EstimatedState::info() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.info)
return _internal_info();
}
template <typename ArgT0, typename... ArgT>
inline PROTOBUF_ALWAYS_INLINE
void EstimatedState::set_info(ArgT0&& arg0, ArgT... args) {
_impl_.info_.Set(static_cast<ArgT0 &&>(arg0), args..., GetArenaForAllocation());
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.info)
}
inline std::string* EstimatedState::mutable_info() {
std::string* _s = _internal_mutable_info();
// @@protoc_insertion_point(field_mutable:NavigationInfo.EstimatedState.info)
return _s;
}
inline const std::string& EstimatedState::_internal_info() const {
return _impl_.info_.Get();
}
inline void EstimatedState::_internal_set_info(const std::string& value) {
_impl_.info_.Set(value, GetArenaForAllocation());
}
inline std::string* EstimatedState::_internal_mutable_info() {
return _impl_.info_.Mutable(GetArenaForAllocation());
}
inline std::string* EstimatedState::release_info() {
// @@protoc_insertion_point(field_release:NavigationInfo.EstimatedState.info)
return _impl_.info_.Release();
}
inline void EstimatedState::set_allocated_info(std::string* info) {
if (info != nullptr) {
} else {
}
_impl_.info_.SetAllocated(info, GetArenaForAllocation());
#ifdef PROTOBUF_FORCE_COPY_DEFAULT_STRING
if (_impl_.info_.IsDefault()) {
_impl_.info_.Set("", GetArenaForAllocation());
}
#endif // PROTOBUF_FORCE_COPY_DEFAULT_STRING
// @@protoc_insertion_point(field_set_allocated:NavigationInfo.EstimatedState.info)
}
// float lon = 2;
inline void EstimatedState::clear_lon() {
_impl_.lon_ = 0;
}
inline float EstimatedState::_internal_lon() const {
return _impl_.lon_;
}
inline float EstimatedState::lon() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.lon)
return _internal_lon();
}
inline void EstimatedState::_internal_set_lon(float value) {
_impl_.lon_ = value;
}
inline void EstimatedState::set_lon(float value) {
_internal_set_lon(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.lon)
}
// float lat = 3;
inline void EstimatedState::clear_lat() {
_impl_.lat_ = 0;
}
inline float EstimatedState::_internal_lat() const {
return _impl_.lat_;
}
inline float EstimatedState::lat() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.lat)
return _internal_lat();
}
inline void EstimatedState::_internal_set_lat(float value) {
_impl_.lat_ = value;
}
inline void EstimatedState::set_lat(float value) {
_internal_set_lat(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.lat)
}
// float height = 4;
inline void EstimatedState::clear_height() {
_impl_.height_ = 0;
}
inline float EstimatedState::_internal_height() const {
return _impl_.height_;
}
inline float EstimatedState::height() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.height)
return _internal_height();
}
inline void EstimatedState::_internal_set_height(float value) {
_impl_.height_ = value;
}
inline void EstimatedState::set_height(float value) {
_internal_set_height(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.height)
}
// float offsetNorth = 5;
inline void EstimatedState::clear_offsetnorth() {
_impl_.offsetnorth_ = 0;
}
inline float EstimatedState::_internal_offsetnorth() const {
return _impl_.offsetnorth_;
}
inline float EstimatedState::offsetnorth() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetNorth)
return _internal_offsetnorth();
}
inline void EstimatedState::_internal_set_offsetnorth(float value) {
_impl_.offsetnorth_ = value;
}
inline void EstimatedState::set_offsetnorth(float value) {
_internal_set_offsetnorth(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetNorth)
}
// float offsetEast = 6;
inline void EstimatedState::clear_offseteast() {
_impl_.offseteast_ = 0;
}
inline float EstimatedState::_internal_offseteast() const {
return _impl_.offseteast_;
}
inline float EstimatedState::offseteast() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetEast)
return _internal_offseteast();
}
inline void EstimatedState::_internal_set_offseteast(float value) {
_impl_.offseteast_ = value;
}
inline void EstimatedState::set_offseteast(float value) {
_internal_set_offseteast(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetEast)
}
// float offsetDown = 7;
inline void EstimatedState::clear_offsetdown() {
_impl_.offsetdown_ = 0;
}
inline float EstimatedState::_internal_offsetdown() const {
return _impl_.offsetdown_;
}
inline float EstimatedState::offsetdown() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.offsetDown)
return _internal_offsetdown();
}
inline void EstimatedState::_internal_set_offsetdown(float value) {
_impl_.offsetdown_ = value;
}
inline void EstimatedState::set_offsetdown(float value) {
_internal_set_offsetdown(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.offsetDown)
}
// float roll = 8;
inline void EstimatedState::clear_roll() {
_impl_.roll_ = 0;
}
inline float EstimatedState::_internal_roll() const {
return _impl_.roll_;
}
inline float EstimatedState::roll() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.roll)
return _internal_roll();
}
inline void EstimatedState::_internal_set_roll(float value) {
_impl_.roll_ = value;
}
inline void EstimatedState::set_roll(float value) {
_internal_set_roll(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.roll)
}
// float pitch = 9;
inline void EstimatedState::clear_pitch() {
_impl_.pitch_ = 0;
}
inline float EstimatedState::_internal_pitch() const {
return _impl_.pitch_;
}
inline float EstimatedState::pitch() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.pitch)
return _internal_pitch();
}
inline void EstimatedState::_internal_set_pitch(float value) {
_impl_.pitch_ = value;
}
inline void EstimatedState::set_pitch(float value) {
_internal_set_pitch(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.pitch)
}
// float yaw = 10;
inline void EstimatedState::clear_yaw() {
_impl_.yaw_ = 0;
}
inline float EstimatedState::_internal_yaw() const {
return _impl_.yaw_;
}
inline float EstimatedState::yaw() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.yaw)
return _internal_yaw();
}
inline void EstimatedState::_internal_set_yaw(float value) {
_impl_.yaw_ = value;
}
inline void EstimatedState::set_yaw(float value) {
_internal_set_yaw(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.yaw)
}
// float linearVelocityNorth = 11;
inline void EstimatedState::clear_linearvelocitynorth() {
_impl_.linearvelocitynorth_ = 0;
}
inline float EstimatedState::_internal_linearvelocitynorth() const {
return _impl_.linearvelocitynorth_;
}
inline float EstimatedState::linearvelocitynorth() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityNorth)
return _internal_linearvelocitynorth();
}
inline void EstimatedState::_internal_set_linearvelocitynorth(float value) {
_impl_.linearvelocitynorth_ = value;
}
inline void EstimatedState::set_linearvelocitynorth(float value) {
_internal_set_linearvelocitynorth(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityNorth)
}
// float linearVelocityEast = 12;
inline void EstimatedState::clear_linearvelocityeast() {
_impl_.linearvelocityeast_ = 0;
}
inline float EstimatedState::_internal_linearvelocityeast() const {
return _impl_.linearvelocityeast_;
}
inline float EstimatedState::linearvelocityeast() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityEast)
return _internal_linearvelocityeast();
}
inline void EstimatedState::_internal_set_linearvelocityeast(float value) {
_impl_.linearvelocityeast_ = value;
}
inline void EstimatedState::set_linearvelocityeast(float value) {
_internal_set_linearvelocityeast(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityEast)
}
// float linearVelocityDown = 13;
inline void EstimatedState::clear_linearvelocitydown() {
_impl_.linearvelocitydown_ = 0;
}
inline float EstimatedState::_internal_linearvelocitydown() const {
return _impl_.linearvelocitydown_;
}
inline float EstimatedState::linearvelocitydown() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.linearVelocityDown)
return _internal_linearvelocitydown();
}
inline void EstimatedState::_internal_set_linearvelocitydown(float value) {
_impl_.linearvelocitydown_ = value;
}
inline void EstimatedState::set_linearvelocitydown(float value) {
_internal_set_linearvelocitydown(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.linearVelocityDown)
}
// float angularVelocityNorth = 14;
inline void EstimatedState::clear_angularvelocitynorth() {
_impl_.angularvelocitynorth_ = 0;
}
inline float EstimatedState::_internal_angularvelocitynorth() const {
return _impl_.angularvelocitynorth_;
}
inline float EstimatedState::angularvelocitynorth() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityNorth)
return _internal_angularvelocitynorth();
}
inline void EstimatedState::_internal_set_angularvelocitynorth(float value) {
_impl_.angularvelocitynorth_ = value;
}
inline void EstimatedState::set_angularvelocitynorth(float value) {
_internal_set_angularvelocitynorth(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityNorth)
}
// float angularVelocityEast = 15;
inline void EstimatedState::clear_angularvelocityeast() {
_impl_.angularvelocityeast_ = 0;
}
inline float EstimatedState::_internal_angularvelocityeast() const {
return _impl_.angularvelocityeast_;
}
inline float EstimatedState::angularvelocityeast() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityEast)
return _internal_angularvelocityeast();
}
inline void EstimatedState::_internal_set_angularvelocityeast(float value) {
_impl_.angularvelocityeast_ = value;
}
inline void EstimatedState::set_angularvelocityeast(float value) {
_internal_set_angularvelocityeast(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityEast)
}
// float angularVelocityDown = 16;
inline void EstimatedState::clear_angularvelocitydown() {
_impl_.angularvelocitydown_ = 0;
}
inline float EstimatedState::_internal_angularvelocitydown() const {
return _impl_.angularvelocitydown_;
}
inline float EstimatedState::angularvelocitydown() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.angularVelocityDown)
return _internal_angularvelocitydown();
}
inline void EstimatedState::_internal_set_angularvelocitydown(float value) {
_impl_.angularvelocitydown_ = value;
}
inline void EstimatedState::set_angularvelocitydown(float value) {
_internal_set_angularvelocitydown(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.angularVelocityDown)
}
// float depth = 17;
inline void EstimatedState::clear_depth() {
_impl_.depth_ = 0;
}
inline float EstimatedState::_internal_depth() const {
return _impl_.depth_;
}
inline float EstimatedState::depth() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.depth)
return _internal_depth();
}
inline void EstimatedState::_internal_set_depth(float value) {
_impl_.depth_ = value;
}
inline void EstimatedState::set_depth(float value) {
_internal_set_depth(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.depth)
}
// float altitude = 18;
inline void EstimatedState::clear_altitude() {
_impl_.altitude_ = 0;
}
inline float EstimatedState::_internal_altitude() const {
return _impl_.altitude_;
}
inline float EstimatedState::altitude() const {
// @@protoc_insertion_point(field_get:NavigationInfo.EstimatedState.altitude)
return _internal_altitude();
}
inline void EstimatedState::_internal_set_altitude(float value) {
_impl_.altitude_ = value;
}
inline void EstimatedState::set_altitude(float value) {
_internal_set_altitude(value);
// @@protoc_insertion_point(field_set:NavigationInfo.EstimatedState.altitude)
}
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif // __GNUC__
// @@protoc_insertion_point(namespace_scope)
} // namespace NavigationInfo
// @@protoc_insertion_point(global_scope)
#include <google/protobuf/port_undef.inc>
#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto

58
src/pDataManagement/main.cpp Normal file
View File

@@ -0,0 +1,58 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: main.cpp */
/* DATE: */
/************************************************************/
#include <string>
#include "MBUtils.h"
#include "ColorParse.h"
#include "DataManagement.h"
#include "DataManagement_Info.h"
// #include <ros/ros.h>
// #include <rosbag/bag.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 << "pDataManagement launching as " << run_command << endl;
cout << termColor() << endl;
// ros::init(argc, argv, "log2rosbag");
// ros::NodeHandle nh;
DataManagement DataManagement;
DataManagement.Run(run_command.c_str(), mission_file.c_str());
return(0);
}

9
src/pDataManagement/pDataManagement.moos Normal file
View File

@@ -0,0 +1,9 @@
//------------------------------------------------
// pDataManagement config block
ProcessConfig = pDataManagement
{
AppTick = 4
CommsTick = 4
}

1
src/pEmulator/.LastOpenedMOOSLogDirectory Normal file
View File

@@ -0,0 +1 @@
LastOpenedLoggingDirectory=./MOOSLog_24_10_2023_____11_46_34

21
src/pEmulator/CMakeLists.txt Normal file
View File

@@ -0,0 +1,21 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pTaskManger
# Author(s): zjk
#--------------------------------------------------------
# set(INCLUDE_IVP /home/zjk/Software/moos-ivp/include/ivp)
FILE(GLOB SRC *.cpp *.hpp)
ADD_EXECUTABLE(pEmulator ${SRC})
#需要把被依赖的库放到依赖库的后面
TARGET_LINK_LIBRARIES(pEmulator
${MOOS_LIBRARIES}
${MOOSGeodesy_LIBRARIES}
contacts
geometry
apputil
mbutil
m
pthread
)

416
src/pEmulator/Emulator.cpp Normal file
View File

@@ -0,0 +1,416 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-08 17:33:41
* @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
// #define DEBUG
#include"Emulator.hpp"
#include <exception>
#include"ACTable.h"
bool _ListenCB(void * pParam)
{
Emulator* pMe = (Emulator* )pParam;
return pMe->receiveUdpDate();
}
bool _ListenBSC(void* pParam)
{
_150server* pMe = (_150server*)pParam;
return pMe->listenInfo();
}
bool _Connect(void* pParam)
{
Emulator* pMe = (Emulator* )pParam;
return pMe->_150Connect();
}
bool Emulator::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
string key = msg.m_sKey;
string sval = msg.m_sVal;
double dval = msg.m_dfVal;
double dfT;
msg.IsSkewed(m_curr_time, &dfT);
// if(key == "DESIRED_RUDDER")
// {
// sendBuf[0] = dval;
// remus100.rudder = dval;
// }
// else if(key == "DESIRED_ELEVATOR")
// {
// sendBuf[1] = dval;
// remus100.elevator = dval;
// }
// else if(key == "DESIRED_THRUST")
// {
// sendBuf[2] = dval;
// remus100.thrust = dval;
// }
// else
if(key == "DESIRED_DEPTH")
sendBuf[3] = dval;
else if(key == "DESIRED_HEADING")
sendBuf[4] = dval;
else if(key == "DESIRED_SPEED")
sendBuf[5] = dval;
// reportRunWarning("UnKown var:" + key);
}
return true;
}
bool Emulator::Iterate()
{
AppCastingMOOSApp::Iterate();
if(_150ServerThread.IsThreadRunning())
{ // 这里需要转换数据
if(p_150server_1._150cmd.helm_top_angle > 128)
sendBuf[0] = -(p_150server_1._150cmd.helm_top_angle - 128);
else
sendBuf[0] = p_150server_1._150cmd.helm_top_angle;
if(p_150server_1._150cmd.helm_right_angle > 128)
sendBuf[1] = -(p_150server_1._150cmd.helm_right_angle - 128);
else
sendBuf[1] = p_150server_1._150cmd.helm_right_angle;
sendBuf[2] = (uint8_t)p_150server_1._150cmd.thrust * 1525 / 100;
set150Info();
p_150server_1.postInfo();
isConnect = " 150 Server...";
}
else
isConnect = "No 150 Connect...";
udp->iSendMessageTo((void*)sendBuf, sendBufSize, matalb_port, matlab_host);
//postNodeRecordUpdate(prefix, record);
AppCastingMOOSApp::PostReport();
return true;
}
bool Emulator::OnConnectToServer()
{
registerVariables();
return true;
}
bool Emulator::OnStartUp()
{
AppCastingMOOSApp::OnStartUp();
//TODO: 添加初始状态设置
STRING_LIST sParams;
m_MissionReader.GetConfiguration(GetAppName(), sParams);
STRING_LIST::iterator p;
for(p=sParams.begin(); p!=sParams.end(); p++)
{
bool unhandled = false;
string orig = *p;
string line = *p;
string param = toupper(biteStringX(line, '='));
string value = line;
double dval = atof(value.c_str());
cout << param << endl;
if(param == "MATLAB_HOST")
{
matlab_host = value;
}
else if(param == "MATLAB_PORT")
{
matalb_port = (long int)dval;
}
else if(param == "LOCAL_PORT")
{
local_port = (long int)dval;
}
else if(param == "PREFIX")
{
setNonWhiteVarOnString(prefix, value);
}
else if(param == "START_X")
{
start_x = dval;
}
else if(param == "START_Y")
{
start_y = dval;
}
else if(param == "START_Z")
{
start_z = dval;
}
else if(param == "START_HEADING")
{
start_heading = dval;
}
else
unhandled = true;
if(unhandled)
reportUnhandledConfigWarning(orig);
}
//添加UDP 设置
//TODO：添加try
try
{
udp = new XPCUdpSocket(local_port);
udp->vSetBroadcast(true);
udp->vBindSocket();
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
reportRunWarning("Local port Open Field");
}
//初始化数据
receiveBufSize = receiveDateSize*sizeof(double);
receiveBuf = new char[receiveBufSize];
sendBufSize = sendDataSize*sizeof(double);
sendBuf = new double[sendDataSize];
for(int i=0; i<sendDataSize; i++)
sendBuf[i] = 0;
for(int i=0; i<receiveBufSize; i++)
receiveBuf[i] = 0;
// sendBuf[0] = 0.1;
// sendBuf[1] = 0.1;
// sendBuf[2] = 1525;
record.setX(start_x);
record.setY(start_y);
record.setDepth(start_z);
record.setHeading(start_heading);
receiveThread.Initialise(_ListenCB,this);
receiveThread.Start();
_150ConnectThread.Initialise(_Connect, this);
_150ConnectThread.Start();
//启动150 server
//
//
return true;
}
bool Emulator::_150Connect()
{
cout << "Connect ... " << endl;
isConnect = "Connect...";
p_150server_1._150_startServer();
_150ServerThread.Initialise(_ListenBSC,&(this->p_150server_1));
_150ServerThread.Start();
_150ConnectThread.RequestQuit();
isConnect = "Connected and start server";
}
void Emulator::registerVariables()
{
AppCastingMOOSApp::RegisterVariables();
Register("DESIRED_RUDDER",0);
Register("DESIRED_THRUST",0);
Register("DESIRED_ELEVATOR",0);
Register("DESIRED_DEPTH");
Register("DESIRED_HEADING");
Register("DESIRED_SPEED");
}
bool Emulator::buildReport()
{
// for(int i=0; i<sendDataSize; i++)
// {
// m_msgs << sendBuf[i] << "!#";
// }
m_msgs << "MATLAB : " << matlab_host << " : " << matalb_port << endl;
m_msgs << "LOCAL : " << local_port << endl;
m_msgs << "_150 Connect: " << isConnect << endl;
m_msgs << "_150 Cmd Recive: " << p_150server_1._150_recive << endl;
m_msgs << "=============================================" << endl;
double *s = (double*)receiveBuf;
m_msgs << "DESIRED_RUDDER : " << sendBuf[0] << " (deg)" << endl;
m_msgs << "DESIRED_ELEVATOR : " << sendBuf[1] << " (deg)" << endl;
m_msgs << "DESIRED_THRUST : " << sendBuf[2] << " (rpm)" << endl;
m_msgs << "Pose : " << "[ " << doubleToStringX(s[6],1) << " , " << doubleToStringX(s[7],1) << " ]" << endl;
m_msgs << "Depth : " << doubleToStringX(s[8],1) << " (m)" << endl;
m_msgs << "u : " << doubleToStringX(s[0],2) << " (m/s)" << endl;
m_msgs << "v : " << doubleToStringX(s[1],2) << " (m/s)" << endl;
m_msgs << "w : " << doubleToStringX(s[2],2) << " (m/s)" << endl;
m_msgs << "p : " << doubleToStringX(s[3],1) << " (deg/s)" << endl;
m_msgs << "q : " << doubleToStringX(s[4],1) << " (deg/s)" << endl;
m_msgs << "r : " << doubleToStringX(s[5],1) << " (deg/s)" << endl;
m_msgs << "roll : " << doubleToStringX(s[9],1) << " (deg)" << endl;
m_msgs << "pitch : " << doubleToStringX(s[10],1) << " (deg)" << endl;
m_msgs << "yaw : " << doubleToStringX(s[11],1) << " (deg)" << endl;
m_msgs << "Lat : " << doubleToStringX(s[12],5) << " (du)" << endl;
m_msgs << "Lon : " << doubleToStringX(s[13],5) << " (du)" << endl;
m_msgs << "Alt : " << doubleToStringX(s[14],5) << " (m)" << endl;
return(true);
}
void Emulator::postNodeRecordUpdate(std::string, const NodeRecord& record)
{
double nav_x = record.getX();
double nav_y = record.getY();
Notify(prefix+"_X", nav_x, m_curr_time);
Notify(prefix+"_Y", nav_y, m_curr_time);
if(geoOk) {
double nav_lat = record.getLat();
double nav_lon = record.getLon();
Notify(prefix+"_LAT", nav_lat, m_curr_time);
Notify(prefix+"_LONG", nav_lon, m_curr_time);
}
double new_speed = record.getSpeed();
new_speed = snapToStep(new_speed, 0.01);
Notify(prefix+"_HEADING", record.getHeading(), m_curr_time);
Notify(prefix+"_SPEED", new_speed, m_curr_time);
Notify(prefix+"_DEPTH", record.getDepth(), m_curr_time);
// Added by HS 120124 to make it work ok with iHuxley
// Notify("SIMULATION_MODE","TRUE", m_curr_time);
Notify(prefix+"_Z", -record.getDepth(), m_curr_time);
Notify(prefix+"_PITCH", record.getPitch(), m_curr_time);
Notify(prefix+"_YAW", record.getYaw(), m_curr_time);
Notify("TRUE_X", nav_x, m_curr_time);
Notify("TRUE_Y", nav_y, m_curr_time);
double hog = angle360(record.getHeadingOG());
double sog = record.getSpeedOG();
Notify(prefix+"_HEADING_OVER_GROUND", hog, m_curr_time);
Notify(prefix+"_SPEED_OVER_GROUND", sog, m_curr_time);
if(record.isSetAltitude())
Notify(prefix+"_ALTITUDE", record.getAltitude(), m_curr_time);
}
bool Emulator::receiveUdpDate()
{
double u,v,w;
double U;
while(!receiveThread.IsQuitRequested())
{
int iRx = udp->iRecieveMessage(receiveBuf,receiveBufSize);
if(iRx)
{
for(int i=0; i<receiveDateSize; i++)
{
// MOOSTrace(doubleToString(*((double*)receiveBuf+i))+"\n");
double data = *((double*)receiveBuf+i);
switch (i)
{
case 0: //u
u = data;
remus100.u = u*100;
break;
case 1: //v
// record.set
v = data;
remus100.v = v*100;
break;
case 2: //w
// record
w = data;
remus100.w = w*100;
U = sqrt(u*u + v*v + w*w);
record.setSpeed(U);
break;
case 3: //p
remus100.p = data*100;
break;
case 4: //q
remus100.q = data*100;
break;
case 5: //r
remus100.r = data*100;
break;
case 6: //x
remus100.x = data*100;
record.setY(data);
break;
case 7: //y
remus100.y = data*100;
record.setX(data);
break;
case 8: //z
remus100.z = data*100;
record.setDepth(data);
break;
case 9: //phi
remus100.roll = data*100;
break;
case 10: //theta
remus100.pitch = data*100;
record.setPitch(data);
break;
case 11: //psi
remus100.yaw = data*100;
record.setHeading(data);
break;
case 12:
remus100.lat = data;
record.setLat(data);
break;
case 13:
remus100.lon = data;
record.setLon(data);
break;
case 14:
remus100.alt = data;
break;
default:
break;
}
}
}
}
}
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.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]
p_150server_1.embeddedInfoSrc.pitch = (int16_t)remus100.pitch; //8:[12,13]
p_150server_1.embeddedInfoSrc.roll = (int16_t)remus100.roll; //9:[14,15]
p_150server_1.embeddedInfoSrc.ins_vx = (int16_t)remus100.u; //10:[16,17]
p_150server_1.embeddedInfoSrc.ins_vy = (int16_t)remus100.v; //11:[18,19]
p_150server_1.embeddedInfoSrc.ins_vz = (int16_t)remus100.w; //12:[20,21]
p_150server_1.embeddedInfoSrc.lon = (int32_t)(remus100.lon * 1000000); //13:[22,23,24,25]
p_150server_1.embeddedInfoSrc.lat = (int32_t)(remus100.lat * 1000000) ; //14:[26,27,28,29]
p_150server_1.embeddedInfoSrc.alt = (int16_t)(remus100.alt * 100);//15:[30,31]
p_150server_1.embeddedInfoSrc.dvl_vx = (int16_t)remus100.u; //16:[32,33]
p_150server_1.embeddedInfoSrc.dvl_vy = (int16_t)remus100.v; //17:[34,35]
p_150server_1.embeddedInfoSrc.dvl_vz = (int16_t)remus100.w; //18:[36,37]
p_150server_1.embeddedInfoSrc.rpm = (int16_t)remus100.thrust; //19:[38,39]
p_150server_1.embeddedInfoSrc.lightEnable = (uint8_t)0x01; //20:[40]
p_150server_1.embeddedInfoSrc.battery_voltage = (uint16_t)1024; //21:[41,42]
p_150server_1.embeddedInfoSrc.battery_level = 60; //22:[43]
p_150server_1.embeddedInfoSrc.battery_temp = 21.3 / 0.1; //23:[44,45]
p_150server_1.embeddedInfoSrc.fault_leakSensor = 0x80; //24:[46,47,48,49]
p_150server_1.embeddedInfoSrc.fault_battery = 0x2E; //25:[50]
p_150server_1.embeddedInfoSrc.fault_emergencyBattery = 0xAA; //26:[51]
p_150server_1.embeddedInfoSrc.fault_thrust = 0x76; //27:[52]
p_150server_1.embeddedInfoSrc.iridium = 0xFF; //28:[53]
p_150server_1.embeddedInfoSrc.throwing_load = 0xFF; //29:[54]
p_150server_1.embeddedInfoSrc.dvl_status = 0x00; //30:[55]
p_150server_1.embeddedInfoSrc.crc = 0XFF; //31:[56]
p_150server_1.embeddedInfoSrc.footer = 0XEE1A; //32:[57,58]
}

97
src/pEmulator/Emulator.hpp Normal file
View File

@@ -0,0 +1,97 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 15:57:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-08 17:24:35
* @FilePath: /moos-ivp-pi/src/pEmulator/Emulator.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef __EMULATOR_H
#define __EMULATOR_H
#define UNIX
#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include"MOOS/libMOOS/Comms/MulticastNode.h"
#include <iostream>
#include<vector>
#include "VarDataPair.h"
#include "MBUtils.h"
#include "AngleUtils.h"
#include <list>
#include"MOOS/libMOOS/Comms/XPCUdpSocket.h"
#include "NodeRecord.h"
#include "MOOS/libMOOS/Utils/MOOSThread.h"
#include "_150server.hpp"
using namespace std;
typedef struct uuv
{
double u;
double v;
double w;
double p;
double q;
double r;
double x;
double y;
double z;
double roll;
double pitch;
double yaw;
double thrust;
double rudder;
double elevator;
double lon;
double lat;
double alt;
};
class Emulator : public AppCastingMOOSApp
{
public:
Emulator(){};
~Emulator(){delete receiveBuf;delete sendBuf;};
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void registerVariables();
bool buildReport();
void postNodeRecordUpdate(std::string, const NodeRecord&);
bool receiveUdpDate();
void set150Info();
bool _150Connect();
private:
long int local_port = 8080;
long int matalb_port = 8085;
string matlab_host = "127.0.0.1";
XPCUdpSocket* udp;
unsigned int receiveDateSize = 15;
unsigned int sendDataSize = 6;
unsigned int receiveBufSize;
unsigned int sendBufSize;
char *receiveBuf;
double *sendBuf;
NodeRecord record;
string prefix="NAV";
bool geoOk = false;
//UUV init state
double start_x = 0;
double start_y = 0;
double start_z = 0;
double start_heading = 0;
uuv remus100;
string isConnect = "";
//
CMOOSThread receiveThread;
CMOOSThread _150ServerThread;
CMOOSThread _150ConnectThread;
//150 Server
_150server p_150server_1;
};
#endif

206
src/pEmulator/_150server.cpp Normal file
View File

@@ -0,0 +1,206 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-11-07 14:59:47
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-08 09:19:46
* @FilePath: /moos-ivp-pi/src/pEmulator/_150server.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include "_150server.hpp"
uint16_t _150server::serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr)
{
memcpy(bfr, &(embeddedInfo.header), sizeof(embeddedInfo.header));
bfr += sizeof(embeddedInfo.header);
memcpy(bfr, &(embeddedInfo.count), sizeof(embeddedInfo.count));
bfr += sizeof(embeddedInfo.count);
memcpy(bfr, &(embeddedInfo.size), sizeof(embeddedInfo.size));
bfr += sizeof(embeddedInfo.size);
memcpy(bfr, &(embeddedInfo.drive_mode), sizeof(embeddedInfo.drive_mode));
bfr += sizeof(embeddedInfo.drive_mode);
memcpy(bfr, &(embeddedInfo.height), sizeof(embeddedInfo.height));
bfr += sizeof(embeddedInfo.height);
memcpy(bfr, &(embeddedInfo.depth), sizeof(embeddedInfo.depth));
bfr += sizeof(embeddedInfo.depth);
memcpy(bfr, &(embeddedInfo.yaw), sizeof(embeddedInfo.yaw));
bfr += sizeof(embeddedInfo.yaw);
memcpy(bfr, &(embeddedInfo.pitch), sizeof(embeddedInfo.pitch));
bfr += sizeof(embeddedInfo.pitch);
memcpy(bfr, &(embeddedInfo.roll), sizeof(embeddedInfo.roll));
bfr += sizeof(embeddedInfo.roll);
memcpy(bfr, &(embeddedInfo.ins_vx), sizeof(embeddedInfo.ins_vx));
bfr += sizeof(embeddedInfo.ins_vx);
memcpy(bfr, &(embeddedInfo.ins_vy), sizeof(embeddedInfo.ins_vy));
bfr += sizeof(embeddedInfo.ins_vy);
memcpy(bfr, &(embeddedInfo.ins_vz), sizeof(embeddedInfo.ins_vz));
bfr += sizeof(embeddedInfo.ins_vz);
memcpy(bfr, &(embeddedInfo.lon), sizeof(embeddedInfo.lon));
bfr += sizeof(embeddedInfo.lon);
memcpy(bfr, &(embeddedInfo.lat), sizeof(embeddedInfo.lat));
bfr += sizeof(embeddedInfo.lat);
memcpy(bfr, &(embeddedInfo.alt), sizeof(embeddedInfo.alt));
bfr += sizeof(embeddedInfo.alt);
memcpy(bfr, &(embeddedInfo.dvl_vx), sizeof(embeddedInfo.dvl_vx));
bfr += sizeof(embeddedInfo.dvl_vx);
memcpy(bfr, &(embeddedInfo.dvl_vy), sizeof(embeddedInfo.dvl_vy));
bfr += sizeof(embeddedInfo.dvl_vy);
memcpy(bfr, &(embeddedInfo.dvl_vz), sizeof(embeddedInfo.dvl_vz));
bfr += sizeof(embeddedInfo.dvl_vz);
memcpy(bfr, &(embeddedInfo.rpm), sizeof(embeddedInfo.rpm));
bfr += sizeof(embeddedInfo.rpm);
memcpy(bfr, &(embeddedInfo.lightEnable), sizeof(embeddedInfo.lightEnable));
bfr += sizeof(embeddedInfo.lightEnable);
memcpy(bfr, &(embeddedInfo.battery_voltage), sizeof(embeddedInfo.battery_voltage));
bfr += sizeof(embeddedInfo.battery_voltage);
memcpy(bfr, &(embeddedInfo.battery_level), sizeof(embeddedInfo.battery_level));
bfr += sizeof(embeddedInfo.battery_level);
memcpy(bfr, &(embeddedInfo.battery_temp), sizeof(embeddedInfo.battery_temp));
bfr += sizeof(embeddedInfo.battery_temp);
memcpy(bfr, &(embeddedInfo.fault_leakSensor), sizeof(embeddedInfo.fault_leakSensor));
bfr += sizeof(embeddedInfo.fault_leakSensor);
memcpy(bfr, &(embeddedInfo.fault_battery), sizeof(embeddedInfo.fault_battery));
bfr += sizeof(embeddedInfo.fault_battery);
memcpy(bfr, &(embeddedInfo.fault_emergencyBattery), sizeof(embeddedInfo.fault_emergencyBattery));
bfr += sizeof(embeddedInfo.fault_emergencyBattery);
memcpy(bfr, &(embeddedInfo.fault_thrust), sizeof(embeddedInfo.fault_thrust));
bfr += sizeof(embeddedInfo.fault_thrust);
memcpy(bfr, &(embeddedInfo.iridium), sizeof(embeddedInfo.iridium));
bfr += sizeof(embeddedInfo.iridium);
memcpy(bfr, &(embeddedInfo.throwing_load), sizeof(embeddedInfo.throwing_load));
bfr += sizeof(embeddedInfo.throwing_load);
memcpy(bfr, &(embeddedInfo.dvl_status), sizeof(embeddedInfo.dvl_status));
bfr += sizeof(embeddedInfo.dvl_status);
memcpy(bfr, &(embeddedInfo.crc), sizeof(embeddedInfo.crc));
bfr += sizeof(embeddedInfo.crc);
memcpy(bfr, &(embeddedInfo.footer), sizeof(embeddedInfo.footer));
bfr += sizeof(embeddedInfo.footer);
}
void _150server::postInfo()
{
bzero(embeddedBuffer, 0);
serializeFields(embeddedInfoSrc, embeddedBuffer);
write(cfd, embeddedBuffer, 59);
};
bool _150server::listenInfo()
{
while (1)
{
int lens = read(cfd, recvbuf, sizeof(recvbuf));
if(lens>0)
{
_150_recive++;
uint16_t* buf16;
printf("header: %u, %u\n", recvbuf[0], recvbuf[1]);
printf("count: %u\n", recvbuf[2], recvbuf[3]);
printf("size: %u\n", recvbuf[4]);
printf("drive_mode: %u\n", recvbuf[5]);
printf("thrust: %u\n", recvbuf[6]);
_150cmd.thrust = (double)recvbuf[6];
printf("yaw: %u, %u\n", recvbuf[7], recvbuf[8]);
memcpy(buf16, &recvbuf[7], 2*sizeof(recvbuf[7]));
_150cmd.yaw = *((double*)buf16);
printf("depth: %u, %u\n", recvbuf[9], recvbuf[10]);
memcpy(buf16, &recvbuf[9], 2*sizeof(recvbuf[9]));
_150cmd.depth = *((double*)buf16);
printf("helm_top_angle: %u\n", recvbuf[11]);
_150cmd.helm_top_angle = (double)recvbuf[11];
printf("helm_bottom_angle: %u\n", recvbuf[12]);
_150cmd.helm_bottom_angle = (double)recvbuf[12];
_150cmd.helm_left_angle = (double)recvbuf[13];
printf("helm_left_angle: %u\n", recvbuf[13]);
_150cmd.helm_right_angle = (double)recvbuf[14];
printf("helm_right_angle: %u\n", recvbuf[14]);
_150cmd.light_enable = (double)recvbuf[15];
printf("light_enable: %u\n", recvbuf[15]);
printf("dvl_enable: %u\n", recvbuf[16]);
_150cmd.dvl_enable = (double)recvbuf[16];
printf("throwing_load_enable: %u\n", recvbuf[17]);
_150cmd.throwing_load_enable = (double)recvbuf[16];
printf("crc: %u\n", recvbuf[18]);
_150cmd.crc = (double)recvbuf[18];
printf("footer: %u, %u\n", recvbuf[19], recvbuf[20]);
memcpy(buf16, &recvbuf[19], 2*sizeof(recvbuf[19]));
_150cmd.footer = *((double*)buf16);
}
}
return true;
}
void _150server::_150_startServer()
{
std::cout << "--------------------" << std::endl;
lfd = socket(AF_INET, SOCK_STREAM, 0);
if(lfd==-1)
{
perror("socket");
exit(-1);
}
//2.绑定
struct sockaddr_in saddr;
saddr.sin_family = PF_INET;
saddr.sin_addr.s_addr = INADDR_ANY; //0.0.0.0
saddr.sin_port = htons(8001);
int ret = bind(lfd, (struct sockaddr *)&saddr, sizeof(saddr));
if(ret == -1)
{
perror("bind");
exit(-1);
}
//3.监听
listen(lfd, 5);
if(ret==-1)
{
perror("listen");
exit(-1);
}
//4.接受客户端连接
struct sockaddr_in caddr;
socklen_t len = sizeof(caddr);
cfd = accept(lfd, (struct sockaddr *)&caddr, &len);
if(cfd==-1)
{
perror("accept");
exit(-1);
}
std::cout << "--------------------" << std::endl;
//输出客户端的信息
char cip[16];
inet_ntop(AF_INET, &caddr.sin_addr.s_addr, cip, sizeof(cip));
unsigned short cport = ntohs(caddr.sin_port);
printf("client ip is %s,port is %d\n", cip, cport);
}

95
src/pEmulator/_150server.hpp Normal file
View File

@@ -0,0 +1,95 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-11-07 14:59:36
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-07 17:12:30
* @FilePath: /moos-ivp-pi/src/pEmulator/_150server.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef __150SERVER_H
#define __150SERVER_H
#include <stdio.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <sstream>
#include <iostream>
struct AUVEmbedded
{
uint16_t header; //1:[0,1] hx
uint16_t count; //2:[2,3] hu
uint8_t size; //3:[4] u
uint8_t drive_mode; //4:[5] u
uint16_t height; //5:[6,7] hu
uint16_t depth; //6:[8,9] hu
uint16_t yaw; //7:[10,11] hu
int16_t pitch; //8:[12,13] hd
int16_t roll; //9:[14,15] hd
int16_t ins_vx; //10:[16,17] hd
int16_t ins_vy; //11:[18,19] hd
int16_t ins_vz; //12:[20,21] hd
int32_t lon; //13:[22,23,24,25] d
int32_t lat; //14:[26,27,28,29] d
int16_t alt; //15:[30,31] hd
int16_t dvl_vx; //16:[32,33] hd
int16_t dvl_vy; //17:[34,35] hd
int16_t dvl_vz; //18:[36,37] hd
int16_t rpm; //19:[38,39] hd
uint8_t lightEnable; //20:[40] u
uint16_t battery_voltage; //21:[41,42] hu
uint8_t battery_level; //22:[43] u
uint16_t battery_temp; //23:[44,45] hu
uint32_t fault_leakSensor; //24:[46,47,48,49] u
uint8_t fault_battery; //25:[50] u
uint8_t fault_emergencyBattery; //26:[51] u
uint8_t fault_thrust; //27:[52] u
uint8_t iridium; //28:[53] u
uint8_t throwing_load; //29:[54] u
uint8_t dvl_status; //30:[55] u
uint8_t crc; //31:[56] u
uint16_t footer; //32:[57,58] hu
};
struct AUVCmd
{
uint16_t header = 0xEBA2;
uint16_t count;
uint8_t size;
uint8_t mode = 0XFF;
uint8_t thrust;
uint16_t yaw;
uint16_t depth;
uint8_t helm_top_angle;
uint8_t helm_bottom_angle;
uint8_t helm_left_angle;
uint8_t helm_right_angle;
uint8_t light_enable;
uint8_t dvl_enable = 0XFF;
uint8_t throwing_load_enable = 0XFF;
uint8_t crc;
uint16_t footer = 0XEE2A;
};
class _150server
{
private:
/* data */
int lfd;
int cfd;
public:
_150server(/* args */){};
~_150server(){};
uint16_t serializeFields(AUVEmbedded &embeddedInfo, uint8_t* bfr);
bool listenInfo();
void postInfo();
void _150_startServer();
AUVEmbedded embeddedInfoSrc;
AUVCmd _150cmd;
unsigned char embeddedBuffer[59];
unsigned char recvbuf[65535] = {0};
};
#endif

249
src/pEmulator/a.moos Normal file
View File

@@ -0,0 +1,249 @@
//-------------------------------------------------
// NAME: M. Benjamin, MIT CSAIL
// FILE: alpha.moos
//-------------------------------------------------
ServerHost = localhost
ServerPort = 9000
Community = alpha
MOOSTimeWarp = 1
// Forest Lake
LatOrigin = 43.825300
LongOrigin = -70.330400
// MIT Sailing Pavilion (use this one)
// LatOrigin = 42.358456
// LongOrigin = -71.087589
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pEmulator @ NewConsole = true
//Run = pLogger @ NewConsole = false
Run = pMotionControler @ NewConsole = false
Run = pTaskManger @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
}
ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
}
ProcessConfig = pEmulator
{
AppTick = 5
CommsTick = 5
matlab_host = 192.168.0.11
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 10
start_y = 9
start_z = 1
start_heading = 30
}
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
//Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
//Log = REPORT @ 0 NOSYNC
//Log = BHV_SETTINGS @ 0 NOSYNC
Log = OPREGION_RESET @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
//------------------------------------------
// uProcessWatch config block
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uQueryDB*
nowatch = uXMS*
nowatch = uMAC*
}
//------------------------------------------
// uSimMarineV22 config block
//------------------------------------------
// pHelmIvP config block
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMotionControler
{
AppTick = 10
CommsTick = 10
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 10
yaw_pid_kd = 0.0
yaw_pid_ki = 0.01
yaw_pid_integral_limit = 10
// Speed PID controller
speed_pid_kp = 20.0
speed_pid_kd = 0.0
speed_pid_ki = 1
speed_pid_integral_limit = 100
maxpitch = 15
maxelevator = 30
pitch_pid_kp = 1.5
pitch_pid_kd = 0
pitch_pid_ki = 1.0
pitch_pid_integral_limit = 0
z_to_pitch_pid_kp = 0.12
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.004
z_to_pitch_pid_integral_limit = 0.05
//MAXIMUMS MAXRUDDER
maxrudder = 30
maxthrust = 1525
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
}
//------------------------------------------
// pMarineViewer config block
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
vehicles_name_mode = names+depth //+shortmode
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = xlarge
procs_font_size = xlarge
appcast_font_size = large
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
//button_one = START # START=true
//button_one = MOOS_MANUAL_OVERRIDE=false
button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
button_two = STOP # uMission_action_cmd={"taskName":"east_waypt_survey","action":"stop"}
//button_two = MOOS_MANUAL_OVERRIDE=true
button_three = FaultClear # ClearFalut = true
button_four = SendSecurityZone # SendSaftRules = true
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
//platform_type = kayak
//更改显示形状为uuv
platform_type = UUV
platform_color = red
platform_length = 4
}

181
src/pEmulator/alpha.bhv Normal file
View File

@@ -0,0 +1,181 @@
//-------- FILE: alpha.bhv -------------
initialize RUN = false
initialize TaskNum=t1
initialize SendTask=false
//--------------模式判断------------------------
set MODE = ACTIVE{
RUN=true
} INACTIVE
set MODE = T1{
MODE=ACTIVE
TaskNum = t1
}
//----------路径点任务----------------------------
Behavior = BHV_Waypoint
{
name = waypt_survey
pwt = 100 //优先权重
condition = MODE==T1
//endflag = START=false
endflag = END_WayPoint=true
configflag = CRUISE_SPD = $[SPEED]
//configflag = OSPOS = $[OSX],$[OSY]
activeflag = INFO=$[OWNSHIP]
activeflag = INFO=$[BHVNAME]
activeflag = INFO=$[BHVTYPE]
//cycleflag = CINFO=$[OSX],$[OSY]
wptflag = CurrentPointComplete=true
wptflag = PREV=$(PX),$(PY)
wptflag = NEXT=$(NX),$(NY)
wptflag = TEST=$(X),$(Y)
wptflag = OSPOS=$(OSX),$(OSY)
//wptflag_on_start = true
updates = WPT_UPDATE
//perpetual = true
templating = spawn
// speed_alt = 1.2
//use_alt_speed = true
lead = 8
lead_damper = 1
lead_to_start = false
speed = 1 // meters per second
capture_line = true
capture_radius = 5.0
slip_radius = 15.0
efficiency_measure = all
polygon = 60,-40
order = normal
//repeat = 3
visual_hints = nextpt_color=yellow
visual_hints = nextpt_vertex_size=8
visual_hints = nextpt_lcolor=gray70
visual_hints = vertex_color=dodger_blue, edge_color=white
visual_hints = vertex_size=5, edge_size=1
}
//--------------定深任务------------------
Behavior=BHV_ConstantDepth
{
name = const_depth
pwt = 100
//condition = DEPLOY = true
condition = MODE==T1
duration = no-time-limit
updates = DEPTH_UPDATE
depth = 0
}
//--------------定向任务--------------------
Behavior=BHV_ConstantHeading
{
name = const_heading
pwt = 100
//condition = START_TURN = true
//condition = DEPLOY = true
condition = MODE==T3
perpetual = true
activeflag = TURN = started
//endflag = TURN = done
//endflag = RETURN = true
//endflag = START_TURN = false
endflag = START=false
heading = 225
complete_thresh = 5
duration = no-time-limit
}
//--------------定速任务--------------------
Behavior=BHV_ConstantSpeed
{
name = const_speed
pwt = 1000
condition = MODE==T1
perpetual = true
updates = SPEED_UPDATE
//endflag = START=false
speed = 5
duration = no-time-limit
//peakwidth = 0.5
//basewidth = 0.5
}
//----------------安全模式-----------------------
//----------------计时器---------------------
Behavior = BHV_Timer
{
name = mtime
condition = MODE==T1
pwt = 100
templating = spawn
//duration_status = MSTATUS
//duration = 10
endflag = TIME_OUT=true
updates = TIMER_UPDATES
//perpetual = true
}
//-------------最大深度限制--------------------------
Behavior = BHV_MaxDepth
{
name = maxdepth
pwt = 200
condition = MODE==ACTIVE
updates = MAXDEEP_UPDATES
max_depth = 20
tolerance = 0
duration = no-time-limit
}
//--------------安全区域设置-----------------------
Behavior = BHV_OpRegion
{
// General Behavior Parameters
// ---------------------------
name = op_region // example
pwt = 300 // default
condition = MODE==TN
updates = OPREGION_UPDATES // example
// Parameters specific to this behavior
// ------------------------------------
max_time = 20 // default (seconds)
max_depth = 25 // default (meters)
min_altitude = 0 // default (meters)
reset_var = OPREGION_RESET // example
trigger_entry_time = 1 // default (seconds)
trigger_exit_time = 0.5 // default (seconds)
polygon = pts={-80,-00:-30,-175:150,-100:95,25}
breached_altitude_flag = TaskFault = AltitudeOut
breached_depth_flag = TaskFault = DepthOut
breached_poly_flag = TaskFault = RegionOut
breached_time_flag = TaskFault = TimeOut
visual_hints = vertex_color = brown // default
visual_hints = vertex_size = 3 // default
visual_hints = edge_color = aqua // default
visual_hints = edge_size = 1 // default
}

63
src/pEmulator/main.cpp Normal file
View File

@@ -0,0 +1,63 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:06
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-10-20 16:52:52
* @FilePath: /moos-ivp-extend/src/pEmulator/main.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include<iostream>
using namespace std;
#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
// #include "VarDataPair.h"
#include"Emulator.hpp"
#include <iostream>
#include<string>
#include "MBUtils.h"
#include "ColorParse.h"
// #include "MBUtils.h"
// #include "ColorParse.h"
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();
cout << " Version : 0.01" << endl;
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
// showExampleConfigAndExit();
cout << " example : NULL" << endl;
else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
// showHelpAndExit();
cout << " Low leave Control for UUV " << endl;
else if((argi=="-i") || (argi == "--interface"))
// showInterfaceAndExit();
cout << " UUV Motion Control " << endl;
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 << "Emulator launching as " << run_command << endl;
cout << termColor() << endl;
Emulator s;
s.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
return(0);
return(0);
}

15
src/pEmulator/pEmulator.moos Normal file
View File

@@ -0,0 +1,15 @@
ProcessConfig = pEmulator
{
AppTick = 5
CommsTick = 5
//matlab_host = 192.168.0.11
matlab_host = 10.25.0.206
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 0
start_y = 0
start_z = 0
start_heading = 30
}

22
src/pMotionControler/CMakeLists.txt Normal file
View File

@@ -0,0 +1,22 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pTaskManger
# Author(s): zjk
#--------------------------------------------------------
FILE(GLOB SRC *.cpp *.hpp)
include_directories(/usr/include/jsoncpp/)
link_directories(/usr/local/lib/)
ADD_EXECUTABLE(pMotionControler ${SRC})
#需要把被依赖的库放到依赖库的后面
TARGET_LINK_LIBRARIES(pMotionControler
${MOOS_LIBRARIES}
geometry
mbutil
m
pthread
jsoncpp
)

43
src/pMotionControler/ControlParam.json Normal file
View File

@@ -0,0 +1,43 @@
{
"speed" :
{
"Kp" : 10.0,
"Ki" : 5.0,
"Kd" : 0.0,
"LimitDelta" : 50,
"MaxOut" : 100,
"MinOut" : 0
},
"heading" :
{
"Kp" : 0.8,
"Ki" : 0.05,
"Kd" : 2.2,
"LimitDelta" : 5,
"MaxOut" : 30,
"MinOut" : -30
},
"depth" :
{
"Kp" : 10.0,
"Ki" : 0.3,
"Kd" : 2.0,
"LimitDelta" : 5,
"MaxOut" : 10,
"MinOut" : -10
},
"pitch" :
{
"Kp" : 0.6,
"Ki" : 0.03,
"Kd" : 1.5,
"LimitDelta" : 5,
"MaxOut" : 30,
"MinOut" : -30
},
"const_thrust" : 0,
"dead_zone" : 10,
"speedCol" : true,
"depthCol" : true,
"HeadingCol" : true
}

424
src/pMotionControler/Controler.cpp Normal file
View File

@@ -0,0 +1,424 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-16 14:04:53
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-02 15:17:38
* @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include"Controler.hpp"
Controler::Controler()
{
// Error_capacity = 10;
initVariable();
}
void Controler::initVariable()
{
ClearValTim(desired_speed);
ClearValTim(desired_heading);
ClearValTim(desired_depth);
ClearValTim(desired_pitch);
ClearValTim(current_speed);
ClearValTim(current_heading);
ClearValTim(current_pitch);
ClearValTim(current_depth);
current_time = 0;
start_time = 0;
ClearValTim(desired_thrust);
ClearValTim(desired_rudder);
ClearValTim(desired_elevator);
iterations = 0;
start_time = 0.0;
tardy_helm_thresh = 2.0;
tardy_nav_thresh = 2.0;
// current_error = 0;
// last_error = 0;
// Error.clear();
}
bool Controler::setDesSpeed(double spd, double tim)
{
desired_speed.value = spd;
desired_speed.time = tim;
return true;
}
bool Controler::setDesDepth(double dph, double tim)
{
desired_depth.value = dph;
desired_depth.time = tim;
return true;
}
bool Controler::setDesPitch(double pth, double tim)
{
desired_pitch.value = pth;
desired_pitch.time = tim;
return true;
}
bool Controler::setDesHeading(double hdg, double tim)
{
if(is_rad)
hdg = radToDegrees(hdg);
desired_heading.value = angle180(hdg);
desired_heading.time = tim;
return true;
}
bool Controler::setCurSpeed(double spd, double tim)
{
current_speed.value = spd;
current_speed.time = tim;
return true;
}
bool Controler::setCurDepth(double dph, double tim)
{
current_depth.value = dph;
current_depth.time = tim;
return true;
}
bool Controler::setCurPitch(double pth, double tim)
{
current_pitch.value = pth;
current_pitch.time = tim;
return true;
}
bool Controler::setCurHeading(double hdg, double tim)
{
current_heading.value = hdg;
current_heading.time = tim;
return true;
}
bool Controler::setDesiredValues()
{
iterations++;
desired_thrust.value = 0;
desired_rudder.value = 0;
desired_elevator.value = 0;
//=============================================================
// Part 1: Ensure all nav and helm messages have been received
// for first time. If not, we simply wait, without declaring an
// override. After all messages have been received at least
// once, staleness is checked below.
//=============================================================
runMsg.push_back("desired_heading-time:" + doubleToString(desired_heading.time));
runMsg.push_back("desired_speed-time:" + doubleToString(desired_speed.time));
runMsg.push_back("current_heading-time:" + doubleToString(current_heading.time));
runMsg.push_back("current_speed-time:" + doubleToString(current_speed.time));
runMsg.push_back("desired_depth-time:" + doubleToString(desired_depth.time));
runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
runMsg.push_back("current_pitch-time:" + doubleToString(current_pitch.time));
// runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
if(has_speedCtrl)
{
if((desired_heading.time == 0) || (desired_speed.time == 0))
return true;
}
if(has_headCtrl)
{
if((current_heading.time == 0) || (current_speed.time == 0))
return true;
}
if(has_depthCtrl)
{
if(desired_depth.time == 0)
return true;
if((current_depth.time == 0) || (current_pitch.time == 0))
return true;
}
//=============================================================
// Part 2: Critical info staleness (if not in simulation mode)
//=============================================================
//TODO: 验证信息过时操作
if(cheakStalensee)
{
bool is_stale = checkForStaleness();
if(is_stale)
{
has_override = true;
return false;
}
}
//=============================================================
// Part 3: Set the actuator values. Note: If desired thrust is
// zero others desired values will automatically be zero too.
//=============================================================
// m_desired_thrust = setDesiredThrust();
// if(m_desired_thrust > 0)
// m_desired_rudder = setDesiredRudder();
// if(m_desired_thrust > 0 && m_depth_control)
// m_desired_elevator = setDesiredElevator();
return true;
}
//TODO： 添加操控权判断
bool Controler::overrived(string sval)
{
if(tolower(sval) == "true")
{
if(has_override == false)
addPosting("HAS_CONTROL", "false");
has_override = true;
}
else if(tolower(sval) == "false")
{
// Upon lifting an override, the timestamps are reset. New values
// for all will need to be received before desired_* outputs will
// be produced. If we do not reset, it's possible we may
// interpret older pubs as being stale but they may have been
// paused also during an override.
if(has_override == true)
{
desired_speed.time = 0;
desired_heading.time = 0;
desired_depth.time = 0;
desired_pitch.time = 0;
current_speed.time = 0;
current_heading.time = 0;
current_pitch.time = 0;
current_depth.time = 0;
}
has_override = false;
addPosting("HAS_CONTROL", "true");
}
return true;
}
bool Controler::checkForStaleness()
{
bool is_stale = false;
// =========================================================
// Part 1: Check for Helm staleness
// =========================================================
double hdg_delta = (current_time - desired_heading.time);
if(hdg_delta > tardy_helm_thresh) {
string staleness = doubleToStringX(hdg_delta,3);
addPosting("PID_STALE", "Stale DesHdg:" + staleness);
is_stale = true;
}
double spd_delta = (current_time - desired_speed.time);
if(spd_delta > tardy_helm_thresh) {
string staleness = doubleToStringX(spd_delta,3);
addPosting("PID_STALE", "Stale DesSpd:" + staleness);
is_stale = true;
}
// =========================================================
// Part 2B: Check for Nav staleness
// =========================================================
double nav_hdg_delta = (current_time - current_heading.time);
if(nav_hdg_delta > tardy_nav_thresh) {
string staleness = doubleToStringX(nav_hdg_delta,3);
addPosting("PID_STALE", "Stale NavHdg:" + staleness);
is_stale = true;
}
double nav_spd_delta = (current_time - current_speed.time);
if(nav_spd_delta > tardy_nav_thresh) {
string staleness = doubleToStringX(nav_spd_delta,3);
addPosting("PID_STALE", "Stale NavSpd:" + staleness);
is_stale = true;
}
// =========================================================
// Part 2C: If depth control, check for Helm Depth staleness
// =========================================================
if(has_depthCtrl) {
double dep_delta = (current_time - desired_depth.time);
if(dep_delta > tardy_helm_thresh) {
string staleness = doubleToStringX(dep_delta,3);
addPosting("PID_STALE", "Stale DesDepth:" + staleness);
is_stale = true;
}
}
// =========================================================
// Part 2D: If depth control, check for Nav Depth staleness
// =========================================================
if(has_depthCtrl) {
double nav_dep_delta = (current_time - current_depth.time);
if(nav_dep_delta > tardy_nav_thresh) {
string staleness = doubleToStringX(nav_dep_delta,3);
addPosting("PID_STALE", "Stale NavDep:" + staleness);
is_stale = true;
}
double nav_pit_delta = (current_time - current_pitch.time);
if(nav_pit_delta > tardy_nav_thresh) {
string staleness = doubleToStringX(nav_pit_delta,3);
addPosting("PID_STALE", "Stale NavPitch:" + staleness);
is_stale = true;
}
}
return(is_stale);
}
// bool Controler::setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator)
// {
// limit_thrust[0] = min_thrush;
// limit_thrust[1] = max_thrush;
// limit_rudder[0] = min_rubber;
// limit_rudder[1] = max_rudder;
// limit_elevator[0] = min_elevator;
// limit_elevator[1] = max_elevator;
// }
bool Controler::Limit(double &a, double max, double min)
{
a = (a < min) ? min : a;
a = (a > max) ? max : a;
return true;
}
bool Controler::setCtrl(bool speed, bool heading, bool depth)
{
has_speedCtrl = speed;
has_headCtrl = heading;
has_depthCtrl = depth;
return true;
}
void Controler::addPosting(std::string var, std::string sval)
{
VarDataPair pair(var, sval);
postings.push_back(pair);
}
void Controler::addPosting(std::string var, double val)
{
VarDataPair pair(var, val);
postings.push_back(pair);
}
double Controler::getFrequency() const
{
double elapsed = current_time - start_time;
double frequency = 0;
if(elapsed > 0)
frequency = ((double)(iterations)) / elapsed;
return(frequency);
}
bool Controler::ClearValTim(vlaTim &a)
{
if(&a == NULL)
return false;
else
{
a.value = 0;
a.time = 0;
}
}
//TODO: 控制器参数配置函数
list<std::string> Controler::setConfigParams(std::list<std::string>)
{
list<string> unhandled_params;
list<string>::iterator p;
// for(p=param_lines.begin(); p!=param_lines.end(); p++) {
// string orig = *p;
// string line = tolower(orig);
// string param = biteStringX(line, '=');
// if(param == "speed_factor")
// m_config_params.push_front(orig);
// else if(param == "simulation")
// m_config_params.push_front(orig);
// else if(param == "sim_instability")
// m_config_params.push_front(orig);
// else if(param == "tardy_helm_threshold")
// m_config_params.push_front(orig);
// else if(param == "tardy_nav_threshold")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_kp")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_kd")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_ki")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_integral_limit")
// m_config_params.push_front(orig);
// else if(param == "yaw_pid_ki_limit")
// m_config_params.push_front(orig);
// else if(param == "maxrudder")
// m_config_params.push_front(orig);
// else if(param == "heading_debug")
// m_config_params.push_front(orig);
// else if(param == "speed_pid_kp")
// m_config_params.push_front(orig);
// else if(param == "speed_pid_kd")
// m_config_params.push_front(orig);
// else if(param == "speed_pid_ki")
// m_config_params.push_front(orig);
// else if(param == "speed_pid_integral_limit")
// m_config_params.push_front(orig);
// else if(param == "maxthrust")
// m_config_params.push_front(orig);
// else if(param == "speed_debug")
// m_config_params.push_front(orig);
// else if(param == "depth_control")
// m_config_params.push_front(orig);
// else if(param == "z_to_pitch_pid_kp")
// m_config_params.push_front(orig);
// else if(param == "z_to_pitch_pid_kd")
// m_config_params.push_front(orig);
// else if(param == "z_to_pitch_pid_ki")
// m_config_params.push_front(orig);
// else if(param == "z_to_pitch_pid_integral_limit")
// m_config_params.push_front(orig);
// else if(param == "maxpitch")
// m_config_params.push_front(orig);
// else if(param == "depth_debug")
// m_config_params.push_front(orig);
// else if(param == "pitch_pid_kp")
// m_config_params.push_front(orig);
// else if(param == "pitch_pid_kd")
// m_config_params.push_front(orig);
// else if(param == "pitch_pid_ki")
// m_config_params.push_front(orig);
// else if(param == "pitch_pid_integral_limit")
// m_config_params.push_front(orig);
// else if(param == "maxelevator")
// m_config_params.push_front(orig);
// else
// unhandled_params.push_front(orig);
// }
return(unhandled_params);
}
Json::Value Controler::getConfig(string path)
{
ifstream ifs;
ifs.open(path, ios::in);
Json::Reader taskConfigureReader;
Json::Value inputJsonValue;
taskConfigureReader.parse(ifs, inputJsonValue);
ifs.close();
return inputJsonValue;
}
void Controler::setCheakStalensee(string s)
{
if(s == "true")
cheakStalensee = true;
else
cheakStalensee = false;
}

155
src/pMotionControler/Controler.hpp Normal file
View File

@@ -0,0 +1,155 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-16 14:05:16
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-02 14:31:16
* @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef __COBTROLLER_H
#define __CONTROLLER_H
#include<iostream>
#include<vector>
#include "VarDataPair.h"
#include "MBUtils.h"
#include "AngleUtils.h"
#include <list>
#include "json/json.h"
#include <fstream>
#include <map>
// #include "VarDataPair.h"
using namespace std;
typedef struct
{
double value;
double time;
} vlaTim;
class Controler
{
public:
Controler();
~Controler(){};
virtual int step(){return 0;}
virtual bool setConfigParams(){return false;}
virtual bool overrived(string sval);
virtual bool handleYawSettings(){return false;}
virtual bool handleSpeedSettings(){return false;};
virtual bool handleDepthSettings(){return false;};
virtual bool hasConfigSettings() const{return false;};
// bool setError(double err);
bool setDesSpeed(double spd, double tim);
bool setDesDepth(double dph, double tim);
bool setDesPitch(double pth, double tim);
bool setDesHeading(double hdg, double tim);
bool setCurSpeed(double spd, double tim);
bool setCurDepth(double dph, double tim);
bool setCurPitch(double pth, double tim);
bool setCurHeading(double hdg, double tim);
// bool setCurTime(double tim){current_time = tim;}
void updateTime(double tim){current_time = tim;}
double getCurTime(){return current_time;}
void setStartTime(double tim){start_time = tim;}
void setOverride(bool v){has_override = v;}
void setTardyHelm(double t){ tardy_helm_thresh = t;}
void setTardyNav(double t){ tardy_nav_thresh = t;}
void setCheakStalensee(string s);
void setConstThrust(double v){const_thrust = v;}
void setDeadZone(double v){dead_zone=v;}
void setDepthControl(bool v){has_depthCtrl=v;}
void setSpeedControl(bool v){has_speedCtrl=v;}
void setHeadingControl(bool v){has_headCtrl=v;}
bool setDesiredValues();
// bool setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator);
bool setCtrl(bool speed, bool heading, bool depth);
bool Limit(double &a, double max, double min);
bool checkForStaleness();
void addPosting(std::string var, std::string sval);
void addPosting(std::string var, double val);
vector<VarDataPair> getPostings() {return(postings);}
void clearPostings() {postings.clear();}
bool ClearValTim(vlaTim &a);
list<std::string> setConfigParams(std::list<std::string>); //使用MOOS风格配置参数函数
Json::Value getConfig(string path);
double getFrequency() const;
bool hasControl(){return(!has_override);}
bool hasSpdCtrl(){return has_speedCtrl;}
bool hasDphCtrl(){return has_depthCtrl;}
bool hasHdgCtrl(){return has_headCtrl;}
double getDesiredRudder() const {return(desired_rudder.value);}
double getDesiredThrust() const {return(desired_thrust.value);}
double getDesiredElevator() const {return(desired_elevator.value);}
// bool setErrorCap(int c){Error_capacity = c;}
inline void initVariable();
list<string> getConfigParams(){return config_params;}
vector<string> getConfigErrors(){return config_errors;}
vector<string> getConfigInfo(){return config_info;}
vector<string> getRunMsg(){return runMsg;}
Json::Value getReport(){return RepList;}
void ClearRunMsg(){runMsg.clear();}
protected:
vlaTim desired_speed;
vlaTim desired_heading;
vlaTim desired_depth;
vlaTim desired_pitch;
vlaTim current_speed;
vlaTim current_heading;
vlaTim current_pitch;
vlaTim current_depth;
double current_time;
double start_time;
vlaTim desired_thrust;
vlaTim desired_rudder;
vlaTim desired_elevator;
double limit_thrust[2];
double limit_rudder[2];
double limit_elevator[2];
double max_rudder;
double max_thrust;
double max_pitch;
double max_elevator;
bool has_speedCtrl = true;
bool has_headCtrl = true;
bool has_depthCtrl = true;
bool has_override = true;
bool cheakStalensee = true;
vector<VarDataPair> postings;
unsigned int iterations;
double tardy_helm_thresh;
double tardy_nav_thresh;
double const_thrust;
double dead_zone;
double is_rad = false;
list<string> config_params;
vector<string> config_errors;
vector<string> config_info;
vector<string> runMsg;
Json::Value RepList;
};
#endif

242
src/pMotionControler/MotionControler.cpp Normal file
View File

@@ -0,0 +1,242 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-07 12:01:57
* @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
// #define DEBUG
#include"MotionControler.hpp"
//TODO：增加启用哪个控制器功能
bool MotionControler::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
string key = msg.m_sKey;
string sval = msg.m_sVal;
double dval = msg.m_dfVal;
double dfT;
msg.IsSkewed(m_curr_time, &dfT);
if(fabs(dfT) < ok_skew)
{
if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
pengine.overrived(sval);
else if(key == "DESIRED_HEADING")
pengine.setDesHeading(dval, MOOSTime());
else if(key == "DESIRED_SPEED")
pengine.setDesSpeed(dval, MOOSTime());
else if(key == "DESIRED_DEPTH")
pengine.setDesDepth(dval, MOOSTime());
else if(key == "NAV_HEADING")
pengine.setCurHeading(angle360(dval), MOOSTime());
else if(key == "NAV_SPEED")
pengine.setCurSpeed(dval, MOOSTime());
else if(key == "NAV_DEPTH")
pengine.setCurDepth(dval, MOOSTime());
else if(key == "NAV_PITCH")
pengine.setCurPitch(dval, MOOSTime());
else if(key == MSG_ReadConfig) //重新读取配置参数可以清除故障码
{
int e = pengine.setParam(configFilePath);
pengine.setOverride(true);
if(e != 0)
faultCode = 10 + e;
else
faultCode = 0;
}
}
}
return true;
}
bool MotionControler::Iterate()
{
bool a;
AppCastingMOOSApp::Iterate();
pengine.updateTime(m_curr_time);
int i = pengine.step();
switch (i)
{
case 0:
RepList["State : "] = "Run";
break;
case 1:
RepList["State : "] = "Ready";
break;
case -1:
RepList["State : "] = "Fault";
faultCode = 1;//信息超时
break;
default:
break;
}
postPengineResults();
postPenginePostings();
postCharStatus();
Notify(MSG_FALUT,faultCode);
AppCastingMOOSApp::PostReport();
return true;
}
bool MotionControler::OnConnectToServer()
{
registerVariables();
return true;
}
bool MotionControler::OnStartUp()
{
AppCastingMOOSApp::OnStartUp();
pengine.setStartTime(MOOSTime());
STRING_LIST sParams;
m_MissionReader.GetConfiguration(GetAppName(), sParams);
//pengine.setConfigParams(sParams);
//bool handled = true;
STRING_LIST::iterator p;
for(p=sParams.begin(); p!=sParams.end(); p++) {
string orig = *p;
string line = (orig); //识别大小写
string param = biteStringX(line, '=');
string value = line;
double dval = atof(value.c_str());
if(param == "config_file")
configFilePath = value;
else if(param == "tardy_helm_thresh")
pengine.setTardyHelm(dval);
else if(param == "tardy_nav_thresh")
pengine.setTardyNav(dval);
else if(param == "cheak_stalensee")
pengine.setCheakStalensee(value);
else if(param == "AppTick")
setFrequency = dval;
else if(param == "delta_freqency")
frequency_delta = dval;
else
reportUnhandledConfigWarning(orig);
}
int e = pengine.setParam(configFilePath);
if(e != 0)
{
faultCode = 10 + e;
return false;
}
return true;
}
void MotionControler::registerVariables()
{
AppCastingMOOSApp::RegisterVariables();
Register("NAV_HEADING", 0);
Register("NAV_SPEED", 0);
Register("NAV_DEPTH", 0);
Register("NAV_PITCH", 0);
Register("DESIRED_HEADING", 0);
Register("DESIRED_SPEED", 0);
Register("DESIRED_DEPTH", 0);
Register("PID_VERBOSE", 0);
Register("SPEED_FACTOR", 0);
Register("MOOS_MANUAL_OVERIDE", 0);
Register("MOOS_MANUAL_OVERRIDE", 0);
Register(MSG_ReadConfig,0);
}
bool MotionControler::buildReport()
{
double frequency = pengine.getFrequency();
double delta_freq = 100.0*(setFrequency - frequency) / setFrequency;
if(abs(delta_freq) > frequency_delta)
faultCode = 2;
m_msgs << "Frequency Delta : " << frequency << endl;
m_msgs << "PID has_control : " << boolToString(pengine.hasControl()) << endl;
m_msgs << "Config File Path : " << configFilePath << endl;
m_msgs << "S : H : D : | " << intToString(pengine.hasSpdCtrl())+ " | "
<< intToString(pengine.hasHdgCtrl())+" | "
<< intToString(pengine.hasDphCtrl())+" |" << endl;
RepList["to BS"] = colVar;
RepList["PID"] = pengine.getReport()["W"];
string rep = Json::writeString(RepJsBuilder, RepList);
m_msgs << rep << endl;
return(true);
}
void MotionControler::postPenginePostings()
{
vector<VarDataPair> m_postings = pengine.getPostings();
for(unsigned int i=0; i<m_postings.size(); i++)
{
VarDataPair pair = m_postings[i];
string var = pair.get_var();
if(pair.is_string() && pair.get_sdata_set())
Notify(var, pair.get_sdata());
else if(!pair.is_string() && pair.get_ddata_set())
Notify(var, pair.get_ddata());
}
pengine.clearPostings();
}
void MotionControler::postPengineResults()
{
bool all_stop = true;
if(pengine.hasControl())
all_stop = false;
if(all_stop)
{
if(allstop_posted)
return;
Notify("DESIRED_RUDDER", 0.0);
Notify("DESIRED_THRUST", 0.0);
// if(pengine.hasDphCtrl())
Notify("DESIRED_ELEVATOR", 0.0);
postColVarToBS(0,0,0);
allstop_posted = true;
}
else
{
int T=0,S=0,R=0;
if(pengine.hasHdgCtrl())
{
Notify("DESIRED_RUDDER", pengine.getDesiredRudder());
R = (int)pengine.getDesiredRudder();
}
if(pengine.hasSpdCtrl())
{
Notify("DESIRED_THRUST", pengine.getDesiredThrust());
T = (int)pengine.getDesiredThrust();
}
if(pengine.hasDphCtrl())
{
Notify("DESIRED_ELEVATOR", pengine.getDesiredElevator());
S = pengine.getDesiredElevator();
}
postColVarToBS(T,S,R);
allstop_posted = false;
}
}
void MotionControler::postCharStatus()
{
if(!verbose)
return;
if(pengine.hasControl())
cout << "$" << flush;
else
cout << "*" << flush;
}
void MotionControler::postColVarToBS(int T, int S, int R)
{
string s_msg;
colVar["mode"] = 2;
colVar["thrust"] = T;
colVar["rudder"] = R;
colVar["elevator"] = S;
s_msg = Json::writeString(RepJsBuilder,colVar);
Notify(MSG_TO_BS, s_msg);
}

66
src/pMotionControler/MotionControler.hpp Normal file
View File

@@ -0,0 +1,66 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 15:57:27
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-07 11:33:04
* @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef TESTAPP
#define TESEAPP
#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <iostream>
#include"pidControl.hpp"
#include<vector>
// #include"MOOS/libMOOS/Comms/XPCUdpSocket.h"
using namespace std;
class MotionControler : public AppCastingMOOSApp
{
public:
MotionControler(){};
~MotionControler(){};
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void registerVariables();
bool buildReport();
void postPenginePostings();
void postPengineResults();
void postColVarToBS(int T, int S, int R);
void postCharStatus();
const string MSG_FALUT = "uMotion_fault_fb";
const string MSG_ReadConfig = "uMotion_config_cmd";
const string MSG_TO_BS = "uMotion_control_cmd";
private:
bool ignore_nav_yaw;
bool allstop_posted;
bool verbose;
bool override;
double ok_skew = 2;
int faultCode = 0;
double setFrequency;
double frequency_delta;
Json::Value RepList;
Json::StreamWriterBuilder RepJsBuilder;
Json::Value colVar;
pidControl pengine;
string configFilePath;
int e;
};
#endif
//1.读取参数配置错误 faultCode = 10~
//2.信息过时错误 faultCode=1
//3.频率相差过大错误 faultCode=2

46
src/pMotionControler/a.moos Normal file
View File

@@ -0,0 +1,46 @@
ServerHost = localhost
ServerPort = 9000
ProcessConfig = pMotionControler
{
AppTick = 20
CommsTick = 20
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 1.2
yaw_pid_kd = 0.0
yaw_pid_ki = 0.3
yaw_pid_integral_limit = 0.07
// Speed PID controller
speed_pid_kp = 1.0
speed_pid_kd = 0.0
speed_pid_ki = 0.1
speed_pid_integral_limit = 0.07
maxpitch = 15
maxelevator = 13
pitch_pid_kp = 1.5
pitch_pid_kd = 0
pitch_pid_ki = 1.0
pitch_pid_integral_limit = 0
z_to_pitch_pid_kp = 0.12
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.004
z_to_pitch_pid_integral_limit = 0.05
//MAXIMUMS
maxrudder = 100
maxthrust = 100
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
}

181
src/pMotionControler/alpha.bhv Normal file
View File

@@ -0,0 +1,181 @@
//-------- FILE: alpha.bhv -------------
initialize RUN = false
initialize TaskNum=t1
initialize SendTask=false
//--------------模式判断------------------------
set MODE = ACTIVE{
RUN=true
} INACTIVE
set MODE = T1{
MODE=ACTIVE
TaskNum = t1
}
//----------路径点任务----------------------------
Behavior = BHV_Waypoint
{
name = waypt_survey
pwt = 100 //优先权重
condition = MODE==T1
//endflag = START=false
endflag = END_WayPoint=true
configflag = CRUISE_SPD = $[SPEED]
//configflag = OSPOS = $[OSX],$[OSY]
activeflag = INFO=$[OWNSHIP]
activeflag = INFO=$[BHVNAME]
activeflag = INFO=$[BHVTYPE]
//cycleflag = CINFO=$[OSX],$[OSY]
wptflag = CurrentPointComplete=true
wptflag = PREV=$(PX),$(PY)
wptflag = NEXT=$(NX),$(NY)
wptflag = TEST=$(X),$(Y)
wptflag = OSPOS=$(OSX),$(OSY)
//wptflag_on_start = true
updates = WPT_UPDATE
//perpetual = true
templating = spawn
// speed_alt = 1.2
//use_alt_speed = true
lead = 8
lead_damper = 1
lead_to_start = false
speed = 1 // meters per second
capture_line = true
capture_radius = 5.0
slip_radius = 15.0
efficiency_measure = all
polygon = 60,-40
order = normal
//repeat = 3
visual_hints = nextpt_color=yellow
visual_hints = nextpt_vertex_size=8
visual_hints = nextpt_lcolor=gray70
visual_hints = vertex_color=dodger_blue, edge_color=white
visual_hints = vertex_size=5, edge_size=1
}
//--------------定深任务------------------
Behavior=BHV_ConstantDepth
{
name = const_depth
pwt = 100
//condition = DEPLOY = true
condition = MODE==T1
duration = no-time-limit
updates = DEPTH_UPDATE
depth = 0
}
//--------------定向任务--------------------
Behavior=BHV_ConstantHeading
{
name = const_heading
pwt = 100
//condition = START_TURN = true
//condition = DEPLOY = true
condition = MODE==T3
perpetual = true
activeflag = TURN = started
//endflag = TURN = done
//endflag = RETURN = true
//endflag = START_TURN = false
endflag = START=false
heading = 225
complete_thresh = 5
duration = no-time-limit
}
//--------------定速任务--------------------
Behavior=BHV_ConstantSpeed
{
name = const_speed
pwt = 1000
condition = MODE==T1
perpetual = true
updates = SPEED_UPDATE
//endflag = START=false
speed = 5
duration = no-time-limit
//peakwidth = 0.5
//basewidth = 0.5
}
//----------------安全模式-----------------------
//----------------计时器---------------------
Behavior = BHV_Timer
{
name = mtime
condition = MODE==T1
pwt = 100
templating = spawn
//duration_status = MSTATUS
//duration = 10
endflag = TIME_OUT=true
updates = TIMER_UPDATES
//perpetual = true
}
//-------------最大深度限制--------------------------
Behavior = BHV_MaxDepth
{
name = maxdepth
pwt = 200
condition = MODE==ACTIVE
updates = MAXDEEP_UPDATES
max_depth = 20
tolerance = 0
duration = no-time-limit
}
//--------------安全区域设置-----------------------
Behavior = BHV_OpRegion
{
// General Behavior Parameters
// ---------------------------
name = op_region // example
pwt = 300 // default
condition = MODE==T5
updates = OPREGION_UPDATES // example
// Parameters specific to this behavior
// ------------------------------------
max_time = 20 // default (seconds)
max_depth = 25 // default (meters)
min_altitude = 0 // default (meters)
reset_var = OPREGION_RESET // example
trigger_entry_time = 1 // default (seconds)
trigger_exit_time = 0.5 // default (seconds)
polygon = pts={-80,-00:-30,-175:150,-100:95,25}
breached_altitude_flag = TaskFault = AltitudeOut
breached_depth_flag = TaskFault = DepthOut
breached_poly_flag = TaskFault = RegionOut
breached_time_flag = TaskFault = TimeOut
visual_hints = vertex_color = brown // default
visual_hints = vertex_size = 3 // default
visual_hints = edge_color = aqua // default
visual_hints = edge_size = 1 // default
}

285
src/pMotionControler/alpha.moos Normal file
View File

@@ -0,0 +1,285 @@
//-------------------------------------------------
// NAME: M. Benjamin, MIT CSAIL
// FILE: alpha.moos
//-------------------------------------------------
ServerHost = localhost
ServerPort = 9000
Community = alpha
MOOSTimeWarp = 1
// Forest Lake
LatOrigin = 43.825300
LongOrigin = -70.330400
// MIT Sailing Pavilion (use this one)
// LatOrigin = 42.358456
// LongOrigin = -71.087589
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
Run = pLogger @ NewConsole = false
//Run = uSimMarineV22 @ NewConsole = false
//Run = pMarinePIDV22 @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pRealm @ NewConsole = false
Run = pTaskManger @ NewConsole = false
Run = pMotionControler @ NewConsole = false
Run = pEmulator @ NewConsole = true
//Run = uTimerScript @ NewConsole = false
}
ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
}
ProcessConfig = pEmulator
{
AppTick = 5
CommsTick = 5
matlab_host = 192.168.0.11
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 10
start_y = 9
start_z = 1
start_heading = 30
}
//------------------------------------------
// pLogger config block
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
//Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
//Log = REPORT @ 0 NOSYNC
//Log = BHV_SETTINGS @ 0 NOSYNC
Log = OPREGION_RESET @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
//------------------------------------------
// uProcessWatch config block
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uQueryDB*
nowatch = uXMS*
nowatch = uMAC*
}
//------------------------------------------
// uSimMarineV22 config block
ProcessConfig = uSimMarineV22
{
AppTick = 4
CommsTick = 4
start_pos = x=0, y=-20, heading=180, speed=5
prefix = NAV
turn_rate = 40
thrust_map = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5
//thrust_reflect = true
buoyancy_rate = 0.075
max_depth_rate = 5
max_depth_rate_speed = 2.0
default_water_depth = 400
}
//------------------------------------------
// pHelmIvP config block
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMotionControler
{
AppTick = 10
CommsTick = 10
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 3.0
yaw_pid_kd = 0.0
yaw_pid_ki = 0.01
yaw_pid_integral_limit = 5.0
// Speed PID controller
speed_pid_kp = 50.0
speed_pid_kd = 0.0
speed_pid_ki = 10.0
speed_pid_integral_limit = 500.0
maxpitch = 10
maxelevator = 30
pitch_pid_kp = 10.0
pitch_pid_kd = 0
pitch_pid_ki = 0.1
pitch_pid_integral_limit = 5.0
z_to_pitch_pid_kp = 10.0
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.1
z_to_pitch_pid_integral_limit = 1.0
//MAXIMUMS MAXRUDDER
maxrudder = 35
maxthrust = 1525
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
}
//------------------------------------------
// pMarineViewer config block
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
vehicles_name_mode = names+depth //+shortmode
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = xlarge
procs_font_size = xlarge
appcast_font_size = large
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
//button_one = MOOS_MANUAL_OVERRIDE=false
button_two = STOP # START=false
//button_two = MOOS_MANUAL_OVERRIDE=true
button_three = FaultClear # ClearFalut = true
button_four = SendSecurityZone # SendSaftRules = true
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
//platform_type = kayak
//更改显示形状为uuv
platform_type = UUV
platform_color = red
platform_length = 4
}
ProcessConfig = uTimerScript
{
AppTick = 4
CommsTick = 4
condition = DEPLOY = true
randvar = varname = RND_DEPTH, min=20, max=80, key=at_reset
event = var = DEPTH_UPDATE, val=depth=$[RND_DEPTH], time=120
reset_max = nolimit reset_time = all-posted
}

61
src/pMotionControler/main.cpp Normal file
View File

@@ -0,0 +1,61 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-12 09:52:06
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-10-24 15:11:21
* @FilePath: /moos-ivp-extend/src/pMotionControler/main.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include<iostream>
using namespace std;
#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
// #include "VarDataPair.h"
#include"MotionControler.hpp"
#include <iostream>
#include<string>
#include "MBUtils.h"
#include "ColorParse.h"
// #include "MBUtils.h"
// #include "ColorParse.h"
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();
cout << " Version : 0.01" << endl;
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
// showExampleConfigAndExit();
cout << " example : NULL" << endl;
else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
// showHelpAndExit();
cout << " Low leave Control for UUV " << endl;
else if((argi=="-i") || (argi == "--interface"))
// showInterfaceAndExit();
cout << " UUV Motion Control " << endl;
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 << "pMarinePIDV22 launching as " << run_command << endl;
cout << termColor() << endl;
MotionControler marinePID;
marinePID.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
return(0);
}

305
src/pMotionControler/pidControl.cpp Normal file
View File

@@ -0,0 +1,305 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-16 15:14:15
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-03 11:30:38
* @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#include"pidControl.hpp"
pidControl::pidControl()
{
pidClear(pid_depth);
pidClear(pid_heading);
pidClear(pid_pitch);
pidClear(pid_speed);
setCurDepth(0,0);
setDesDepth(0,0);
setCurHeading(0,0);
setDesHeading(0,0);
setCurPitch(0,0);
setDesPitch(0,0);
setCurSpeed(0,0);
setDesSpeed(0,0);
const_thrust = 0;
}
int pidControl::step()
{
if(!setDesiredValues())
{
//这里不对时间进行重置，保持故障状态
pidClear(pid_depth);
pidClear(pid_heading);
pidClear(pid_pitch);
pidClear(pid_speed);
return -1; //Falut
}
if(has_override)
{
//由手动移交的控制权，需要对时间进行重置，防止进入故障状态
pidClear(pid_depth);
pidClear(pid_heading);
pidClear(pid_pitch);
pidClear(pid_speed);
setCurDepth(0,0);
setDesDepth(0,0);
setCurHeading(0,0);
setDesHeading(0,0);
setCurPitch(0,0);
setDesPitch(0,0);
setCurSpeed(0,0);
setDesSpeed(0,0);
RepList["Waring"] = int(has_override);
return 1; //Ready
}
RepList["Waring"] = int(has_override);
string rpt = "";
if(has_speedCtrl)
{
if(const_thrust == 0)
{
double speed_error = desired_speed.value - current_speed.value;
desired_thrust.value = pidStep(speed_error, pid_speed);
desired_thrust.time = current_time;
//Limit(desired_thrust.value, max_thrust, 0);
rpt = "PID_SPEED: ";
rpt += " (Want):" + doubleToString(desired_speed.value);
rpt += " (Curr):" + doubleToString(current_speed.value);
rpt += " (Diff):" + doubleToString(speed_error);
rpt += " (Delt):" + doubleToString(pid_speed.delta_output);
rpt += " THRUST:" + doubleToString(desired_thrust.value);
addPosting("PID_SPD_DEBUG", rpt);
RepList["spd_pid"]["Want"] = doubleToString(desired_speed.value,2);
RepList["spd_pid"]["Curr"] = doubleToString(current_speed.value,2);
RepList["spd_pid"]["Diff"] = doubleToString(speed_error,2);
RepList["spd_pid"]["Delt"] = doubleToString(pid_speed.delta_output,2);
RepList["spd_pid"]["THRUST"] = doubleToString(desired_thrust.value,2);
}
else
{
desired_thrust.value = const_thrust;
desired_thrust.time = current_time;
}
}
if(desired_thrust.value <= dead_zone)
{
desired_rudder.value = 0;
desired_rudder.time = current_time;
desired_elevator.value = 0;
desired_elevator.time = current_time;
return true;
}
if(has_headCtrl)
{
double heading_error = desired_heading.value - current_heading.value;
heading_error = angle180(heading_error);
desired_rudder.value = pidStep(heading_error, pid_heading);
desired_rudder.time = current_time;
//Limit(desired_rudder.value, max_rudder, -max_rudder);
// Limit();
rpt = "PID_COURSE: ";
rpt += " (Want):" + doubleToString(desired_heading.value);
rpt += " (Curr):" + doubleToString(current_heading.value);
rpt += " (Diff):" + doubleToString(heading_error);
rpt += " (Delt):" + doubleToString(pid_heading.delta_output);
rpt += " RUDDER:" + doubleToString(desired_rudder.value);
addPosting("PID_HDG_DEBUG", rpt);
RepList["hdg_pid"]["Want"] = doubleToString(desired_heading.value,2);
RepList["hdg_pid"]["Curr"] = doubleToString(current_heading.value,2);
RepList["hdg_pid"]["Diff"] = doubleToString(heading_error,2);
RepList["hdg_pid"]["Delt"] = doubleToString(pid_heading.delta_output,2);
RepList["hdg_pid"]["RUDDER"] = doubleToString(desired_rudder.value,2);
}
if(has_depthCtrl)
{
double depth_error = desired_depth.value - current_depth.value;
desired_pitch.value = -pidStep(depth_error, pid_depth);
desired_pitch.time = current_time;
//Limit(desired_pitch.value, max_pitch, -max_pitch);
double pitch_error = desired_pitch.value - current_pitch.value;
desired_elevator.value = pidStep(pitch_error, pid_pitch);
desired_elevator.time = current_time;
//Limit(desired_elevator.value, max_elevator, -max_elevator);
rpt = "PID_DEPTH: ";
rpt += " (Want):" + doubleToString(desired_depth.value);
rpt += " (Curr):" + doubleToString(current_depth.value);
rpt += " (Diff):" + doubleToString(depth_error);
rpt += " ELEVATOR:" + doubleToString(desired_elevator.value);
addPosting("PID_DEP_DEBUG", rpt);
RepList["dep_pid"]["Want"] = doubleToString(desired_depth.value,2);
RepList["dep_pid"]["Curr"] = doubleToString(current_depth.value,2);
RepList["dep_pid"]["Diff"] = doubleToString(depth_error,2);
RepList["dep_pid"]["Delt"] = doubleToString(pid_depth.delta_output,2);
RepList["pth_pid"]["Want"] = doubleToString(desired_pitch.value,2);
RepList["pth_pid"]["Curr"] = doubleToString(current_pitch.value,2);
RepList["pth_pid"]["Diff"] = doubleToString(pitch_error,2);
RepList["pth_pid"]["Delt"] = doubleToString(pid_pitch.delta_output,2);
RepList["pth_pid"]["ELEVATOR"] = doubleToString(desired_elevator.value,2);
}
// Limit();
return 0;
}
bool pidControl::setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid)
{
// pid.kp = p + i + d;
// pid.ki = -p - 2*d;
// pid.kd = d;
pid.kp = p;
pid.ki = i;
pid.kd = d;
pid.limit_delta = limitDelta;
pid.output = 0;
pid.max_out = max;
pid.min_out = min;
return true;
}
double pidControl::pidStep(double error, pidInc &pid)
{
double deltaOutPut = 0;
pid.error_0 = error;
deltaOutPut
= pid.kp * (pid.error_0 - pid.error_1 )
+ pid.ki * (pid.error_0 )
+ pid.kd * (pid.error_0 -2.0*pid.error_1 + pid.error_2);
// RepList["W"]["d"] = deltaOutPut;
// RepList["W"]["e1"] = pid.kp;
// RepList["W"]["e2"] = pid.ki;
// RepList["W"]["e3"] = pid.kd;
Limit(deltaOutPut, pid.limit_delta, -pid.limit_delta);
pid.output += deltaOutPut;
Limit(pid.output, pid.max_out, pid.min_out);
pid.delta_output = deltaOutPut;
pid.error_2 = pid.error_1;
pid.error_1 = pid.error_0;
return pid.output;
}
// double pidControl::picStep_p(double error, pidInc &pid)
// {
// pid.delta_output =
// }
bool pidControl::overrived(string svar)
{
Controler::overrived(svar);
return true;
}
//TODO: 快速检测当前参数是具有与当前控制器相关
bool pidControl::hasConfigSettings() const
{
if(config_params.size() == 0)
return(false);
list<string>::const_iterator p;
for(p=config_params.begin(); p!=config_params.end(); p++)
{
string line = *p;
string param = tolower(biteStringX(line, '='));
// if(param == "yaw_pid_kp")
// return(true);
// else if(param == "yaw_pid_kd")
// return(true);
// else if(param == "yaw_pid_ki")
// return(true);
// else if(param == "yaw_pid_integral_limit")
// return(true);
// else if(param == "yaw_pid_ki_limit")
// return(true);
return true;
}
return(false);
}
bool pidControl::setParam(char n, double param, pidInc *pid_t)
{
if(pid_t == NULL)
return false;
switch (n)
{
case 'p':
pid_t->kp = param;
break;
case 'i':
pid_t->ki = param;
break;
case 'd':
pid_t->kd = param;
break;
default:
return false;
break;
}
return true;
}
int pidControl::setParam(string filePath)
{
Json::Value paramList = getConfig(filePath);
if(paramList.empty())
return 1;
if(paramList["speed"].empty() || paramList["pitch"].empty()
|| paramList["depth"].empty() || paramList["pitch"].empty()
|| paramList["speedCol"].empty() || paramList["depthCol"].empty()
|| paramList["HeadingCol"].empty())
return 2; //重要参数不全
Json::Value param = paramList["speed"];
if(!setParam(param, pid_speed))
return 3;
RepList["speed-param"] = param;
param = paramList["heading"];
if(!setParam(param, pid_heading))
return 4;
RepList["heading-param"] = param;
param = paramList["pitch"];
if(!setParam(param, pid_pitch))
return 5;
RepList["pitch-param"] = param;
param = paramList["depth"];
if(!setParam(param, pid_depth))
return 6;
RepList["depth-param"] = param;
setSpeedControl(paramList["speedCol"].asBool());
setDepthControl(paramList["depthCol"].asBool());
setHeadingControl(paramList["HeadingCol"].asBool());
//次要参数
if(!paramList["dead_zone"].empty())
{
dead_zone = paramList["dead_zone"].asDouble();
RepList["dead_zone"] = dead_zone;
}
if(!paramList["const_thrust"].empty())
{
const_thrust = paramList["const_thrust"].asDouble();
RepList["const_thrust"] = const_thrust;
}
return 0;
}
bool pidControl::setParam(Json::Value param, pidInc &pid)
{
if(param.empty())
return false;
if(param["Kp"].empty() || param["Ki"].empty() || param["Kd"].empty())
return false;
pid.kp = param["Kp"].asDouble();
pid.ki = param["Ki"].asDouble();
pid.kd = param["Kd"].asDouble();
if(param["LimitDelta"].empty() || param["MaxOut"].empty() || param["MinOut"].empty())
return false;
pid.limit_delta = param["LimitDelta"].asDouble();
pid.max_out = param["MaxOut"].asDouble();
pid.min_out = param["MinOut"].asDouble();
return true;
}

77
src/pMotionControler/pidControl.hpp Normal file
View File

@@ -0,0 +1,77 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-10-16 15:14:26
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-02 18:03:14
* @FilePath: /moos-ivp-pi/src/pMotionControler/pidControl.hpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
#ifndef _PIDCONTROL_H
#define _PIDCONTROL_H
#include"Controler.hpp"
typedef struct
{
double kp;
double ki;
double kd;
double error_0;
double error_1;
double error_2;
double limit_delta;
double max_out;
double min_out;
double delta_output;
double output;
} pidInc;
class pidControl : public Controler
{
private:
/* data */
pidInc pid_speed;
pidInc pid_heading;
pidInc pid_pitch;
pidInc pid_depth;
double current_error;
double last_error;
vector<double> Error;
int Error_capacity;
public:
pidControl(/* args */);
~pidControl(){};
int step();
bool setParam(double p, double i, double d, double limitDelta, double max, double min, pidInc &pid);
bool setParam(char n, double param, pidInc *pid_t);
int setParam(string filePath);
bool setParam(Json::Value param, pidInc &pid);
bool hasConfigSettings() const;
double pidStep(double error, pidInc &pid);
//double picStep_p(double error, pidInc &pid);
bool overrived(string svar);
inline void pidClear(pidInc &pid)
{
pid.error_0 = 0;
pid.error_1 = 0;
pid.error_2 = 0;
pid.delta_output = 0;
pid.output = 0;
}
};
#endif

260
src/pMotionControler/simMat.moos Normal file
View File

@@ -0,0 +1,260 @@
//-------------------------------------------------
// NAME: M. Benjamin, MIT CSAIL
// FILE: alpha.moos
//-------------------------------------------------
ServerHost = localhost
ServerPort = 9000
Community = alpha
MOOSTimeWarp = 1
// Forest Lake
LatOrigin = 43.825300
LongOrigin = -70.330400
// MIT Sailing Pavilion (use this one)
// LatOrigin = 42.358456
// LongOrigin = -71.087589
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pEmulator @ NewConsole = false
//Run = pLogger @ NewConsole = false
Run = pMotionControler @ NewConsole = false
Run = pTaskManger @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
}
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
}
ProcessConfig = pEmulator
{
AppTick = 5
CommsTick = 5
matlab_host = 192.168.0.11
matlab_port = 8085
local_port = 8080
prefix = NAV
start_x = 10
start_y = 9
start_z = 1
start_heading = 30
}
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
//Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
//Log = REPORT @ 0 NOSYNC
//Log = BHV_SETTINGS @ 0 NOSYNC
Log = OPREGION_RESET @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
//------------------------------------------
// uProcessWatch config block
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uQueryDB*
nowatch = uXMS*
nowatch = uMAC*
}
//------------------------------------------
// uSimMarineV22 config block
//------------------------------------------
// pHelmIvP config block
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMotionControler
{
AppTick = 5
CommsTick = 5
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 10
yaw_pid_kd = 0.0
yaw_pid_ki = 0.01
yaw_pid_integral_limit = 10
// Speed PID controller
speed_pid_kp = 20.0
speed_pid_kd = 0.0
speed_pid_ki = 1
speed_pid_integral_limit = 100
maxpitch = 15
maxelevator = 30
pitch_pid_kp = 1.5
pitch_pid_kd = 0
pitch_pid_ki = 1.0
pitch_pid_integral_limit = 0
z_to_pitch_pid_kp = 0.12
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.004
z_to_pitch_pid_integral_limit = 0.05
//MAXIMUMS MAXRUDDER
maxrudder = 30
maxthrust = 1525
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
}
//------------------------------------------
// pMarineViewer config block
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
vehicles_name_mode = names+depth //+shortmode
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = xlarge
procs_font_size = xlarge
appcast_font_size = large
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
button_one = START # START=true
//button_one = MOOS_MANUAL_OVERRIDE=false
button_two = STOP # START=false
//button_two = MOOS_MANUAL_OVERRIDE=true
button_three = FaultClear # ClearFalut = true
button_four = SendSecurityZone # SendSaftRules = true
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
//platform_type = kayak
//更改显示形状为uuv
platform_type = UUV
platform_color = red
platform_length = 4
}

53
src/pSurfaceSupportComm/CMakeLists.txt Normal file
View File

@@ -0,0 +1,53 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pSurfaceSupportComm
# Author(s): Xiaobin Zeng
#--------------------------------------------------------
SET(SRC
SurfaceSupportComm.cpp
SurfaceSupportComm_Info.cpp
PeriodicUDPEvent.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(FindProtobuf)
# find_package(Protobuf REQUIRED)
# include_directories(${Protobuf_INCLUDE_DIR})
# protobuf_generate_cpp(PROTO_SRC PROTO_HEADER Behavior.proto NavigationInfo.proto)
# add_library(proto ${PROTO_HEADER} ${PROTO_SRC})
# include_directories(${CMAKE_CURRENT_SOURCE_DIR})
# find_package (jsoncpp NO_MODULE REQUIRED)
include_directories(/usr/include/jsoncpp/)
link_directories(/usr/local/lib/)
find_package (GeographicLib REQUIRED)
include_directories(${GeographicLib_INCLUDE_DIRS})
ADD_EXECUTABLE(pSurfaceSupportComm ${SRC})
TARGET_LINK_LIBRARIES(pSurfaceSupportComm
${MOOS_LIBRARIES}
${CMAKE_DL_LIBS}
${SYSTEM_LIBS}
${DUNE_LIB}
${GeographicLib_LIBRARIES}
mbutil
m
pthread
jsoncpp
# jsoncpp_lib_static
# protobuf
# protoc
# proto
# ${PROTOBUF_LIBRARY}
)

142
src/pSurfaceSupportComm/PeriodicTCPEvent.cpp Normal file
View File

@@ -0,0 +1,142 @@
#include "PeriodicTCPEvent.h"
#define TCP_RECEIVE_PORT 8000
class PeriodicTCPEvent::Impl
{
public:
Impl()
{
pfn_ = DefaultCallback;
pParamCaller_= NULL;
Period_ = 1.0;
}
static bool DefaultCallback(DUNE::IMC::Message * msg)
{
// UNUSED_PARAMETER(pParamCaller);
// std::cout.setf(std::ios::fixed);
// std::cout<<std::setprecision(4);
// std::cout<<"Timer Callback \n";
// std::cout<<" TimeNow "<<TimeNow<<"\n";
// std::cout<<" TimeScheduled "<<TimeScheduled<<"\n";
// std::cout<<" TimeLastRun "<<TimeLastRun<<"\n";
return true;
}
static bool PeriodicTCPEventDispatch(void * pParam)
{
PeriodicTCPEvent::Impl* pMe = static_cast<PeriodicTCPEvent::Impl*> (pParam);
return pMe->DoWork();
}
void SetCallback(bool (*pfn)(DUNE::IMC::Message * msg), void * pCallerParam)
{
UNUSED_PARAMETER(pCallerParam);
pfn_ = pfn;
//pParamCaller_ = pCallerParam;
}
bool SetPeriod(double PeriodSeconds)
{
if(PeriodSeconds<0)
return false;
Period_ = PeriodSeconds;
return true;
}
bool Stop()
{
Thread_.Stop();
return true;
}
bool Start()
{
if(Thread_.IsThreadRunning())
return false;
else
{
Thread_.Initialise(PeriodicTCPEventDispatch,this);
return Thread_.Start();
}
}
bool DoWork()
{
double TimeLast = MOOS::Time();
MOOS::BoostThisThread();
sock_tcp_receive.bind(TCP_RECEIVE_PORT);
sock_tcp_receive.listen(5);
while(!Thread_.IsQuitRequested())
{
double TimeScheduled=TimeLast+Period_;
size_t size = sock_tcp_receive.read(tcpReceiveBuffer, sizeof(tcpReceiveBuffer)/sizeof(uint8_t));
if (size > 0)
{
DUNE::IMC::Message * msg = DUNE::IMC::Packet::deserialize(tcpReceiveBuffer, size);
if(!(*pfn_)(msg))
{
break;
}
}
}
return true;
}
CMOOSThread Thread_;
bool (*pfn_)(DUNE::IMC::Message * msg);
void * pParamCaller_;
double Period_;
// DUNE::Network::UDPSocket sock_udp_send;
DUNE::Network::TCPSocket sock_tcp_receive;
uint8_t tcpReceiveBuffer[65535];
CMOOSLock m_Lock;
// std::stack<DUNE::IMC::Message *> msgBuffer;
};
void PeriodicTCPEvent::SetCallback(bool (*pfn)(DUNE::IMC::Message * msg), void * pCallerParam)
{
Impl_->SetCallback(pfn,pCallerParam);
}
PeriodicTCPEvent::PeriodicTCPEvent(): Impl_(new PeriodicTCPEvent::Impl )
{
}
bool PeriodicTCPEvent::SetPeriod(double PeriodSeconds)
{
return Impl_->SetPeriod(PeriodSeconds);
}
bool PeriodicTCPEvent::Start()
{
return Impl_->Start();
}
bool PeriodicTCPEvent::Stop()
{
return Impl_->Stop();
}
// bool PeriodicTCPEvent::Push(DUNE::IMC::Message* msg)
// {
// Impl_->msgBuffer.push(msg);
// }

39
src/pSurfaceSupportComm/PeriodicTCPEvent.h Normal file
View File

@@ -0,0 +1,39 @@
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
#include "MOOS/libMOOS/Utils/ThreadPriority.h"
#include "MOOS/libMOOS/Utils/MOOSThread.h"
#include <DUNE/DUNE.hpp>
#include <iostream>
#include <iomanip>
#include <chrono>
#include <thread>
class PeriodicTCPEvent
{
public:
PeriodicTCPEvent();
/**
* this sets the callback you wish to have called
*/
void SetCallback(bool (*pfn)(DUNE::IMC::Message * msg), void * pCallerParam);
/**
* Set the period of the event
*/
bool SetPeriod(double PeriodSeconds);
/** start the service*/
bool Start();
/** stop the service */
bool Stop();
private:
class Impl;
Impl * Impl_;
};

156
src/pSurfaceSupportComm/PeriodicUDPEvent.cpp Normal file
View File

@@ -0,0 +1,156 @@
#include "PeriodicUDPEvent.h"
#define UDP_SEND_PORT 6001
#define DEST_IP_ADDRESS "127.0.0.1"
class PeriodicUDPEvent::Impl
{
public:
Impl()
{
pfn_ = DefaultCallback;
pParamCaller_= NULL;
Period_ = 1.0;
}
static bool DefaultCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * pParamCaller)
{
UNUSED_PARAMETER(pParamCaller);
std::cout.setf(std::ios::fixed);
std::cout<<std::setprecision(4);
std::cout<<"Timer Callback \n";
std::cout<<" TimeNow "<<TimeNow<<"\n";
std::cout<<" TimeScheduled "<<TimeScheduled<<"\n";
std::cout<<" TimeLastRun "<<TimeLastRun<<"\n";
return true;
}
static bool PeriodicUDPEventDispatch(void * pParam)
{
PeriodicUDPEvent::Impl* pMe = static_cast<PeriodicUDPEvent::Impl*> (pParam);
return pMe->DoWork();
}
void SetCallback(bool (*pfn)(double TimeNow,double TimeLastRun,double TimeScheduled, void * pParamCaller), void * pCallerParam)
{
UNUSED_PARAMETER(pCallerParam);
pfn_ = pfn;
//pParamCaller_ = pCallerParam;
}
bool SetPeriod(double PeriodSeconds)
{
if(PeriodSeconds<0)
return false;
Period_ = PeriodSeconds;
return true;
}
bool Stop()
{
Thread_.Stop();
return true;
}
bool Start()
{
if(Thread_.IsThreadRunning())
return false;
else
{
Thread_.Initialise(PeriodicUDPEventDispatch,this);
return Thread_.Start();
}
}
bool DoWork()
{
double TimeLast = MOOS::Time();
MOOS::BoostThisThread();
while(!Thread_.IsQuitRequested())
{
double TimeScheduled=TimeLast+Period_;
std::string addr = DEST_IP_ADDRESS;
int size = -1;
int type = -1;
m_Lock.Lock();
if (!msgBuffer.empty())
{
DUNE::Utils::ByteBuffer bb;
DUNE::IMC::Message *msg = msgBuffer.top();
DUNE::IMC::Packet::serialize(msg, bb);
size = sock_udp_send.write(bb.getBuffer(), msg->getSerializationSize(),
DUNE::Network::Address(addr.c_str()), UDP_SEND_PORT);
type = msg->getId();
// delete msg;
msgBuffer.pop();
}
m_Lock.UnLock();
double TimeNow = MOOS::Time();
if(!(*pfn_)(MOOS::Time(), TimeLast,TimeScheduled,pParamCaller_))
{
break;
}
TimeLast = TimeNow;
}
return true;
}
CMOOSThread Thread_;
bool (*pfn_)(double TimeNow,double TimeLastRun,double TimeScheduled, void* pParam);
void * pParamCaller_;
double Period_;
DUNE::Network::UDPSocket sock_udp_send;
uint8_t udpReceiveBuffer[65535];
CMOOSLock m_Lock;
std::stack<DUNE::IMC::Message *> msgBuffer;
};
void PeriodicUDPEvent::SetCallback(bool (*pfn)(double Now,double LastRun,double Scheduled, void * pParamCaller), void * pCallerParam)
{
Impl_->SetCallback(pfn,pCallerParam);
}
PeriodicUDPEvent::PeriodicUDPEvent(): Impl_(new PeriodicUDPEvent::Impl )
{
}
bool PeriodicUDPEvent::SetPeriod(double PeriodSeconds)
{
return Impl_->SetPeriod(PeriodSeconds);
}
bool PeriodicUDPEvent::Start()
{
return Impl_->Start();
}
bool PeriodicUDPEvent::Stop()
{
return Impl_->Stop();
}
bool PeriodicUDPEvent::Push(DUNE::IMC::Message* msg)
{
Impl_->msgBuffer.push(msg);
}

42
src/pSurfaceSupportComm/PeriodicUDPEvent.h Normal file
View File

@@ -0,0 +1,42 @@
#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h"
#include "MOOS/libMOOS/Utils/ThreadPriority.h"
#include "MOOS/libMOOS/Utils/MOOSThread.h"
#include <DUNE/DUNE.hpp>
#include <iostream>
#include <iomanip>
#include <chrono>
#include <thread>
class PeriodicUDPEvent
{
public:
PeriodicUDPEvent();
/**
* this sets the callback you wish to have called
*/
// void SetCallback(bool (*pfn)(int type, int size), void * pCallerParam);
void SetCallback(bool (*pfn)(double Now,double LastRun,double Scheduled, void * pParamCaller), void * pCallerParam);
/**
* Set the period of the event
*/
bool SetPeriod(double PeriodSeconds);
/** start the service*/
bool Start();
/** stop the service */
bool Stop();
bool Push(DUNE::IMC::Message* msg);
private:
class Impl;
Impl * Impl_;
};

1118
src/pSurfaceSupportComm/SurfaceSupportComm.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

140
src/pSurfaceSupportComm/SurfaceSupportComm.h Normal file
View File

@@ -0,0 +1,140 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: SurfaceSupportComm.h */
/* DATE: */
/************************************************************/
#ifndef SurfaceSupportComm_HEADER
#define SurfaceSupportComm_HEADER
#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <unistd.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <arpa/inet.h>
#include "MOOS/libMOOS/MOOSLib.h"
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <DUNE/DUNE.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <json/json.h>
#include "PeriodicUDPEvent.h"
struct Landmark {
float lon;
float lat;
float depth;
float speed;
};
struct EstimatedState {
float referenceLon;
float referenceLat;
float referenceAltitude;
float currentLon;
float currentLat;
float currentAltitude;
float offsetNorth;
float offsetEast;
float offsetDown;
float roll;
float pitch;
float yaw;
float linearVelocityNorth;
float linearVelocityEast;
float linearVelocityDown;
float height;
float depth;
};
struct DeviceStatus {
unsigned int batteryVoltage;
unsigned int batteryLevel;
float batteryTemp;
unsigned int controllerTemp;
int thrustRpm;
unsigned int lightEnable;
unsigned int throwingLoadEnable;
unsigned int dvlStatus;
};
class SurfaceSupportComm : public CMOOSApp
// class SurfaceSupportComm : public AppCastingMOOSApp
{
public:
SurfaceSupportComm();
~SurfaceSupportComm();
protected: // Standard MOOSApp functions to overload
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void RegisterVariables();
// bool buildReport();
private:
char recv_buf[2048];
DUNE::Network::UDPSocket sock_udp_send;
DUNE::Network::TCPSocket sock_tcp_receive;
DUNE::IO::Poll m_poll;
uint16_t header_src, header_dst;
uint8_t header_src_ent, header_dst_ent;
std::string planConfigPath;
struct EstimatedState estimatedState;
struct DeviceStatus deviceStatus;
struct Client
{
DUNE::Network::TCPSocket* socket; // Socket handle.
DUNE::Network::Address address; // Client address.
uint16_t port; // Client port.
DUNE::IMC::Parser parser; // Parser handle
};
typedef std::list<Client> ClientList;
ClientList m_clients;
bool udpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port);
void tcpReceiveFromClient(char* buf, unsigned int cap, double timeout);
void tcpSendToClient(char* buf, unsigned int cap);
void acceptNewClient(void);
void handleClients(char* buf, unsigned int cap);
void closeConnection(Client& c, std::exception& e);
void processMessage(DUNE::IMC::Message * message);
void ConvertLLAToNED(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_ned);
void ConvertNEDToLLA(std::vector<double> init_lla,
std::vector<double> point_ned,
std::vector<double> &point_lla);
void ConvertLLAToENU(std::vector<double> init_lla,
std::vector<double> point_lla,
std::vector<double>& point_enu);
void ConvertENUToLLA(std::vector<double> init_lla,
std::vector<double> point_enu,
std::vector<double> &point_lla);
void BatchConvertCoordinate(Json::Value &object);
void ParsePlanConfig(Json::Value inputJsonValue);
int simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value);
int retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList);
template <typename Type>
inline int getEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, Type& value);
int testFlag = 0;
double TimeLast;
int sendCount = 0;
PeriodicUDPEvent udpEvent;
// PeriodicTCPEvent tcpEvent;
std::string missionStatusString;
};
#endif

115
src/pSurfaceSupportComm/SurfaceSupportComm_Info.cpp Normal file
View File

@@ -0,0 +1,115 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: SurfaceSupportComm_Info.cpp */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#include <cstdlib>
#include <iostream>
#include "SurfaceSupportComm_Info.h"
#include "ColorParse.h"
#include "ReleaseInfo.h"
using namespace std;
//----------------------------------------------------------------
// Procedure: showSynopsis
void showSynopsis()
{
blk("SYNOPSIS: ");
blk("------------------------------------ ");
blk(" The pSurfaceSupportComm application is used for ");
blk(" ");
blk(" ");
blk(" ");
blk(" ");
}
//----------------------------------------------------------------
// Procedure: showHelpAndExit
void showHelpAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("Usage: pSurfaceSupportComm file.moos [OPTIONS] ");
blu("=============================================================== ");
blk(" ");
showSynopsis();
blk(" ");
blk("Options: ");
mag(" --alias","=<ProcessName> ");
blk(" Launch pSurfaceSupportComm with the given process name ");
blk(" rather than pSurfaceSupportComm. ");
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 pSurfaceSupportComm. ");
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("pSurfaceSupportComm Example MOOS Configuration ");
blu("=============================================================== ");
blk(" ");
blk("ProcessConfig = pSurfaceSupportComm ");
blk("{ ");
blk(" AppTick = 4 ");
blk(" CommsTick = 4 ");
blk(" ");
blk("} ");
blk(" ");
exit(0);
}
//----------------------------------------------------------------
// Procedure: showInterfaceAndExit
void showInterfaceAndExit()
{
blk(" ");
blu("=============================================================== ");
blu("pSurfaceSupportComm 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("pSurfaceSupportComm", "gpl");
exit(0);
}

18
src/pSurfaceSupportComm/SurfaceSupportComm_Info.h Normal file
View File

@@ -0,0 +1,18 @@
/****************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT Cambridge MA */
/* FILE: SurfaceSupportComm_Info.h */
/* DATE: Dec 29th 1963 */
/****************************************************************/
#ifndef SurfaceSupportComm_INFO_HEADER
#define SurfaceSupportComm_INFO_HEADER
void showSynopsis();
void showHelpAndExit();
void showExampleConfigAndExit();
void showInterfaceAndExit();
void showReleaseInfoAndExit();
#endif

52
src/pSurfaceSupportComm/main.cpp Normal file
View File

@@ -0,0 +1,52 @@
/************************************************************/
/* NAME: Xiaobin Zeng */
/* ORGN: MIT */
/* FILE: main.cpp */
/* DATE: */
/************************************************************/
#include <string>
#include "MBUtils.h"
#include "ColorParse.h"
#include "SurfaceSupportComm.h"
#include "SurfaceSupportComm_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 << "pSurfaceSupportComm launching as " << run_command << endl;
cout << termColor() << endl;
SurfaceSupportComm SurfaceSupportComm;
SurfaceSupportComm.Run(run_command.c_str(), mission_file.c_str());
return(0);
}

9
src/pSurfaceSupportComm/pSurfaceSupportComm.moos Normal file
View File

@@ -0,0 +1,9 @@
//------------------------------------------------
// pSurfaceSupportComm config block
ProcessConfig = pSurfaceSupportComm
{
AppTick = 4
CommsTick = 4
}

27
src/pTaskManger/CMakeLists.txt Normal file
View File

@@ -0,0 +1,27 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pTaskManger
# Author(s): zjk
#--------------------------------------------------------
FILE(GLOB SRC *.cpp)
#include_directories(/usr/local/include/jsoncpp)
#link_directories(/usr/local/lib/)
include_directories(/usr/include/jsoncpp/)
link_directories(/usr/local/lib/)
ADD_EXECUTABLE(pTaskManger ${SRC})
TARGET_LINK_LIBRARIES(pTaskManger
${MOOS_LIBRARIES}
mbutil
m
pthread
jsoncpp)

999
src/pTaskManger/TaskManger.cpp Normal file
View File

@@ -0,0 +1,999 @@
/************************************************************/
/* NAME: zjk */
/* ORGN: MIT */
/* FILE: TaskManger.cpp */
/* DATE: */
/************************************************************/
//TODO: 增加故障处理
#include <list>
#include <iterator>
#include "MBUtils.h"
#include "TaskManger.h"
using namespace std;
#define DEBUG
//---------------------------------------------------------
// Constructor
TaskManger::TaskManger()
{
cout << "Task Manger process staring ... " << endl;
}
//---------------------------------------------------------
// Destructor
TaskManger::~TaskManger()
{
Notify("RUN","false");
Notify("MOOS_MANUAL_OVERRIDE","true");
}
//---------------------------------------------------------
// Procedure: OnNewMail
bool TaskManger::OnNewMail(MOOSMSG_LIST &NewMail)
{
AppCastingMOOSApp::OnNewMail(NewMail);
MOOSMSG_LIST::iterator p;
for(p=NewMail.begin(); p!=NewMail.end(); p++)
{
CMOOSMsg &msg = *p;
string msg_name = msg.GetName();
string msg_str = msg.GetString();
double msg_dval = msg.GetDouble();
bool msg_bval = msg.GetBinaryData();
// cout << msg_name + ": " << msg_str << endl;
if(msg_name == MSG_ENDFLAG)
{
if(msg_str == "true")
current_node_complete = true;
else
current_node_complete = false;
}
if(msg_name == MSG_WPTFLAG)
{
if(msg_str == "true")
current_pol_complete = true;
else
current_pol_complete = false;
clearHelmFlag();
}
if(msg_name == MSG_FALUT)
{
state = FAULT;
faultMsg = msg_str;
if(faultMsg == "AltitudeOut")
faultNumber = 4;
else if(faultMsg == "DepthOut")
faultNumber = 1;
else if(faultMsg == "RegionOut")
faultNumber = 2;
else if(faultMsg == "TimeOut")
faultNumber = 2;
else
faultNumber = 99;
}
if(msg_name == MSG_SENDSAFTRULES && msg_str == "true")
{
state = CONFIG;
st = 40;
}
if(msg_name == MSG_CLEARFAULT)
{
st = 39;
state = FAULT;
}
if(msg_name == MSG_IN_SSM)
{
Json::Value j;
Json::Reader a;
a.parse(msg_str,j);
RepList["FormSSM"] = j;
if(j["action"].asString() == "start")
{
taskName = j["taskName"].asString();
state = RUN;
cout << "Task Manager Status is Run!!!" << endl;
start = true;
st = 10;
}
else if (j["action"].asString() == "stop")
{
taskName = "";
state = UNRUN;
cout << "Task Manager Status is UnRun!!!" << endl;
st = 20;
}
else
{
RepList["RunWaring"] = "form SSM have unhandle action : " + j["action"].asString();
}
}
if(msg_name == MSG_IN_MAN)
{
if(msg_dval == 1)
state = MANUAL;
else
state = UNRUN;
}
}
return(true);
}
//---------------------------------------------------------
// Procedure: OnConnectToServer
bool TaskManger::OnConnectToServer()
{
Notify(MSG_ENDFLAG,"false");
Notify(MSG_WPTFLAG,"false");
// Notify(MSG_START,"false");
Notify(MSG_RUN,"false");
RegisterVariables();
InitConfig();
return(true);
}
//---------------------------------------------------------
// Procedure: Iterate()
bool TaskManger::Iterate()
{
// happens AppTick times per second
AppCastingMOOSApp::Iterate();
switch (state)
{
case RUN: //状态0～10
{
switch(st)
{
case 0://发送任务开始、超时和第一个节点
{
//1. send first node
current_node = getNode(nodeList);
if(current_node == "")
st = 9;
else
{
sendNode(current_node, nodeList);
st = 1;
}
//2. send start
taskStart();
current_node_complete = false;
current_pol_complete = false;
break;
}
case 1://等待节点完成信息
{
if(current_node_complete || current_pol_complete)
{
cout << "Current WayPoint Node complete!" << endl;
if(current_node == "")
st = 9;
else
st = 2;
}
break;
}
case 2://读取当前节点信息
{
current_node = getNode(nodeList);
if(current_node != "")
st = 3;
else
st = 1;
break;
}
case 3://发送任务节点
{
sendNode(current_node, nodeList);
current_node_complete = false;
current_pol_complete = false;
st = 1;
break;
}
case 9://task complete
{
taskFinish();
st = 10;
start = false;
break;
}
case 10://等待任务开始
{
if(start)
st = 11;
break;
}
case 11://读取任务文件
{
faultMsg = "";
readTask = readTaskFile(taskName);
if( readTask != 0)
{
state = FAULT;
faultNumber = 10 + readTask;
st =30;
faultMsg = "TaskFileError : " + intToString(readTask);
}
st = 0;
break;
}
default:
{
st = 10;
//cout << "State reset ..." << endl;
//taskFinish();
break;
}
}
break;
}
case UNRUN: //状态20～29
{
switch(st)
{
case 20://进入UNRUN，初始化状态变量
{
current_node_complete = false;
current_pol_complete = false;
current_node="";
start = false;
Notify(MSG_ENDFLAG,"false");
Notify(MSG_WPTFLAG,"false");
// Notify(MSG_START,"false");
Notify(MSG_RUN,"false");
Notify("MOOS_MANUAL_OVERRIDE","true");
cout << "Wating task start......" << endl;
st = 21;
break;
}
case 21://等待任务开始
{
break;
}
default:
{
st = 20;
break;
}
}
break;
}
case FAULT: //状态30～39
{
switch(st)
{
case 30: //判断故障信息
{
if(faultNumber>0 && faultNumber<10)
st = 31;
else if(faultNumber>10 && faultNumber<20)
st = 32;
else
st = 38;
break;
}
case 31: //任务错误
{
postFaultNumber(faultNumber);
state = UNRUN;
break;
}
case 32: //任务文本错误
{
postFaultNumber(faultNumber);
state = UNRUN;
break;
}
case 38: //未知故障
{
postFaultNumber(99);
break;
}
case 39:
{
FaultFlagClear();
state = UNRUN;
break;
}
default:
{
st = 30;
break;
}
}
break;
}
case CONFIG: //状态40～49
{
switch (st)
{
case 40: //配置安全规则
{
readSafetyRules("a");
setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon);
// setMaxDepth(maxDepth);
setMaxDepth("2");
st = 49;
break;
}
case 41: //路径参数配置
{
readWayConfig("a");
setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius);
st = 49;
//setWayConfig();
}
case 49: //等待
{
state = UNRUN;
break;
}
default:
{
st = 49;
break;
}
}
break;
}
case MANUAL: // 手操状态
{
switch (st)
{
case 50: // 清除变量
current_node_complete = false;
current_pol_complete = false;
current_node="";
start = false;
Notify(MSG_ENDFLAG,"false");
Notify(MSG_WPTFLAG,"false");
Notify(MSG_RUN,"false");
Notify("MOOS_MANUAL_OVERRIDE","true");
cout << "Wating task start......" << endl;
st = 51;
break;
case 51: // 等待
break;
default:
st = 50;
break;
}
break;
}
default:
{
st = 30;
break;
}
}
postReportToSSM();
AppCastingMOOSApp::PostReport();
return(true);
}
//---------------------------------------------------------
// Procedure: OnStartUp()
// happens before connection is open
bool TaskManger::OnStartUp()
{
AppCastingMOOSApp::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 == "PLANCONFIGPATH")
{
planConfigPath = value;
RepList["CueenPath"] = planConfigPath;
}
}
}
if(planConfigPath == "")
reportConfigWarning("NO TASK FILE PATH");
// readTaskFile("a");
RegisterVariables();
return(true);
}
//---------------------------------------------------------
// Procedure: RegisterVariables
/**
* @description:
* @param {double} depth
* @return {*}
*/
bool TaskManger::setMaxDepth(string depth)
{
string msgContent = "";
msgContent = "max_depth=";
msgContent += depth;
Notify(UPDATE_MAXDEP, msgContent);
cout << "set max depth is " + depth << "m!" << endl;
return true;
}
bool TaskManger::setWayPol(string polygon, string speed, string depth)
{
string msgContent="";
if(polygon=="#")
{
msgContent = "speed=";
msgContent += speed;
Notify(UPDATE_SPD, msgContent);
msgContent = "depth=" + depth;
Notify(UPDATE_DEP, msgContent);
}
else
{
msgContent = "polygon=";
msgContent += polygon;
msgContent += "#";
msgContent += "speed=";
msgContent += speed;
Notify(UPDATE_WPT, msgContent);
//使用模版参数生成一个新的行为r
Notify(UPDATE_WPT,"name=way_track");
// msgContent = "polygon=";
// msgContent += polygon;
// msgContent += "#";
Notify(UPDATE_WPT, msgContent);
msgContent = "speed=";
msgContent += speed;
Notify(UPDATE_SPD, msgContent);
msgContent = "depth=" + depth;
Notify(UPDATE_DEP, msgContent);
};
return true;
}
/**
* @description:
* @param {string} wpt
* @return {*}
*/
bool TaskManger::setWpt(std::string wpt, string speed, string depth)
{
string msgContent="";
msgContent += "points=";
msgContent += wpt;
msgContent += "#";
msgContent += "speed=";
msgContent += speed;
// msgContent += "#";
Notify(UPDATE_WPT,msgContent);
Notify(UPDATE_WPT,"name=way_topoint");
msgContent = "depth=" + depth;
Notify(UPDATE_DEP, msgContent);
msgContent = "speed=";
msgContent += speed;
Notify(UPDATE_SPD, msgContent);
cout << "MSG is : " << msgContent + "#depth=" << depth << endl;
// Notify("WPT_UPDATE", wpt);
return true;
}
/**
* @description:
* @param {double} speed
* @return {*}
*/
bool TaskManger::setSpeed(double speed)
{
Notify("SPEED_UPDATE", speed);
return true;
}
bool TaskManger::setTaskTimer(string timeCount)
{
cout << "Set task timer is " << timeCount << "seconds!" << endl;
string msgContent="";
msgContent += "name=taskTimer";
msgContent += "#";
msgContent += "duration=";
msgContent += timeCount;
Notify(UPDATE_TIMER,msgContent);
}
bool TaskManger::setSafetyRules(string maxTime, string maxDepth, string minAltitude, string polygon)
{
string msgContent="";
msgContent += "max_time=";
msgContent += maxTime;
msgContent += "#";
msgContent += "max_depth=";
msgContent += maxDepth;
msgContent += "#";
msgContent += "min_altitude=";
msgContent += minAltitude;
msgContent += "#";
msgContent += "polygon=";
msgContent += polygon;
cout << " The Op_Region parm is : " << msgContent << endl;
Notify(UPDATE_OPREGION, msgContent);
return true;
}
bool TaskManger::setWayConfig(string lead, string lead_damper, string capture_line, string capture_radius,string slip_radius)
{
string msgContent="";
msgContent = "lead=";
msgContent += lead;
msgContent += "#";
msgContent += "lead_damper=";
msgContent += lead_damper;
msgContent += "#";
msgContent += "capture_line=";
msgContent += capture_line;
msgContent += "#";
msgContent += "capture_radius=";
msgContent += capture_radius;
msgContent += "#";
msgContent += "slip_radius=";
msgContent += slip_radius;
Notify(UPDATE_WPT, msgContent);
cout << "Config waypoint parm is :" << msgContent << endl;
}
/**
* @description:
* @return {*}
*/
void TaskManger::RegisterVariables()
{
AppCastingMOOSApp::RegisterVariables();
Register(MSG_ENDFLAG, 0);
// Register(MSG_START, 0);
Register(MSG_WPTFLAG,0);
Register(MSG_SENDSAFTRULES,0);
Register(MSG_FALUT,0);
Register(MSG_CLEARFAULT,0);
Register(MSG_IN_SSM,0);
Register(MSG_IN_MAN,0);
}
/**
* @description: read task File to nodeList
* @param {string} fileName
* @return {*} 0 if true else >0
*/
int TaskManger::readTaskFile(string taskName)
{
int faultNubmer = 0;
if(!nodeList.empty())
nodeList.clear();
ifstream ifs;
ifs.open(planConfigPath, ios::in);
Json::Reader taskConfigureReader;
Json::Value inputJsonValue;
Json::Value currentTask;
taskConfigureReader.parse(ifs, inputJsonValue);
ifs.close();
int taskCount = inputJsonValue.size();
taskList = inputJsonValue.getMemberNames();
taskCount = inputJsonValue.size();
//part1 ： 判断是否存在这个任务
if (!inputJsonValue.isMember(taskName))
{
RepList["Task in File"] = "False";
return faultNubmer=1;
}
else
{
//part2 : 读取任务
string node="";
currentTask = inputJsonValue[taskName];
if(currentTask["taskName"].asString() != taskName)
return faultNubmer=2;
double currentTask_maxTime = currentTask["duration"].asDouble();
double repeat = currentTask["repeat"].asDouble();
if(!currentTask["points"].isArray())
return faultNubmer=3;
Json::Value currentTask_Points = currentTask["points"];
int ps_cnt = currentTask_Points.size();
for(int i=0; i<ps_cnt; i++)
{
string node_name = currentTask_Points[i]["name"].asString();
string node_x = currentTask_Points[i]["north"].asString();
string node_y = currentTask_Points[i]["east"].asString();
string node_depth = currentTask_Points[i]["depth"].asString();
string node_speed = currentTask_Points[i]["speed"].asString();
string node_type = currentTask_Points[i]["type"].asString();
if(node_type == "point")
node_type = "@1";
else if(node_type == "track")
node_type = "@2";
else
return faultNubmer=4;
node = node_type;
node += ",";
node += node_x;
node += ",";
node += node_y;
node += ",";
node += node_depth;
node += ",";
node += node_speed;
node += ",";
node += node_name;
nodeList.push_back(node);
}
}
auto i = nodeList.begin();
cout << "-------current task node--------" << endl;
while (i != nodeList.end())
{
cout << *i << endl;
i++;
}
cout << "--------------------------------" << endl;
return 0;
}
int TaskManger::readSafetyRules(string fileName)
{
maxDepth = "30";
maxTime = "1000";
safePolygon = "pts={-80,-00:-30,-175:150,-100:95,25}";
return 0;
}
int TaskManger::readWayConfig(string filename)
{
lead = "8";
lead_damper = "1";
lead_to_start = "false";
capture_line = "true";
capture_radius = "5";
slip_radius = "15";
efficiency_measure = "all";
}
/**
* @description: get Depth form
* @param {string} node
* @return {*}
*/
string TaskManger::getDepth(string node)
{
string depth;
if(getWord(node, 3, depth))
{
// cout << "Current Node Depth is " << depth <<endl;
return depth;
}
else
cout << "Warming: read Depth Parm Filed" << endl;
return depth;
}
/**
* @description:
* @param {string} node
* @return {*}
*/
string TaskManger::getSpeed(string node)
{
string speed;
if(getWord(node, 4, speed))
{
// cout << "Current Node Speed is " << speed <<endl;
return speed;
}
else
cout << "Warming: read Speed Parm Filed" << endl;
// getWord(node, 3, depth);
return speed;
}
/**
* @description:
* @param {string} node
* @return {*}
*/
string TaskManger::getWayPoint(string node)
{
string x,y;
bool fr;
fr = getWord(node, 1, x);
fr &= getWord(node, 2, y);
if(fr)
{
// cout << "Current WayPoint is: " << x << "," << y << endl;
return x+","+y;
}
else
cout << "Warming: read WayPoint Parm Filed" << endl;
}
string TaskManger::getWayPolygon(const list<string> nodeList)
{
auto i = nodeList.begin();
string polygon="";
TaskType t;
while(i != nodeList.end())
{
if(getTaskTpye(*i)==WAYPOLYGON)
{
polygon += getWayPoint(*i);
polygon += ":";
}
else
break;
i++;
}
if(polygon.size()>1)
polygon.erase(polygon.end()-1);
// cout << "Current way polygon is: " << polygon << endl;
return polygon;
}
/**
* @description:
* @param {string} node
* @return {*}
*/
TaskType TaskManger::getTaskTpye(string node)
{
string type;
TaskType nodetype;
if(getWord(node, 0, type))
{
// cout << "Current task type is: " << type << endl;
// return type;
if(type=="@1")
return nodetype=WAYPOINTS;
else if(type=="@2")
return nodetype=WAYPOLYGON;
else if(type=="@3")
return nodetype=CONSTHEADING;
else
return nodetype=NOTYPE;
}
else
cout << "Warming: read type of task Parm Filed" << endl;
}
/**
* @description:
* @param {string} str
* @param {int} c
* @return {*}
*/
bool TaskManger::getWord(const string str, int ct, string &word)
{
string cword;
int count=0;
for(auto c:str)
{
if(c==',')
{
if(count==ct)
{
word = cword;
return true;
}
cword="";
count++;
}
else
cword += c;
}
word = cword;
if(ct==0)
return true;
else
return false;
}
string TaskManger::getNode(list<string> &nodelist)
{
string current_node;
if(!nodelist.empty())
{
cout << "The remaining number of nodes is: " << nodelist.size() << endl;
current_node = nodelist.front();
nodelist.pop_front();
cout << "Current Node is " << current_node << endl;
}
else
current_node="";
return current_node;
}
string TaskManger::getTimeCount()
{
return "1000";
}
string TaskManger::getMaxDepth()
{
return "20";
}
string TaskManger::getNodeName(const string node)
{
string name="";
if(getWord(node, 5, name))
{
return name;
}
else
cout << "Warming: read Speed Parm Filed" << endl;
// getWord(node, 3, depth);
return name;
}
bool TaskManger::sendNode(const string current_node, list<string> &nodeList)
{
// cout << "Current WayPoint Node complete" << endl;
string node_pol;
nodeType = getTaskTpye(current_node);
switch(nodeType)
{
case WAYPOINTS:
{
node_wpt = getWayPoint(current_node);
node_depth = getDepth(current_node);
node_speed = getSpeed(current_node);
setWpt(node_wpt, node_speed, node_depth);
current_node_complete = false;
break;
}
case WAYPOLYGON:
{
node_depth = getDepth(current_node);
node_speed = getSpeed(current_node);
if (current_pol_complete && !current_node_complete) //wayflag发布但是endflag没有发布时代表不是首个节点，不发布路径信息
{
node_pol = "#";
}
else //if(current_pol_complete && current_node_complete)
{
nodeList.push_front(current_node);
node_pol = getWayPolygon(nodeList);
nodeList.pop_front();
}
setWayPol(node_pol, node_speed, node_depth);
cout << "MSG is :Pol=" + node_pol + "#speed=" + node_speed + "#depth=" + node_depth<< endl;
break;
}
default:
break;
}
current_node_complete = false;
current_pol_complete = false;
return true;
}
void TaskManger::FaultFlagClear()
{
faultNumber = 0;
if(faultMsg == "AltitudeOut")
Notify(UPDATE_RESETFAULT,"altittude");
else if(faultMsg == "DepthOut")
Notify(UPDATE_RESETFAULT,"depth");
else if(faultMsg == "RegionOut")
Notify(UPDATE_RESETFAULT,"poly");
else if(faultMsg == "TimeOut")
Notify(UPDATE_RESETFAULT,"time");
else
cout << "Can not Clear Unknow Fault Flag!!!" << endl;
cout << "Clear Fault : " + faultMsg << endl;
}
bool TaskManger::buildReport()
{
switch (state)
{
case UNRUN:
{
m_msgs << "$Task Manager Status$:" << "UNRUN" << endl;
m_msgs << "$Idel Task Number$:" << taskCount << endl;
break;
}
case RUN:
{
m_msgs << "$Task Manager Status$:" << "RUN" << endl;
m_msgs << "$Current Task Point$ :" << current_node << endl;
m_msgs << "Currnet Task List :" << endl;
for(string i : nodeList)
m_msgs << " : " << i << endl;
break;
}
case FAULT:
{
m_msgs << "$Task Manager Status$:" << "FAULT" << endl;
break;
}
default:
break;
}
m_msgs << "=========================================================" << endl;
RepList["Current Node"] = current_node;
RepList["remaining number of nodes"] = nodeList.size()+1;
string rep = Json::writeString(RepJsBuilder, RepList);
m_msgs << rep << endl;
return true;
}
inline void TaskManger::taskStart()
{
Notify("RUN","true");
Notify("MOOS_MANUAL_OVERRIDE","false");
}
inline void TaskManger::taskFinish()
{
current_node="";
Notify("RUN","false");
state = UNRUN;
cout << "The task node is all complete!!!" << endl;
Notify("MOOS_MANUAL_OVERRIDE","true");
}
inline void TaskManger::InitConfig()
{
Notify(MSG_ENDFLAG,"false");
Notify(MSG_WPTFLAG,"false");
// Notify(MSG_START,"false");
Notify(MSG_RUN,"false");
//
readSafetyRules("q");
setSafetyRules(maxTime, maxDepth, minAltitude, safePolygon);
readWayConfig("a");
setWayConfig(lead,lead_damper,capture_line,capture_radius,slip_radius);
}
void TaskManger::postReportToSSM()
{
string s_msg;
Json::Value msg;
msg["state"] = state;
msg["taskName"] = taskName;
msg["destName"] = getNodeName(current_node);
msg["errorCode"] = faultNumber;
// msg["progess"] =
// msg["eta"] =
RepList["toSSM"] = msg;
s_msg = Json::writeString(RepJsBuilder,msg);
Notify(MSG_TO_SSM, s_msg);
}
//1:超深
//2.超时
//3.超出区域
//4.低于最小高度
//11:任务文本
//99.未知故障
void TaskManger::postFaultNumber(int n)
{
Notify(MSG_TO_FH,n);
}

150
src/pTaskManger/TaskManger.h Normal file
View File

@@ -0,0 +1,150 @@
/*
* @Author: 1553836110 1553836110@qq.com
* @Date: 2023-09-28 15:45:17
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-11-08 14:53:05
* @FilePath: /moos-ivp-pi/src/pTaskManger/TaskManger.h
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
/************************************************************/
/* NAME: zjk */
/* ORGN: MIT */
/* FILE: TaskManger.h */
/* DATE: */
/************************************************************/
#ifndef TaskManger_HEADER
#define TaskManger_HEADER
#include "MOOS/libMOOS/MOOSLib.h"
#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h"
#include <queue>
#include <list>
#include "json/json.h"
#include "VarDataPair.h"
using namespace std;
enum TaskType{WAYPOINTS, CONSTDEPTH, CONSTSPEED, CONSTHEADING, WAYPOLYGON, NOTYPE};
enum Status{FAULT=0, UNRUN=1, MANUAL=2 ,RUN=3, CONFIG=5};
// class TaskManger : public CMOOSApp
class TaskManger : public AppCastingMOOSApp
{
public:
TaskManger();
virtual ~TaskManger();
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void RegisterVariables();
bool buildReport();
void postFaultNumber(int n);
bool setSpeed(double speed);
bool setMaxDepth(string depth);
bool setWpt(std::string wpt, string speed, string depth);
bool setWayPol(string polygon, string speed, string depth);
bool setTaskTimer(string timerCount);
bool setSafetyRules(string maxTime, string maxDepth, string minAltitude, string polygon);
bool setWayConfig(string lead, string lead_damper, string capture_line, string capture_radius,string slip_radius);
bool sendNode(const string current_node, list<string> &nodeList);
bool getWord(const string str, int ct, string &word);
string getSpeed(const string node);
string getDepth(const string node);
string getWayPoint(const string node);
string getWayPolygon(const list<string> nodeList);
string getNodeName(const string node);
string getNode(list<string> &nodelist);
string getTimeCount();
string getMaxDepth();
TaskType getTaskTpye(const string node);
int readTaskFile(string taskName);
int readSafetyRules(string fileName);
int readWayConfig(string fileName);
void postReportToSSM();
inline void clearHelmFlag(){Notify("HELM_MAP_CLEAR",0.0);}
inline void taskStart();
inline void taskFinish();
void FaultFlagClear();
inline void InitConfig();
const string UPDATE_WPT = "WPT_UPDATE";
const string UPDATE_SPD = "SPEED_UPDATE";
const string UPDATE_DEP = "DEPTH_UPDATE";
const string UPDATE_TIMER = "TIMER_UPDATES";
const string UPDATE_MAXDEP = "MAXDEEP_UPDATES";
const string UPDATE_OPREGION = "OPREGION_UPDATES";
const string UPDATE_RESETFAULT = "OPREGION_RESET";
const string MSG_WPTFLAG = "CurrentPointComplete";
const string MSG_ENDFLAG = "END_WayPoint";
const string MSG_START = "START";
const string MSG_SENDSAFTRULES = "SendSaftRules";
const string MSG_FALUT = "TaskFault";
const string MSG_RUN = "RUN";
const string MSG_CLEARFAULT = "ClearFalut";
const string MSG_IN_SSM = "uMission_action_cmd";
const string MSG_TO_SSM = "uMission_task_fb";
const string MSG_TO_FH = "uMission_fault_fb";
const string MSG_IN_FH = "uDrive_fault_fb";
const string MSG_IN_MAN = "uManual_enable_cmd"; //TODO: 增加手操状态
protected:
// 任务参数
string node_wpt;
string node_depth;
string node_speed;
// 安全规则
string maxDepth;
string maxTime;
string safePolygon;
string minAltitude="0";
// 路径参数
string lead;
string lead_damper;
string lead_to_start;
string capture_line;
string capture_radius;
string slip_radius;
string efficiency_measure;
string faultMsg;
TaskType nodeType;
list<string> nodeList;
private:
bool current_node_complete = false;
bool current_pol_complete = false;
bool start=false;
bool time_out=false;
int st=10;
int faultNumber = 0;
int readTask = 0;
string msg;
string current_node;
Status state = UNRUN;
Json::Value RepList;
Json::StreamWriterBuilder RepJsBuilder;
//任务文件参数
//TODO:动态配置任务文件等参数
string planConfigPath;
vector<string> taskList;
int taskCount;
string taskName;
//发送节点信息
Json::Value node_rpt;
};
#endif

65
src/pTaskManger/TaskMangerMain.cpp Normal file
View File

@@ -0,0 +1,65 @@
/*
* @Author: zjk 1553836110@qq.com
* @Date: 2023-09-28 15:45:17
* @LastEditors: zjk 1553836110@qq.com
* @LastEditTime: 2023-10-24 15:13:22
* @FilePath: /moos-ivp-extend/src/pTaskManger/TaskMangerMain.cpp
* @Description:
*
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
*/
/************************************************************/
/* NAME: zjk */
/* ORGN: MIT */
/* FILE: TaskMangerMain.cpp */
/* DATE: */
/************************************************************/
#include <string>
#include "TaskManger.h"
#include "ColorParse.h"
#include "MBUtils.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();
cout << " Version : 0.01" << endl;
else if((argi=="-e") || (argi=="--example") || (argi=="-example"))
// showExampleConfigAndExit();
cout << " example : NULL" << endl;
else if((argi=="-h") || (argi == "--help") || (argi=="-help"))
// showHelpAndExit();
cout << " Low leave Control for UUV " << endl;
else if((argi=="-i") || (argi == "--interface"))
// showInterfaceAndExit();
cout << " UUV Motion Control " << endl;
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 << "pTaskManger launching as " << run_command << endl;
cout << termColor() << endl;
TaskManger TaskMangerApp;
TaskMangerApp.Run(run_command.c_str(), mission_file.c_str(), argc, argv);
return(0);
}

181
src/pTaskManger/alpha.bhv Normal file
View File

@@ -0,0 +1,181 @@
//-------- FILE: alpha.bhv -------------
initialize RUN = false
initialize TaskNum=t1
initialize SendTask=false
//--------------模式判断------------------------
set MODE = ACTIVE{
RUN=true
} INACTIVE
set MODE = T1{
MODE=ACTIVE
TaskNum = t1
}
//----------路径点任务----------------------------
Behavior = BHV_Waypoint
{
name = waypt_survey
pwt = 100 //优先权重
condition = MODE==T1
//endflag = START=false
endflag = END_WayPoint=true
configflag = CRUISE_SPD = $[SPEED]
//configflag = OSPOS = $[OSX],$[OSY]
activeflag = INFO=$[OWNSHIP]
activeflag = INFO=$[BHVNAME]
activeflag = INFO=$[BHVTYPE]
//cycleflag = CINFO=$[OSX],$[OSY]
wptflag = CurrentPointComplete=true
wptflag = PREV=$(PX),$(PY)
wptflag = NEXT=$(NX),$(NY)
wptflag = TEST=$(X),$(Y)
wptflag = OSPOS=$(OSX),$(OSY)
//wptflag_on_start = true
updates = WPT_UPDATE
//perpetual = true
templating = spawn
// speed_alt = 1.2
//use_alt_speed = true
lead = 8
lead_damper = 1
lead_to_start = false
speed = 1 // meters per second
capture_line = true
capture_radius = 5.0
slip_radius = 15.0
efficiency_measure = all
polygon = 60,-40
order = normal
//repeat = 3
visual_hints = nextpt_color=yellow
visual_hints = nextpt_vertex_size=8
visual_hints = nextpt_lcolor=gray70
visual_hints = vertex_color=dodger_blue, edge_color=white
visual_hints = vertex_size=5, edge_size=1
}
//--------------定深任务------------------
Behavior=BHV_ConstantDepth
{
name = const_depth
pwt = 100
//condition = DEPLOY = true
condition = MODE==T1
duration = no-time-limit
updates = DEPTH_UPDATE
depth = 0
}
//--------------定向任务--------------------
Behavior=BHV_ConstantHeading
{
name = const_heading
pwt = 100
//condition = START_TURN = true
//condition = DEPLOY = true
condition = MODE==T3
perpetual = true
activeflag = TURN = started
//endflag = TURN = done
//endflag = RETURN = true
//endflag = START_TURN = false
endflag = START=false
heading = 225
complete_thresh = 5
duration = no-time-limit
}
//--------------定速任务--------------------
Behavior=BHV_ConstantSpeed
{
name = const_speed
pwt = 1000
condition = MODE==T1
perpetual = true
updates = SPEED_UPDATE
//endflag = START=false
speed = 5
duration = no-time-limit
//peakwidth = 0.5
//basewidth = 0.5
}
//----------------安全模式-----------------------
//----------------计时器---------------------
Behavior = BHV_Timer
{
name = mtime
condition = MODE==T1
pwt = 100
templating = spawn
//duration_status = MSTATUS
//duration = 10
endflag = TIME_OUT=true
updates = TIMER_UPDATES
//perpetual = true
}
//-------------最大深度限制--------------------------
Behavior = BHV_MaxDepth
{
name = maxdepth
pwt = 200
condition = MODE==ACTIVE
updates = MAXDEEP_UPDATES
max_depth = 20
tolerance = 0
duration = no-time-limit
}
//--------------安全区域设置-----------------------
Behavior = BHV_OpRegion
{
// General Behavior Parameters
// ---------------------------
name = op_region // example
pwt = 300 // default
condition = MODE==TN
updates = OPREGION_UPDATES // example
// Parameters specific to this behavior
// ------------------------------------
max_time = 20 // default (seconds)
max_depth = 25 // default (meters)
min_altitude = 0 // default (meters)
reset_var = OPREGION_RESET // example
trigger_entry_time = 1 // default (seconds)
trigger_exit_time = 0.5 // default (seconds)
polygon = pts={-80,-00:-30,-175:150,-100:95,25}
breached_altitude_flag = TaskFault = AltitudeOut
breached_depth_flag = TaskFault = DepthOut
breached_poly_flag = TaskFault = RegionOut
breached_time_flag = TaskFault = TimeOut
visual_hints = vertex_color = brown // default
visual_hints = vertex_size = 3 // default
visual_hints = edge_color = aqua // default
visual_hints = edge_size = 1 // default
}

269
src/pTaskManger/alpha.moos Normal file
View File

@@ -0,0 +1,269 @@
//-------------------------------------------------
// NAME: M. Benjamin, MIT CSAIL
// FILE: alpha.moos
//-------------------------------------------------
ServerHost = localhost
ServerPort = 9000
Community = pi
MOOSTimeWarp = 1
// Forest Lake
LatOrigin = 43.825300
LongOrigin = -70.330400
// MIT Sailing Pavilion (use this one)
// LatOrigin = 42.358456
// LongOrigin = -71.087589
//------------------------------------------
// Antler configuration block
ProcessConfig = ANTLER
{
MSBetweenLaunches = 200
Run = MOOSDB @ NewConsole = false
//Run = pLogger @ NewConsole = false
Run = uSimMarineV22 @ NewConsole = false
Run = pMarinePIDV22 @ NewConsole = false
Run = pHelmIvP @ NewConsole = false
Run = pMarineViewer @ NewConsole = false
Run = uProcessWatch @ NewConsole = false
Run = pNodeReporter @ NewConsole = false
Run = pRealm @ NewConsole = false
Run = pTaskManger @ NewConsole = false
//Run = uTimerScript @ NewConsole = false
}
//------------------------------------------
// pLogger config block
ProcessConfig = pTaskManger
{
AppTick = 8
CommsTick = 8
planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
}
ProcessConfig = pLogger
{
AppTick = 8
CommsTick = 8
AsyncLog = true
// For variables that are published in a bundle on their first post,
// explicitly declare their logging request
//Log = IVPHELM_LIFE_EVENT @ 0 NOSYNC
//Log = REPORT @ 0 NOSYNC
//Log = BHV_SETTINGS @ 0 NOSYNC
Log = OPREGION_RESET @ 0 NOSYNC
LogAuxSrc = true
WildCardLogging = true
WildCardOmitPattern = *_STATUS
WildCardOmitPattern = DB_VARSUMMARY
WildCardOmitPattern = DB_RWSUMMARY
WildCardExclusionLog = true
}
//------------------------------------------
// uProcessWatch config block
ProcessConfig = uProcessWatch
{
AppTick = 4
CommsTick = 4
watch_all = true
nowatch = uPokeDB*
nowatch = uQueryDB*
nowatch = uXMS*
nowatch = uMAC*
}
//------------------------------------------
// uSimMarineV22 config block
ProcessConfig = uSimMarineV22
{
AppTick = 4
CommsTick = 4
start_pos = x=0, y=-20, heading=180, speed=5
prefix = NAV
turn_rate = 40
thrust_map = 0:0, 20:1, 40:2, 60:3, 80:4, 100:5
//thrust_reflect = true
buoyancy_rate = 0.075
max_depth_rate = 5
max_depth_rate_speed = 2.0
default_water_depth = 400
}
//------------------------------------------
// pHelmIvP config block
ProcessConfig = pHelmIvP
{
AppTick = 4
CommsTick = 4
behaviors = alpha.bhv
domain = course:0:359:360
domain = speed:0:10:101
domain = depth:0:100:101
park_on_allstop = false
//park_on_allstop = true
}
//------------------------------------------
// pMarinePID config block
ProcessConfig = pMarinePIDV22
{
AppTick = 20
CommsTick = 20
verbose = true
depth_control = true
// SIM_INSTABILITY = 20
// Yaw PID controller
yaw_pid_kp = 1.2
yaw_pid_kd = 0.0
yaw_pid_ki = 0.3
yaw_pid_integral_limit = 0.07
// Speed PID controller
speed_pid_kp = 1.0
speed_pid_kd = 0.0
speed_pid_ki = 0.1
speed_pid_integral_limit = 0.07
maxpitch = 15
maxelevator = 13
pitch_pid_kp = 1.5
pitch_pid_kd = 0
pitch_pid_ki = 1.0
pitch_pid_integral_limit = 0
z_to_pitch_pid_kp = 0.12
z_to_pitch_pid_kd = 0
z_to_pitch_pid_ki = 0.004
z_to_pitch_pid_integral_limit = 0.05
//MAXIMUMS
maxrudder = 100
maxthrust = 100
// A non-zero SPEED_FACTOR overrides use of SPEED_PID
// Will set DESIRED_THRUST = DESIRED_SPEED * SPEED_FACTOR
speed_factor = 0
simulation = true
}
//------------------------------------------
// pMarineViewer config block
ProcessConfig = pMarineViewer
{
AppTick = 4
CommsTick = 4
tiff_file = forrest19.tif
//tiff_file = MIT_SP.tif
vehicles_name_mode = names+depth //+shortmode
set_pan_x = -90
set_pan_y = -280
zoom = 0.65
vehicle_shape_scale = 1.5
hash_delta = 50
hash_shade = 0.22
hash_viewable = true
trails_point_size = 1
//op_vertex = x=-83, y=-47, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=-46.4, y=-129.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=94.6, y=-62.2, lwidth=1,lcolor=yellow,looped=true,group=moa
//op_vertex = x=58, y=20, lwidth=1,lcolor=yellow,looped=true,group=moa
// Appcast configuration
appcast_height = 75
appcast_width = 30
appcast_viewable = true
appcast_color_scheme = indigo
nodes_font_size = xlarge
procs_font_size = xlarge
appcast_font_size = large
// datum_viewable = true
// datum_size = 18
// gui_size = small
// left_context[survey-point] = DEPLOY=true
// left_context[survey-point] = MOOS_MANUAL_OVERRIDE=false
// left_context[survey-point] = RETURN=false
right_context[return] = DEPLOY=true
right_context[return] = MOOS_MANUAL_OVERRIDE=false
right_context[return] = RETURN=false
scope = RETURN
scope = WPT_STAT
scope = VIEW_SEGLIST
scope = VIEW_POINT
scope = VIEW_POLYGON
scope = MVIEWER_LCLICK
scope = MVIEWER_RCLICK
//button_one = START # START=true
button_one = START # uMission_action_cmd={"taskName":"east_waypt_survey","action":"start"}
//button_one = MOOS_MANUAL_OVERRIDE=false
button_two = STOP # START=false
//button_two = MOOS_MANUAL_OVERRIDE=true
button_three = FaultClear # ClearFalut = true
button_four = SendSecurityZone # SendSaftRules = true
action = MENU_KEY=deploy # DEPLOY = true # RETURN = false
action+ = MENU_KEY=deploy # MOOS_MANUAL_OVERRIDE=false
action = RETURN=true
action = UPDATES_RETURN=speed=1.4
}
//------------------------------------------
// pNodeReporter config block
ProcessConfig = pNodeReporter
{
AppTick = 2
CommsTick = 2
//platform_type = kayak
//更改显示形状为uuv
platform_type = UUV
platform_color = red
platform_length = 4
}
ProcessConfig = uTimerScript
{
AppTick = 4
CommsTick = 4
condition = DEPLOY = true
randvar = varname = RND_DEPTH, min=20, max=80, key=at_reset
event = var = DEPTH_UPDATE, val=depth=$[RND_DEPTH], time=120
reset_max = nolimit reset_time = all-posted
}

37
src/pTaskManger/clean.sh Normal file
View File

@@ -0,0 +1,37 @@
#!/bin/bash
#--------------------------------------------------------------
# Script: clean.sh
# Author: Michael Benjamin
# Date: June 2020
#----------------------------------------------------------
# Part 1: Declare global var defaults
#----------------------------------------------------------
VERBOSE=""
#-------------------------------------------------------
# Part 2: Check for and handle command-line arguments
#-------------------------------------------------------
for ARGI; do
if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then
echo "clean.sh [SWITCHES] "
echo " --verbose "
echo " --help, -h "
exit 0;
elif [ "${ARGI}" = "--verbose" -o "${ARGI}" = "-v" ] ; then
VERBOSE="-v"
else
echo "clean.sh: Bad Arg:" $ARGI
exit 1
fi
done
#-------------------------------------------------------
# Part 2: Do the cleaning!
#-------------------------------------------------------
if [ "${VERBOSE}" = "-v" ]; then
echo "Cleaning: $PWD"
fi
rm -rf $VERBOSE MOOSLog_* XLOG_* LOG_*
rm -f $VERBOSE *~ *.moos++
rm -f $VERBOSE targ_*
rm -f $VERBOSE .LastOpenedMOOSLogDirectory

39
src/pTaskManger/launch.sh Normal file
View File

@@ -0,0 +1,39 @@
#!/bin/bash -e
#----------------------------------------------------------
# Script: launch.sh
# Author: Michael Benjamin
# LastEd: May 20th 2019
#----------------------------------------------------------
# Part 1: Set Exit actions and declare global var defaults
#----------------------------------------------------------
TIME_WARP=1
COMMUNITY="alpha"
GUI="yes"
#----------------------------------------------------------
# Part 2: Check for and handle command-line arguments
#----------------------------------------------------------
for ARGI; do
if [ "${ARGI}" = "--help" -o "${ARGI}" = "-h" ] ; then
echo "launch.sh [SWITCHES] [time_warp] "
echo " --help, -h Show this help message "
exit 0;
elif [ "${ARGI}" = "--nogui" ] ; then
GUI="no"
elif [ "${ARGI//[^0-9]/}" = "$ARGI" -a "$TIME_WARP" = 1 ]; then
TIME_WARP=$ARGI
else
echo "launch.sh Bad arg:" $ARGI " Exiting with code: 1"
exit 1
fi
done
#----------------------------------------------------------
# Part 3: Launch the processes
#----------------------------------------------------------
echo "Launching $COMMUNITY MOOS Community with WARP:" $TIME_WARP
pAntler $COMMUNITY.moos --MOOSTimeWarp=$TIME_WARP >& /dev/null &
uMAC -t $COMMUNITY.moos
kill -- -$$

12
src/pTaskManger/pTaskManger.moos Normal file
View File

@@ -0,0 +1,12 @@
// MOOS file
ServerHost = localhost
ServerPort = 9000
ProcessConfig = pTaskManger
{
AppTick = 10
CommsTick = 10
planConfigPath = /home/zjk/Desktop/project/moos-ivp-extend/PlanConfigure.json
}

38
src/pTaskSend/CMakeLists.txt Normal file
View File

@@ -0,0 +1,38 @@
#--------------------------------------------------------
# The CMakeLists.txt for: pTaskSend
# Author(s): zjk
#--------------------------------------------------------
FILE(GLOB SRC *.cpp)
SET(CMAKE_CXX_STANDARD 11)
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(pTaskSend ${SRC})
TARGET_LINK_LIBRARIES(pTaskSend
${MOOS_LIBRARIES}
${CMAKE_DL_LIBS}
${SYSTEM_LIBS}
${DUNE_LIB}
mbutil
m
pthread
fltk # Standard libraries used by this project's FLTK apps...
fltk_gl
dl
tiff
jsoncpp
# jsoncpp_lib_static
)

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

135
src/pTaskSend/TaskSend.h Normal file
View File

@@ -0,0 +1,135 @@
/*
* @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 09:09:54
* @FilePath: /moos-ivp-extend/src/pTaskSend/TaskSend.h
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
*/
/************************************************************/
/* NAME: zjk */
/* ORGN: MIT */
/* FILE: TaskSend.h */
/* DATE: */
/************************************************************/
#ifndef TaskSend_HEADER
#define TaskSend_HEADER
#include "MOOS/libMOOS/MOOSLib.h"
#include <DUNE/DUNE.hpp>
#include <FL/Fl.H>
#include <FL/Fl_Double_Window.H>
#include <FL/Fl_Button.H>
#include <FL/Fl_Check_Button.H>
#include <FL/Fl_Return_Button.H>
#include <FL/Fl_Group.H>
#include <FL/Fl_Text_Editor.H>
#include <FL/Fl_Radio_Round_Button.H>
#include <Fl/Fl_Output.H>
#include <Fl/Fl_Input.H>
#include <Fl/Fl_Menu_Button.H>
#include <Fl/Fl_Multiline_Input.H>
struct Landmark {
float lon;
float lat;
float depth;
float speed;
};
struct WayPointBehavior
{
std::string name;
std::string source;
int priority;
std::vector<Landmark> points;
float duration;
bool closedLoop;
float constSpeed;
int repeate;
bool perpetual;
float minDepth;
float maxDepth;
};
class TaskSend : public CMOOSApp
{
public:
TaskSend();
virtual ~TaskSend();
bool OnNewMail(MOOSMSG_LIST &NewMail);
bool Iterate();
bool OnConnectToServer();
bool OnStartUp();
void RegisterVariables();
protected:
// insert local vars here
Fl_Window * taskEditView();
inline int sendTaskCallback(Fl_Widget *w);
inline int sendTask1Callback(Fl_Widget *w);
inline int sendTask2Callback(Fl_Widget *w);
inline int ModifyTask1Callback(Fl_Widget *w);
inline int FeedbackTaskCallback(Fl_Widget *w);
static void st_sendTask1Callback(Fl_Widget *w, void *f) { ((TaskSend *)f)->sendTask1Callback(w);}
static void st_sendTask2Callback(Fl_Widget *w, void *f) { ((TaskSend *)f)->sendTask2Callback(w);}
static void st_ModifyTask1Callback(Fl_Widget *w, void *f) { ((TaskSend *)f)->ModifyTask1Callback(w);}
static void st_FeedbackTaskCallback(Fl_Widget *w, void *f) { ((TaskSend *)f)->FeedbackTaskCallback(w);}
int SecurityZoneEdit();
// static void st_taskTypeCallback(Fl_Widget *w, void *f) { ((TaskSend *)f)->sendTaskCallback(w);}
// int SecurityZoneEdit();
protected:
typedef struct task
{
std::string type;
std::string duration;//时间设置 no-time-limit / 10/20/300...
std::string pwt;
} task;
typedef struct waypointTask
{
task taskParam;
std::string capture_radius;//捕获半径
std::string capture_line;//捕获线
std::string slip_radius; //滑移半径
std::string lead; //number 引导点距离
std::string lead_damper; //与轨迹的距离
std::string lead_to_start="true";
std::string speed="12";
std::string repeat;
std::string polygon;
} waypointTask;
private:
CMOOSMsg current_msg;
bool error=false;
bool editView=false;
Fl_Output* disp1=nullptr;
Fl_Window* view=nullptr;
Fl_Input* capture_radius = new Fl_Input(280,175+30,100,30,"capture radius");
Fl_Input* capture_line = new Fl_Input(80,175+30,100,30,"capture line");
Fl_Input* slip_radius = new Fl_Input(280,175+30*2,100,30,"slip radius");
Fl_Input* lead = new Fl_Input(80,175+30*2,100,30,"lead");
Fl_Input* lead_damper = new Fl_Input(280,175+30*3,100,30,"lead damper");
Fl_Input* speed = new Fl_Input(80,175+30*3,100,30,"speed");
Fl_Input* repeat = new Fl_Input(80,175+30*4,100,30,"repeat");
Fl_Multiline_Input* polygon = new Fl_Multiline_Input(80,175+30*5,300,100,"polygon");
std::string SetPlan1(std::string sourceName, double stamp);
std::string SetPlan2(std::string sourceName, double stamp);
std::string ModifyPlan1(std::string sourceName, double stamp);
DUNE::Network::TCPSocket sock_tcp_send;
DUNE::Network::UDPSocket sock_udp_receive;
bool tcpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port);
double getTimeStamp();
};
#endif

50
src/pTaskSend/TaskSendMain.cpp Normal file
View File

@@ -0,0 +1,50 @@
/*
* @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: 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
* @LastEditTime: 2023-09-21 22:03:48
* @FilePath: /moos-ivp-extend/src/pTaskSend/TaskSendMain.cpp
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
*/
/************************************************************/
/* NAME: zjk */
/* ORGN: MIT */
/* FILE: TaskSendMain.cpp */
/* DATE: */
/************************************************************/
#include <string>
#include "TaskSend.h"
#include "display.h"
// #include <FL/Fl.H>
// #include <FL/Fl_Window.H>
// #include <FL/Fl_Box.H>
using namespace std;
int main(int argc, char *argv[])
{
// default parameters file
string sMissionFile = "TaskSend.moos";
//under what name shoud the application register with the MOOSDB?
string sMOOSName = "pTaskSend";
switch(argc)
{
case 3:
//command line says don't register with default name
sMOOSName = argv[2];
case 2:
//command line says don't use default config file
sMissionFile = argv[1];
}
//make an application
TaskSend TaskSendApp;
//run it
TaskSendApp.Run(sMOOSName.c_str(), sMissionFile.c_str());
return(0);
}

1
src/pTaskSend/build/.cmake/api/v1/query/client-vscode/query.json Normal file
View File

@@ -0,0 +1 @@
{"requests":[{"kind":"cache","version":2},{"kind":"codemodel","version":2},{"kind":"toolchains","version":1},{"kind":"cmakeFiles","version":1}]}

1235
src/pTaskSend/build/.cmake/api/v1/reply/cache-v2-d6d942c4138e4121aadf.json Normal file
View File

File diff suppressed because it is too large Load Diff

754
src/pTaskSend/build/.cmake/api/v1/reply/cmakeFiles-v1-1b864ed9ef170e655086.json Normal file
View File

@@ -0,0 +1,754 @@
{
"inputs" :
[
{
"path" : "CMakeLists.txt"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineSystem.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeSystem.cmake.in"
},
{
"isGenerated" : true,
"path" : "build/CMakeFiles/3.25.1/CMakeSystem.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeUnixFindMake.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeSystemSpecificInitialize.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompilerId.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCompilerIdDetection.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/ADSP-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/ARMCC-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/ARMClang-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/AppleClang-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompilerInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Borland-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Bruce-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompilerInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Compaq-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Cray-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Embarcadero-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Fujitsu-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/FujitsuClang-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GHS-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/HP-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IAR-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMClang-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Intel-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IntelLLVM-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/LCC-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/MSVC-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/NVHPC-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/NVIDIA-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/OpenWatcom-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/PGI-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/PathScale-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/SCO-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/SDCC-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/SunPro-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/TI-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Tasking-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/TinyCC-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/VisualAge-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-C-DetermineVersionInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Watcom-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/XL-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-C-DetermineVersionInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/XLClang-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/zOS-C-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-C-DetermineVersionInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeFindBinUtils.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-FindBinUtils.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCCompiler.cmake.in"
},
{
"isGenerated" : true,
"path" : "build/CMakeFiles/3.25.1/CMakeCCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCXXCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-Determine-CXX.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompilerId.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCompilerIdDetection.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/ADSP-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/ARMCC-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/ARMClang-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/AppleClang-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompilerInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Borland-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Clang-DetermineCompilerInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Comeau-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Compaq-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Cray-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Embarcadero-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Fujitsu-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/FujitsuClang-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GHS-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/HP-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IAR-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMClang-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Intel-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IntelLLVM-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/LCC-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/MSVC-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/NVHPC-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/NVIDIA-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/OpenWatcom-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/PGI-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/PathScale-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/SCO-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/SunPro-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/TI-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Tasking-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/VisualAge-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-CXX-DetermineVersionInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/Watcom-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/XL-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-CXX-DetermineVersionInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/XLClang-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/zOS-CXX-DetermineCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/IBMCPP-CXX-DetermineVersionInternal.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeFindBinUtils.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-FindBinUtils.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCXXCompiler.cmake.in"
},
{
"isGenerated" : true,
"path" : "build/CMakeFiles/3.25.1/CMakeCXXCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeSystemSpecificInformation.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeGenericSystem.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeInitializeConfigs.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Platform/UnixPaths.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCInformation.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeLanguageInformation.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-C.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/CMakeCommonCompilerMacros.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-GNU-C.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-GNU.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCommonLanguageInclude.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCompilerCommon.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompilerABI.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeParseImplicitIncludeInfo.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeParseImplicitLinkInfo.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeParseLibraryArchitecture.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCompilerCommon.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCCompilerABI.c"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompileFeatures.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Internal/FeatureTesting.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCCompiler.cmake.in"
},
{
"isGenerated" : true,
"path" : "build/CMakeFiles/3.25.1/CMakeCCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCXXInformation.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeLanguageInformation.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU-CXX.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Compiler/GNU.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-GNU-CXX.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Platform/Linux-GNU.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCommonLanguageInclude.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCXXCompiler.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCompilerCommon.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompilerABI.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeParseImplicitIncludeInfo.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeParseImplicitLinkInfo.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeParseLibraryArchitecture.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeTestCompilerCommon.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCXXCompilerABI.cpp"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeDetermineCompileFeatures.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/Internal/FeatureTesting.cmake"
},
{
"isCMake" : true,
"isExternal" : true,
"path" : "/usr/share/cmake-3.25/Modules/CMakeCXXCompiler.cmake.in"
},
{
"isGenerated" : true,
"path" : "build/CMakeFiles/3.25.1/CMakeCXXCompiler.cmake"
}
],
"kind" : "cmakeFiles",
"paths" :
{
"build" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend/build",
"source" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend"
},
"version" :
{
"major" : 1,
"minor" : 0
}
}

56
src/pTaskSend/build/.cmake/api/v1/reply/codemodel-v2-8cafde00da2f94adfddf.json Normal file
View File

@@ -0,0 +1,56 @@
{
"configurations" :
[
{
"directories" :
[
{
"build" : ".",
"jsonFile" : "directory-.-Debug-f5ebdc15457944623624.json",
"projectIndex" : 0,
"source" : ".",
"targetIndexes" :
[
0
]
}
],
"name" : "Debug",
"projects" :
[
{
"directoryIndexes" :
[
0
],
"name" : "Project",
"targetIndexes" :
[
0
]
}
],
"targets" :
[
{
"directoryIndex" : 0,
"id" : "pTaskSend::@6890427a1f51a3e7e1df",
"jsonFile" : "target-pTaskSend-Debug-fe070126a6f1fe44e26b.json",
"name" : "pTaskSend",
"projectIndex" : 0
}
]
}
],
"kind" : "codemodel",
"paths" :
{
"build" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend/build",
"source" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend"
},
"version" :
{
"major" : 2,
"minor" : 4
}
}

14
src/pTaskSend/build/.cmake/api/v1/reply/directory-.-Debug-f5ebdc15457944623624.json Normal file
View File

@@ -0,0 +1,14 @@
{
"backtraceGraph" :
{
"commands" : [],
"files" : [],
"nodes" : []
},
"installers" : [],
"paths" :
{
"build" : ".",
"source" : "."
}
}

132
src/pTaskSend/build/.cmake/api/v1/reply/index-2023-09-21T07-09-11-0928.json Normal file
View File

@@ -0,0 +1,132 @@
{
"cmake" :
{
"generator" :
{
"multiConfig" : false,
"name" : "Unix Makefiles"
},
"paths" :
{
"cmake" : "/usr/bin/cmake",
"cpack" : "/usr/bin/cpack",
"ctest" : "/usr/bin/ctest",
"root" : "/usr/share/cmake-3.25"
},
"version" :
{
"isDirty" : false,
"major" : 3,
"minor" : 25,
"patch" : 1,
"string" : "3.25.1",
"suffix" : ""
}
},
"objects" :
[
{
"jsonFile" : "codemodel-v2-8cafde00da2f94adfddf.json",
"kind" : "codemodel",
"version" :
{
"major" : 2,
"minor" : 4
}
},
{
"jsonFile" : "cache-v2-d6d942c4138e4121aadf.json",
"kind" : "cache",
"version" :
{
"major" : 2,
"minor" : 0
}
},
{
"jsonFile" : "cmakeFiles-v1-1b864ed9ef170e655086.json",
"kind" : "cmakeFiles",
"version" :
{
"major" : 1,
"minor" : 0
}
},
{
"jsonFile" : "toolchains-v1-2781238ec0fa0b9996f9.json",
"kind" : "toolchains",
"version" :
{
"major" : 1,
"minor" : 0
}
}
],
"reply" :
{
"client-vscode" :
{
"query.json" :
{
"requests" :
[
{
"kind" : "cache",
"version" : 2
},
{
"kind" : "codemodel",
"version" : 2
},
{
"kind" : "toolchains",
"version" : 1
},
{
"kind" : "cmakeFiles",
"version" : 1
}
],
"responses" :
[
{
"jsonFile" : "cache-v2-d6d942c4138e4121aadf.json",
"kind" : "cache",
"version" :
{
"major" : 2,
"minor" : 0
}
},
{
"jsonFile" : "codemodel-v2-8cafde00da2f94adfddf.json",
"kind" : "codemodel",
"version" :
{
"major" : 2,
"minor" : 4
}
},
{
"jsonFile" : "toolchains-v1-2781238ec0fa0b9996f9.json",
"kind" : "toolchains",
"version" :
{
"major" : 1,
"minor" : 0
}
},
{
"jsonFile" : "cmakeFiles-v1-1b864ed9ef170e655086.json",
"kind" : "cmakeFiles",
"version" :
{
"major" : 1,
"minor" : 0
}
}
]
}
}
}
}

129
src/pTaskSend/build/.cmake/api/v1/reply/target-pTaskSend-Debug-fe070126a6f1fe44e26b.json Normal file
View File

@@ -0,0 +1,129 @@
{
"artifacts" :
[
{
"path" : "pTaskSend"
}
],
"backtrace" : 1,
"backtraceGraph" :
{
"commands" :
[
"ADD_EXECUTABLE",
"TARGET_LINK_LIBRARIES"
],
"files" :
[
"CMakeLists.txt"
],
"nodes" :
[
{
"file" : 0
},
{
"command" : 0,
"file" : 0,
"line" : 8,
"parent" : 0
},
{
"command" : 1,
"file" : 0,
"line" : 10,
"parent" : 0
}
]
},
"compileGroups" :
[
{
"compileCommandFragments" :
[
{
"fragment" : "-g"
}
],
"language" : "CXX",
"sourceIndexes" :
[
0,
1,
2
]
}
],
"id" : "pTaskSend::@6890427a1f51a3e7e1df",
"link" :
{
"commandFragments" :
[
{
"fragment" : "-g",
"role" : "flags"
},
{
"fragment" : "-rdynamic",
"role" : "flags"
},
{
"backtrace" : 2,
"fragment" : "-lmbutil",
"role" : "libraries"
},
{
"backtrace" : 2,
"fragment" : "-lm",
"role" : "libraries"
},
{
"backtrace" : 2,
"fragment" : "-lpthread",
"role" : "libraries"
}
],
"language" : "CXX"
},
"name" : "pTaskSend",
"nameOnDisk" : "pTaskSend",
"paths" :
{
"build" : ".",
"source" : "."
},
"sourceGroups" :
[
{
"name" : "Source Files",
"sourceIndexes" :
[
0,
1,
2
]
}
],
"sources" :
[
{
"backtrace" : 1,
"compileGroupIndex" : 0,
"path" : "TaskSend.cpp",
"sourceGroupIndex" : 0
},
{
"backtrace" : 1,
"compileGroupIndex" : 0,
"path" : "TaskSendMain.cpp",
"sourceGroupIndex" : 0
},
{
"backtrace" : 1,
"compileGroupIndex" : 0,
"path" : "display.cpp",
"sourceGroupIndex" : 0
}
],
"type" : "EXECUTABLE"
}

107
src/pTaskSend/build/.cmake/api/v1/reply/toolchains-v1-2781238ec0fa0b9996f9.json Normal file
View File

@@ -0,0 +1,107 @@
{
"kind" : "toolchains",
"toolchains" :
[
{
"compiler" :
{
"id" : "GNU",
"implicit" :
{
"includeDirectories" :
[
"/usr/lib/gcc/x86_64-linux-gnu/9/include",
"/usr/local/include",
"/usr/include/x86_64-linux-gnu",
"/usr/include"
],
"linkDirectories" :
[
"/usr/lib/gcc/x86_64-linux-gnu/9",
"/usr/lib/x86_64-linux-gnu",
"/usr/lib",
"/lib/x86_64-linux-gnu",
"/lib"
],
"linkFrameworkDirectories" : [],
"linkLibraries" :
[
"gcc",
"gcc_s",
"c",
"gcc",
"gcc_s"
]
},
"path" : "/usr/bin/gcc",
"version" : "9.4.0"
},
"language" : "C",
"sourceFileExtensions" :
[
"c",
"m"
]
},
{
"compiler" :
{
"id" : "GNU",
"implicit" :
{
"includeDirectories" :
[
"/usr/include/c++/9",
"/usr/include/x86_64-linux-gnu/c++/9",
"/usr/include/c++/9/backward",
"/usr/lib/gcc/x86_64-linux-gnu/9/include",
"/usr/local/include",
"/usr/include/x86_64-linux-gnu",
"/usr/include"
],
"linkDirectories" :
[
"/usr/lib/gcc/x86_64-linux-gnu/9",
"/usr/lib/x86_64-linux-gnu",
"/usr/lib",
"/lib/x86_64-linux-gnu",
"/lib"
],
"linkFrameworkDirectories" : [],
"linkLibraries" :
[
"stdc++",
"m",
"gcc_s",
"gcc",
"c",
"gcc_s",
"gcc"
]
},
"path" : "/usr/bin/g++",
"version" : "9.4.0"
},
"language" : "CXX",
"sourceFileExtensions" :
[
"C",
"M",
"c++",
"cc",
"cpp",
"cxx",
"mm",
"mpp",
"CPP",
"ixx",
"cppm"
]
}
],
"version" :
{
"major" : 1,
"minor" : 0
}
}

58
src/pTaskSend/display.cpp Normal file
View File

@@ -0,0 +1,58 @@
/*
* @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 12:50:21
* @LastEditors: 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
* @LastEditTime: 2023-09-21 22:04:13
* @FilePath: /moos-ivp-extend/src/pTaskSend/display.cpp
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
*/
#include "display.h"
display::display(/* args */)
{
// Fl_Window *window = new Fl_Window(340, 180, "hello");
// Fl_Box *box = new Fl_Box(20, 40, 300, 100, "Hello");
// box->box(FL_UP_BOX);
// box->labelfont(FL_BOLD + FL_ITALIC);
// box->labelsize(36);
// box->labeltype(FL_SHADOW_LABEL);
// window->end();
// window->show();
// Fl::run();
TaskEdit();
}
int display::TaskEdit()
{
Fl_Window *window = new Fl_Window(450, 350, "hello");
// Fl_Double_Window *window(450, 350, "Simple Table"); /* 1. 创建一个窗口 */
Fl_Group* pGroup = new Fl_Group(50, 50, 400, 150); /* 2. 创建一个分组 */
Fl_Button* pButton = new Fl_Button(70, 50, 150, 30, "Fl_Button"); /* 3. 创建控件 */
int xyz;
pButton->callback(sendTask, &xyz);
Fl_Check_Button* pChkButton = new Fl_Check_Button(230, 50, 150, 30, "Fl_Check_Button");
Fl_Return_Button* pRetButton = new Fl_Return_Button(70, 100, 150, 30, "Fl_Return_Button");
Fl_Radio_Round_Button* pRndButton = new Fl_Radio_Round_Button(230, 100, 150, 30, "Fl_Round_Button");
pGroup->end(); /* 4. 结束上个容器的创建 */
Fl_Text_Editor* pText = new Fl_Text_Editor(50, 150, 350, 150);
Fl_Text_Buffer* pBuff = new Fl_Text_Buffer();
pText->buffer(pBuff); /* pBuff->text()中的内容就是Fl_Text_Buffer中显示的内容 */
pBuff->text("示例文字");
pText->end();
window->end(); /* 4. 结束上个容器的创建 */
window->show(); /* 5. 显示窗口 */
Fl::run(); /* 6. 运行FLTK主循环 */
// return true;
}
void display::sendTask(Fl_Widget *w, void *data)
{
}
display::~display()
{
}

39
src/pTaskSend/display.h Normal file
View File

@@ -0,0 +1,39 @@
/*
* @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 12:50:12
* @LastEditors: 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
* @LastEditTime: 2023-09-21 21:45:28
* @FilePath: /moos-ivp-extend/src/pTaskSend/display.h
* @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
*/
#ifndef DISPLAY_HEADER
#define DISPLAY_HEADER
#include <FL/Fl.H>
#include <FL/Fl_Double_Window.H>
#include <FL/Fl_Button.H>
#include <FL/Fl_Check_Button.H>
#include <FL/Fl_Return_Button.H>
#include <FL/Fl_Group.H>
#include <FL/Fl_Text_Editor.H>
#include <FL/Fl_Radio_Round_Button.H>
#include "MOOS/libMOOS/App/MOOSApp.h"
class display
{
private:
/* data */
public:
display(/* args */);
~display();
int TaskEdit();
int SecurityZoneEdit();
protected:
static void sendTask(Fl_Widget *w, void *data);
};
#endif

11
src/pTaskSend/pTaskSend.moos Normal file
View File

@@ -0,0 +1,11 @@
// MOOS file
ServerHost = localhost
ServerPort = 9000
ProcessConfig = pTaskSend
{
AppTick = 4
CommsTick = 4
}