diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e01c098 --- /dev/null +++ b/CMakeLists.txt @@ -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 ) diff --git a/PlanConfigure.json b/PlanConfigure.json new file mode 100644 index 0000000..fdc25d4 --- /dev/null +++ b/PlanConfigure.json @@ -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" + } +} diff --git a/README b/README new file mode 100644 index 0000000..1220cf3 --- /dev/null +++ b/README @@ -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 +############################################################################## + diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..e96a58a --- /dev/null +++ b/build.sh @@ -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} + + diff --git a/clean.sh b/clean.sh new file mode 100755 index 0000000..a1892b1 --- /dev/null +++ b/clean.sh @@ -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 {} \; + diff --git a/launch/alpha.bhv b/launch/alpha.bhv new file mode 100644 index 0000000..7a727f1 --- /dev/null +++ b/launch/alpha.bhv @@ -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 + } \ No newline at end of file diff --git a/launch/launch.moos b/launch/launch.moos new file mode 100644 index 0000000..2ff2039 --- /dev/null +++ b/launch/launch.moos @@ -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 +} diff --git a/missions/alder/README b/missions/alder/README new file mode 100644 index 0000000..3a49d99 --- /dev/null +++ b/missions/alder/README @@ -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 + + diff --git a/missions/alder/alder.bhv b/missions/alder/alder.bhv new file mode 100644 index 0000000..08f2ddc --- /dev/null +++ b/missions/alder/alder.bhv @@ -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 +} + diff --git a/missions/alder/alder.moos b/missions/alder/alder.moos new file mode 100644 index 0000000..0675010 --- /dev/null +++ b/missions/alder/alder.moos @@ -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 +} \ No newline at end of file diff --git a/missions/alder/alder_orig.bhv b/missions/alder/alder_orig.bhv new file mode 100644 index 0000000..fcd9b0a --- /dev/null +++ b/missions/alder/alder_orig.bhv @@ -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 +} + diff --git a/missions/s3_alpha/alpha.bhv b/missions/s3_alpha/alpha.bhv new file mode 100755 index 0000000..b5c8d4c --- /dev/null +++ b/missions/s3_alpha/alpha.bhv @@ -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 + } + diff --git a/missions/s3_alpha/alpha.moos b/missions/s3_alpha/alpha.moos new file mode 100755 index 0000000..a06d443 --- /dev/null +++ b/missions/s3_alpha/alpha.moos @@ -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 +} diff --git a/missions/s3_alpha/clean.sh b/missions/s3_alpha/clean.sh new file mode 100755 index 0000000..a7464c1 --- /dev/null +++ b/missions/s3_alpha/clean.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +rm -rf MOOSLog_* +rm -f *~ +rm -f *.moos++ +rm -f .LastOpenedMOOSLogDirectory diff --git a/missions/s3_alpha/launch.sh b/missions/s3_alpha/launch.sh new file mode 100755 index 0000000..7510b07 --- /dev/null +++ b/missions/s3_alpha/launch.sh @@ -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" + diff --git a/missions/xrelay/xrelay.moos b/missions/xrelay/xrelay.moos new file mode 100644 index 0000000..b53b39a --- /dev/null +++ b/missions/xrelay/xrelay.moos @@ -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 +} + diff --git a/scripts/README b/scripts/README new file mode 100644 index 0000000..a4a75ee --- /dev/null +++ b/scripts/README @@ -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. + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..81bb93f --- /dev/null +++ b/src/CMakeLists.txt @@ -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 +############################################################################## + diff --git a/src/pBoardSupportComm/BoardSupportComm.cpp b/src/pBoardSupportComm/BoardSupportComm.cpp new file mode 100644 index 0000000..44ee2d0 --- /dev/null +++ b/src/pBoardSupportComm/BoardSupportComm.cpp @@ -0,0 +1,785 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: BoardSupportComm.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#include "BoardSupportComm.h" +#include + + +#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 reference = {estimatedState.referenceLon, estimatedState.referenceLat, estimatedState.referenceAltitude}; + std::vector current = {currentLon, currentLat, currentAlt}; + std::vector 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 +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 +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 init_lla, + std::vector point_lla, + std::vector& 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 init_lla, + std::vector point_enu, + std::vector &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 init_lla, + std::vector point_lla, + std::vector& 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 init_lla, + std::vector point_ned, + std::vector &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)); +} diff --git a/src/pBoardSupportComm/BoardSupportComm.h b/src/pBoardSupportComm/BoardSupportComm.h new file mode 100644 index 0000000..8cba0f9 --- /dev/null +++ b/src/pBoardSupportComm/BoardSupportComm.h @@ -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 +#include +#include +#include +#include + +#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 init_lla, + std::vector point_lla, + std::vector& point_enu); + void ConvertENUToLLA(std::vector init_lla, + std::vector point_enu, + std::vector &point_lla); + void ConvertLLAToNED(std::vector init_lla, + std::vector point_lla, + std::vector& point_ned); + void ConvertNEDToLLA(std::vector init_lla, + std::vector point_ned, + std::vector &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 inline uint16_t deserialize(Type& t, const uint8_t* bfr, uint16_t& length); + template 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 diff --git a/src/pBoardSupportComm/BoardSupportComm_Info.cpp b/src/pBoardSupportComm/BoardSupportComm_Info.cpp new file mode 100644 index 0000000..97e3fd3 --- /dev/null +++ b/src/pBoardSupportComm/BoardSupportComm_Info.cpp @@ -0,0 +1,115 @@ +/****************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT Cambridge MA */ +/* FILE: BoardSupportComm_Info.cpp */ +/* DATE: Dec 29th 1963 */ +/****************************************************************/ + +#include +#include +#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","= "); + 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); +} + diff --git a/src/pBoardSupportComm/BoardSupportComm_Info.h b/src/pBoardSupportComm/BoardSupportComm_Info.h new file mode 100644 index 0000000..a070f8b --- /dev/null +++ b/src/pBoardSupportComm/BoardSupportComm_Info.h @@ -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 + diff --git a/src/pBoardSupportComm/CMakeLists.txt b/src/pBoardSupportComm/CMakeLists.txt new file mode 100644 index 0000000..49e749b --- /dev/null +++ b/src/pBoardSupportComm/CMakeLists.txt @@ -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} +) + + diff --git a/src/pBoardSupportComm/main.cpp b/src/pBoardSupportComm/main.cpp new file mode 100644 index 0000000..13bc6f5 --- /dev/null +++ b/src/pBoardSupportComm/main.cpp @@ -0,0 +1,53 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: main.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#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 +#include + +#include +#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 +#include +#include +#include +#include +#include +#include +#include +#include // IWYU pragma: export +#include // IWYU pragma: export +#include +// @@protoc_insertion_point(includes) +#include +#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( + &_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(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 + 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 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( + &_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(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 + 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 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 +inline PROTOBUF_ALWAYS_INLINE +void Coordinate::set_name(ArgT0&& arg0, ArgT... args) { + + _impl_.name_.Set(static_cast(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 +inline PROTOBUF_ALWAYS_INLINE +void WayPointBehavior::set_name(ArgT0&& arg0, ArgT... args) { + + _impl_.name_.Set(static_cast(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 +#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_Behavior_2eproto diff --git a/src/pClientViewer/CMakeLists.txt b/src/pClientViewer/CMakeLists.txt new file mode 100644 index 0000000..053b119 --- /dev/null +++ b/src/pClientViewer/CMakeLists.txt @@ -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} +) + + diff --git a/src/pClientViewer/ClientViewer.cpp b/src/pClientViewer/ClientViewer.cpp new file mode 100644 index 0000000..db6f14b --- /dev/null +++ b/src/pClientViewer/ClientViewer.cpp @@ -0,0 +1,812 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: ClientViewer.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#include "ClientViewer.h" +//#include "Behavior.pb.h" +#include + +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(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(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(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(message); + printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); + DUNE::IMC::MessageList::const_iterator iter1 = msgList->msgs.begin(); + for (; iter1 != msgList->msgs.end(); ++iter1) + { + DUNE::IMC::EntityParameters *entityParameter = static_cast(*iter1); + DUNE::IMC::MessageList::const_iterator iter2 = entityParameter->params.begin(); + for (; iter2 != entityParameter->params.end(); ++iter2) + { + DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); + std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; + } + } + } + if (type == DUNE::IMC::EstimatedState::getIdStatic()) + { + DUNE::IMC::EstimatedState * msg = dynamic_cast(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(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(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(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(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(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(message); + printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); + DUNE::IMC::MessageList::const_iterator iter1 = msgList->msgs.begin(); + for (; iter1 != msgList->msgs.end(); ++iter1) + { + DUNE::IMC::EntityParameters *entityParameter = static_cast(*iter1); + DUNE::IMC::MessageList::const_iterator iter2 = entityParameter->params.begin(); + for (; iter2 != entityParameter->params.end(); ++iter2) + { + DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); + std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; + } + } + } + else if (type == DUNE::IMC::EstimatedState::getIdStatic()) + { + DUNE::IMC::EstimatedState * msg = dynamic_cast(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(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(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; +} diff --git a/src/pClientViewer/ClientViewer.h b/src/pClientViewer/ClientViewer.h new file mode 100644 index 0000000..8824694 --- /dev/null +++ b/src/pClientViewer/ClientViewer.h @@ -0,0 +1,100 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: ClientViewer.h */ +/* DATE: */ +/************************************************************/ + +#ifndef ClientViewer_HEADER +#define ClientViewer_HEADER + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MOOS/libMOOS/MOOSLib.h" +#include "MOOS/libMOOS/Utils/MOOSThread.h" +#include "MOOS/libMOOS/Utils/PeriodicEvent.h" +#include +// #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 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 diff --git a/src/pClientViewer/ClientViewer_Info.cpp b/src/pClientViewer/ClientViewer_Info.cpp new file mode 100644 index 0000000..eefb35b --- /dev/null +++ b/src/pClientViewer/ClientViewer_Info.cpp @@ -0,0 +1,115 @@ +/****************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT Cambridge MA */ +/* FILE: ClientViewer_Info.cpp */ +/* DATE: Dec 29th 1963 */ +/****************************************************************/ + +#include +#include +#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","= "); + 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); +} + diff --git a/src/pClientViewer/ClientViewer_Info.h b/src/pClientViewer/ClientViewer_Info.h new file mode 100644 index 0000000..e1a6839 --- /dev/null +++ b/src/pClientViewer/ClientViewer_Info.h @@ -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 + diff --git a/src/pClientViewer/main.cpp b/src/pClientViewer/main.cpp new file mode 100644 index 0000000..a259fa3 --- /dev/null +++ b/src/pClientViewer/main.cpp @@ -0,0 +1,52 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: main.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#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 +#include "MBUtils.h" +#include "DataManagement.h" +//#include "NavigationInfo.pb.h" +#include + +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 reader(builder.newCharReader()); + if (!reader->parse(estimatedStateString.c_str(), + estimatedStateString.c_str() + static_cast(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 sParams; + m_MissionReader.EnableVerbatimQuoting(false); + if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { + list::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; +} \ No newline at end of file diff --git a/src/pDataManagement/DataManagement.h b/src/pDataManagement/DataManagement.h new file mode 100644 index 0000000..80b588e --- /dev/null +++ b/src/pDataManagement/DataManagement.h @@ -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 diff --git a/src/pDataManagement/DataManagement_Info.cpp b/src/pDataManagement/DataManagement_Info.cpp new file mode 100644 index 0000000..e17d141 --- /dev/null +++ b/src/pDataManagement/DataManagement_Info.cpp @@ -0,0 +1,115 @@ +/****************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT Cambridge MA */ +/* FILE: DataManagement_Info.cpp */ +/* DATE: Dec 29th 1963 */ +/****************************************************************/ + +#include +#include +#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","= "); + 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); +} + diff --git a/src/pDataManagement/DataManagement_Info.h b/src/pDataManagement/DataManagement_Info.h new file mode 100644 index 0000000..2a85845 --- /dev/null +++ b/src/pDataManagement/DataManagement_Info.h @@ -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 + diff --git a/src/pDataManagement/NavigationInfo.pb.h b/src/pDataManagement/NavigationInfo.pb.h new file mode 100644 index 0000000..b7e4a5d --- /dev/null +++ b/src/pDataManagement/NavigationInfo.pb.h @@ -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 +#include + +#include +#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 +#include +#include +#include +#include +#include +#include +#include +#include // IWYU pragma: export +#include // IWYU pragma: export +#include +// @@protoc_insertion_point(includes) +#include +#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( + &_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(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 + 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 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 +inline PROTOBUF_ALWAYS_INLINE +void EstimatedState::set_info(ArgT0&& arg0, ArgT... args) { + + _impl_.info_.Set(static_cast(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 +#endif // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_NavigationInfo_2eproto diff --git a/src/pDataManagement/main.cpp b/src/pDataManagement/main.cpp new file mode 100644 index 0000000..280c603 --- /dev/null +++ b/src/pDataManagement/main.cpp @@ -0,0 +1,58 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: main.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "ColorParse.h" +#include "DataManagement.h" +#include "DataManagement_Info.h" + +// #include +// #include + +using namespace std; + +int main(int argc, char *argv[]) +{ + string mission_file; + string run_command = argv[0]; + + for(int i=1; i +#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; ip_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; iiRecieveMessage(receiveBuf,receiveBufSize); + if(iRx) + { + for(int i=0; i +#include +#include "VarDataPair.h" +#include "MBUtils.h" +#include "AngleUtils.h" +#include +#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 \ No newline at end of file diff --git a/src/pEmulator/_150server.cpp b/src/pEmulator/_150server.cpp new file mode 100644 index 0000000..ec89abf --- /dev/null +++ b/src/pEmulator/_150server.cpp @@ -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); +} \ No newline at end of file diff --git a/src/pEmulator/_150server.hpp b/src/pEmulator/_150server.hpp new file mode 100644 index 0000000..c12d1c5 --- /dev/null +++ b/src/pEmulator/_150server.hpp @@ -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 +#include +#include +#include +#include +#include +#include + +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 \ No newline at end of file diff --git a/src/pEmulator/a.moos b/src/pEmulator/a.moos new file mode 100644 index 0000000..b5cb516 --- /dev/null +++ b/src/pEmulator/a.moos @@ -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 +} diff --git a/src/pEmulator/alpha.bhv b/src/pEmulator/alpha.bhv new file mode 100644 index 0000000..7a727f1 --- /dev/null +++ b/src/pEmulator/alpha.bhv @@ -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 + } \ No newline at end of file diff --git a/src/pEmulator/main.cpp b/src/pEmulator/main.cpp new file mode 100644 index 0000000..19f964e --- /dev/null +++ b/src/pEmulator/main.cpp @@ -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 +using namespace std; +#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" +// #include "VarDataPair.h" +#include"Emulator.hpp" +#include +#include +#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 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 Controler::setConfigParams(std::list) +{ + list unhandled_params; + list::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; +} \ No newline at end of file diff --git a/src/pMotionControler/Controler.hpp b/src/pMotionControler/Controler.hpp new file mode 100644 index 0000000..7aee711 --- /dev/null +++ b/src/pMotionControler/Controler.hpp @@ -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 +#include +#include "VarDataPair.h" +#include "MBUtils.h" +#include "AngleUtils.h" +#include +#include "json/json.h" +#include +#include + +// #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 getPostings() {return(postings);} + void clearPostings() {postings.clear();} + bool ClearValTim(vlaTim &a); + list setConfigParams(std::list); //使用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 getConfigParams(){return config_params;} + vector getConfigErrors(){return config_errors;} + vector getConfigInfo(){return config_info;} + vector 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 postings; + unsigned int iterations; + + double tardy_helm_thresh; + double tardy_nav_thresh; + double const_thrust; + double dead_zone; + + + + double is_rad = false; + + list config_params; + vector config_errors; + vector config_info; + vector runMsg; + Json::Value RepList; +}; + +#endif \ No newline at end of file diff --git a/src/pMotionControler/MotionControler.cpp b/src/pMotionControler/MotionControler.cpp new file mode 100644 index 0000000..fbc7d91 --- /dev/null +++ b/src/pMotionControler/MotionControler.cpp @@ -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 m_postings = pengine.getPostings(); + for(unsigned int i=0; i +#include"pidControl.hpp" +#include +// #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 \ No newline at end of file diff --git a/src/pMotionControler/a.moos b/src/pMotionControler/a.moos new file mode 100644 index 0000000..f3aea08 --- /dev/null +++ b/src/pMotionControler/a.moos @@ -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 +} \ No newline at end of file diff --git a/src/pMotionControler/alpha.bhv b/src/pMotionControler/alpha.bhv new file mode 100644 index 0000000..65b258e --- /dev/null +++ b/src/pMotionControler/alpha.bhv @@ -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 + } \ No newline at end of file diff --git a/src/pMotionControler/alpha.moos b/src/pMotionControler/alpha.moos new file mode 100644 index 0000000..1d506b7 --- /dev/null +++ b/src/pMotionControler/alpha.moos @@ -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 +} \ No newline at end of file diff --git a/src/pMotionControler/main.cpp b/src/pMotionControler/main.cpp new file mode 100644 index 0000000..ab9c11e --- /dev/null +++ b/src/pMotionControler/main.cpp @@ -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 +using namespace std; +#include"MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" +// #include "VarDataPair.h" +#include"MotionControler.hpp" +#include +#include +#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::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; +} \ No newline at end of file diff --git a/src/pMotionControler/pidControl.hpp b/src/pMotionControler/pidControl.hpp new file mode 100644 index 0000000..67254c5 --- /dev/null +++ b/src/pMotionControler/pidControl.hpp @@ -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 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 \ No newline at end of file diff --git a/src/pMotionControler/simMat.moos b/src/pMotionControler/simMat.moos new file mode 100644 index 0000000..f345908 --- /dev/null +++ b/src/pMotionControler/simMat.moos @@ -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 +} \ No newline at end of file diff --git a/src/pSurfaceSupportComm/CMakeLists.txt b/src/pSurfaceSupportComm/CMakeLists.txt new file mode 100644 index 0000000..aa52235 --- /dev/null +++ b/src/pSurfaceSupportComm/CMakeLists.txt @@ -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} +) + diff --git a/src/pSurfaceSupportComm/PeriodicTCPEvent.cpp b/src/pSurfaceSupportComm/PeriodicTCPEvent.cpp new file mode 100644 index 0000000..7900753 --- /dev/null +++ b/src/pSurfaceSupportComm/PeriodicTCPEvent.cpp @@ -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< (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 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); +// } \ No newline at end of file diff --git a/src/pSurfaceSupportComm/PeriodicTCPEvent.h b/src/pSurfaceSupportComm/PeriodicTCPEvent.h new file mode 100644 index 0000000..0743328 --- /dev/null +++ b/src/pSurfaceSupportComm/PeriodicTCPEvent.h @@ -0,0 +1,39 @@ +#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h" +#include "MOOS/libMOOS/Utils/ThreadPriority.h" + +#include "MOOS/libMOOS/Utils/MOOSThread.h" + +#include + + +#include +#include +#include +#include + +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_; + + +}; \ No newline at end of file diff --git a/src/pSurfaceSupportComm/PeriodicUDPEvent.cpp b/src/pSurfaceSupportComm/PeriodicUDPEvent.cpp new file mode 100644 index 0000000..b7bd04a --- /dev/null +++ b/src/pSurfaceSupportComm/PeriodicUDPEvent.cpp @@ -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< (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 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); +} \ No newline at end of file diff --git a/src/pSurfaceSupportComm/PeriodicUDPEvent.h b/src/pSurfaceSupportComm/PeriodicUDPEvent.h new file mode 100644 index 0000000..a56458f --- /dev/null +++ b/src/pSurfaceSupportComm/PeriodicUDPEvent.h @@ -0,0 +1,42 @@ +#include "MOOS/libMOOS/Utils/MOOSUtilityFunctions.h" +#include "MOOS/libMOOS/Utils/ThreadPriority.h" + +#include "MOOS/libMOOS/Utils/MOOSThread.h" + +#include + + +#include +#include +#include +#include + +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_; + + +}; \ No newline at end of file diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.cpp b/src/pSurfaceSupportComm/SurfaceSupportComm.cpp new file mode 100644 index 0000000..a11e121 --- /dev/null +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.cpp @@ -0,0 +1,1118 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: SurfaceSupportComm.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#include "MBUtils.h" +#include "SurfaceSupportComm.h" +#include +#include + +#include + +using namespace std; + +#define UDP_SEND_PORT 6001 +#define TCP_RECEIVE_PORT 8000 + +// #define DEST_IP_ADDRESS "10.25.0.230" //树莓派 +// #define DEST_IP_ADDRESS "127.0.0.1" +#define DEST_IP_ADDRESS "10.25.0.163" +//#define DEST_IP_ADDRESS "10.25.0.160" + +//--------------------------------------------------------- +// Constructor +SurfaceSupportComm::SurfaceSupportComm() +{ + header_src = 0x14; + header_src_ent = 62; + header_dst = 0; + header_dst_ent = 255; + + 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; + +} + +//--------------------------------------------------------- +// Destructor +SurfaceSupportComm::~SurfaceSupportComm() +{ +} + + +// Procedure: OnNewMail +bool SurfaceSupportComm::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(); + + if(key == "uDevice_monitor_fb") + { + std::string estimatedStateString = msg.GetString(); + std::string err; + Json::Value estimatedStateData; + std::istringstream iss(estimatedStateString); + Json::CharReaderBuilder builder; + bool parsingResult = Json::parseFromStream(builder, iss, &estimatedStateData, &err); + if (!parsingResult) { + std::cerr << "Failed to parse JSON string." << std::endl; + return -1; + } + + estimatedState.currentLon = estimatedStateData["currentLon"].asFloat(); + estimatedState.currentLat = estimatedStateData["currentLat"].asFloat(); + estimatedState.currentAltitude = estimatedStateData["currentAltitude"].asFloat(); + estimatedState.referenceLon = estimatedStateData["referenceLon"].asFloat(); + estimatedState.referenceLat = estimatedStateData["referenceLat"].asFloat(); + estimatedState.referenceAltitude = estimatedStateData["referenceAltitude"].asFloat(); + estimatedState.offsetNorth = estimatedStateData["north"].asFloat(); + estimatedState.offsetEast = estimatedStateData["east"].asFloat(); + estimatedState.offsetDown = estimatedStateData["depth"].asFloat(); + estimatedState.roll = estimatedStateData["roll"].asFloat(); + estimatedState.pitch = estimatedStateData["pitch"].asFloat(); + estimatedState.yaw = estimatedStateData["yaw"].asFloat(); //弧度 + estimatedState.linearVelocityNorth = estimatedStateData["dvlVX"].asFloat(); + estimatedState.linearVelocityEast = estimatedStateData["dvlVY"].asFloat(); + estimatedState.linearVelocityDown = estimatedStateData["dvlVZ"].asFloat(); + estimatedState.height = estimatedStateData["height"].asFloat(); + estimatedState.depth = estimatedStateData["depth"].asFloat(); + + deviceStatus.batteryVoltage = estimatedStateData["batteryVoltage"].asUInt(); + deviceStatus.batteryLevel = estimatedStateData["batteryLevel"].asUInt(); + deviceStatus.batteryTemp = estimatedStateData["batteryTemp"].asFloat(); + deviceStatus.thrustRpm = estimatedStateData["rpm"].asInt(); + deviceStatus.lightEnable = estimatedStateData["lightEnable"].asUInt(); + deviceStatus.throwingLoadEnable = estimatedStateData["throwingLoadEnable"].asUInt(); + deviceStatus.dvlStatus = estimatedStateData["dvlStatus"].asUInt(); + } + if(key == "uMission_task_fb") + { + missionStatusString = msg.GetString(); + } + + } + + return(true); +} + +//--------------------------------------------------------- +// Procedure: OnConnectToServer +bool SurfaceSupportComm::OnConnectToServer() +{ + RegisterVariables(); + return(true); +} + +bool SurfaceSupportComm::udpSendToServer(DUNE::IMC::Message * msg, std::string addr, int port) +{ + + DUNE::Utils::ByteBuffer bb; + try { + DUNE::IMC::Packet::serialize(msg, bb); + return sock_udp_send.write(bb.getBuffer(), msg->getSerializationSize(), + DUNE::Network::Address(addr.c_str()), port); + } + 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; +} + +bool SurfaceSupportComm::Iterate() +{ + // AppCastingMOOSApp::Iterate(); +#if 1 + DUNE::IMC::Announce announceMsg; + announceMsg.setTimeStamp(); + announceMsg.setSource(header_src); + announceMsg.setSourceEntity(header_src_ent); + announceMsg.setDestination(header_dst); + announceMsg.setDestinationEntity(header_dst_ent); + announceMsg.sys_name.assign("lauv-150"); + announceMsg.sys_type = 2; + // announceMsg.owner = 65535; + announceMsg.lat = estimatedState.currentLat * M_PI / 180; + announceMsg.lon = estimatedState.currentLon * M_PI / 180; + announceMsg.height = estimatedState.depth; + // announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+udp://127.0.0.1:6001/;imc+tcp://127.0.0.1:8000/"); + announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+tcp://10.25.0.230:8000/"); + //announceMsg.services.assign("dune://0.0.0.0/version/2022.04.0;imc+info://0.0.0.0/version/5.4.30-8be592a;imc+udp://10.25.0.160:6001/;imc+tcp://10.25.0.160:8000/"); +#endif + + +#if 1 + DUNE::IMC::PlanControlState planControlStateMsg; + planControlStateMsg.setTimeStamp(); + planControlStateMsg.setSource(header_src); + planControlStateMsg.setSourceEntity(header_src_ent); + planControlStateMsg.setDestination(header_dst); + planControlStateMsg.setDestinationEntity(header_dst_ent); + + DUNE::IMC::VehicleState vehicleStateMsg; //用于上报故障信息或正常状态 + vehicleStateMsg.setTimeStamp(); + vehicleStateMsg.setSource(header_src); + vehicleStateMsg.setSourceEntity(header_src_ent); + vehicleStateMsg.setDestination(header_dst); + vehicleStateMsg.setDestinationEntity(header_dst_ent); + + if (!missionStatusString.empty()) + { + std::string err; + Json::Value missionStatusObject; + std::istringstream iss(missionStatusString); + Json::CharReaderBuilder builder; + bool parsingResult = Json::parseFromStream(builder, iss, &missionStatusObject, &err); + if (!parsingResult) + { + std::cerr << "Failed to parse JSON string." << std::endl; + return -1; + } + int planExecutingStatus = missionStatusObject["state"].asInt(); + planControlStateMsg.state = planExecutingStatus; + planControlStateMsg.plan_id = missionStatusObject["taskName"].asString(); //子任务名称，对应PlanDB info的[taskName] + planControlStateMsg.plan_eta = -1; //缺省值 + planControlStateMsg.plan_progress = -1; //缺省值 + planControlStateMsg.man_id = missionStatusObject["destName"].asString(); //对应PlanDB info的[points][name] + planControlStateMsg.man_type = 450; //未知 + planControlStateMsg.man_eta = -1; //缺省值 + planControlStateMsg.last_outcome = missionStatusObject["errorCode"].asUInt(); + + if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_BLOCKED) + { + //故障状态 + vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_ERROR; + } + else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_READY) + { + //没有在执行任务，等待执行 + vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE; + } + else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_INITIALIZING) + { + //手操状态 + vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_EXTERNAL; + } + else if (planControlStateMsg.state == DUNE::IMC::PlanControlState::StateEnum::PCS_EXECUTING) + { + //正在执行任务 + vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_MANEUVER; + } + else + { + vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE; + } + vehicleStateMsg.error_count = 0; + vehicleStateMsg.error_ents = ""; + vehicleStateMsg.maneuver_type = 0xffff; + vehicleStateMsg.maneuver_stime = -1; + vehicleStateMsg.maneuver_eta = 65535; + vehicleStateMsg.control_loops = DUNE::IMC::CLoopsMask::CL_NONE; + vehicleStateMsg.flags = 0; + vehicleStateMsg.last_error = ""; + vehicleStateMsg.last_error_time = -1; + } + else + { + planControlStateMsg.state = DUNE::IMC::PlanControlState::StateEnum::PCS_READY; + planControlStateMsg.plan_id = ""; //子任务名称，对应PlanDB info的[taskName] + planControlStateMsg.plan_eta = -1; //缺省值 + planControlStateMsg.plan_progress = -1; //缺省值 + planControlStateMsg.man_id = ""; //对应PlanDB info的[points][name] + planControlStateMsg.man_type = 450; //未知 + planControlStateMsg.man_eta = -1; //缺省值 + planControlStateMsg.last_outcome = DUNE::IMC::PlanControlState::LastPlanOutcomeEnum::LPO_NONE; + + vehicleStateMsg.op_mode = DUNE::IMC::VehicleState::OperationModeEnum::VS_SERVICE; + vehicleStateMsg.error_count = 0; + vehicleStateMsg.error_ents = ""; + vehicleStateMsg.maneuver_type = 0xffff; + vehicleStateMsg.maneuver_stime = -1; + vehicleStateMsg.maneuver_eta = 65535; + vehicleStateMsg.control_loops = DUNE::IMC::CLoopsMask::CL_NONE; + vehicleStateMsg.flags = 0; + vehicleStateMsg.last_error = ""; + vehicleStateMsg.last_error_time = -1; + } + + + +#endif + +#if 1 + DUNE::IMC::MsgList equipmentMsgList; + equipmentMsgList.setTimeStamp(); + equipmentMsgList.setSource(header_src); + equipmentMsgList.setSourceEntity(header_src_ent); + equipmentMsgList.setDestination(header_dst); + equipmentMsgList.setDestinationEntity(header_dst_ent); + retrieveEntityStatus(equipmentMsgList); +#endif + +#if 1 + DUNE::IMC::EstimatedState estimatedStateMsg; + estimatedStateMsg.setTimeStamp(); + estimatedStateMsg.setSource(header_src); + estimatedStateMsg.setSourceEntity(header_src_ent); + estimatedStateMsg.setDestination(header_dst); + estimatedStateMsg.setDestinationEntity(header_dst_ent); + estimatedStateMsg.lat = estimatedState.currentLat * M_PI / 180; + estimatedStateMsg.lon = estimatedState.currentLon * M_PI / 180; + estimatedStateMsg.depth = estimatedState.depth; + estimatedStateMsg.height = estimatedState.height; + // estimatedStateMsg.phi = estimatedState.roll * M_PI / 180; //x axis + // estimatedStateMsg.theta = estimatedState.pitch * M_PI / 180; //y axis + // estimatedStateMsg.psi = estimatedState.yaw * M_PI / 180; //z axis + estimatedStateMsg.phi = estimatedState.roll; //x axis + estimatedStateMsg.theta = estimatedState.pitch; //y axis + //estimatedStateMsg.psi = estimatedState.yaw; //z axis + + float yawTemp = estimatedState.yaw; + if ((yawTemp >= 0) && (yawTemp <= M_PI)) + { + estimatedStateMsg.psi = yawTemp; + std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI + << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl; + } + + if ((yawTemp > M_PI) && (yawTemp <= 2 * M_PI)) + { + estimatedStateMsg.psi = -1 * (2 * M_PI - yawTemp); + std::cout << "before: (" << estimatedState.yaw << "," << estimatedState.yaw * 180 / M_PI + << "), after:" << estimatedStateMsg.psi << ", " << estimatedStateMsg.psi * 180 / M_PI << std::endl; + } + + estimatedStateMsg.vx = estimatedState.linearVelocityNorth; //x axis + estimatedStateMsg.vy = estimatedState.linearVelocityEast; //y axis + estimatedStateMsg.vz = estimatedState.linearVelocityDown; //z axis +#endif + + + + double TimeScheduled = TimeLast + 0.5; + // double TimeScheduled = TimeLast + 1.0; + int MillisecondsSleep = static_cast( ( TimeScheduled-MOOS::Time() )*1000.0 ); + if(MillisecondsSleep>0) + { + MOOS::Pause(MillisecondsSleep); + } + sendCount++; + + if (sendCount % 2 == 0) + { + // udpEvent.Push(static_cast(&announceMsg)); + udpSendToServer(&announceMsg, DEST_IP_ADDRESS, UDP_SEND_PORT); + sendCount = 0; + } + // udpEvent.Push(static_cast(&planControlStateMsg)); + // udpEvent.Push(static_cast(&equipmentMsgList)); + // udpEvent.Push(static_cast(&estimatedStateMsg)); + // udpEvent.Push(static_cast(&vehicleStateMsg)); + udpSendToServer(&planControlStateMsg, DEST_IP_ADDRESS, UDP_SEND_PORT); + udpSendToServer(&equipmentMsgList, DEST_IP_ADDRESS, UDP_SEND_PORT); + udpSendToServer(&estimatedStateMsg, DEST_IP_ADDRESS, UDP_SEND_PORT); + udpSendToServer(&vehicleStateMsg, DEST_IP_ADDRESS, UDP_SEND_PORT); + + double TimeNow = MOOS::Time(); + TimeLast = TimeNow; + +#if 1 + tcpReceiveFromClient(recv_buf, sizeof(recv_buf)/sizeof(char), 0.005); +#endif + + // AppCastingMOOSApp::PostReport(); + + return(true); +} + +int SurfaceSupportComm::simulateGetEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, std::string value) +{ + int result = -1; + + DUNE::IMC::EntityParameter subMsg; + subMsg.name = name; + subMsg.value = value; + entityParameter.params.push_back(subMsg); + + result = 0; + return result; +} + +double dfMean = 0; +double dfVar = 0; +unsigned int N = 0; + +bool periodicCallback(double TimeNow,double TimeLastRun,double TimeScheduled, void * /*pParamCaller*/) +{ + //lets wait a while + MOOSPause(2); + + //some stats + double dfE = TimeNow-TimeScheduled; + N=N+1; + dfMean = dfMean*(N-1)/N+(dfE)/N; + + if(N>1) + dfVar = dfE/(N-1)+(N-1)/N*dfVar; + + std::cout.setf(std::ios::fixed); + std::cout<getId(); +// #if 1 +// if (type == DUNE::IMC::Announce::getIdStatic()) +// { +// DUNE::IMC::Announce * msg = dynamic_cast(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(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(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(message); +// printf("server receive %s: %lf\n", msgList->getName(), msgList->getTimeStamp()); +// DUNE::IMC::MessageList::const_iterator iter1 = msgList->msgs.begin(); +// for (; iter1 != msgList->msgs.end(); ++iter1) +// { +// DUNE::IMC::EntityParameters *entityParameter = static_cast(*iter1); +// DUNE::IMC::MessageList::const_iterator iter2 = entityParameter->params.begin(); +// for (; iter2 != entityParameter->params.end(); ++iter2) +// { +// DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter2); +// std::cout << entityParameter->name << ": " << subEntityParameter->name << ", " << subEntityParameter->value << std::endl; +// } +// } +// } +// if (type == DUNE::IMC::EstimatedState::getIdStatic()) +// { +// DUNE::IMC::EstimatedState * msg = dynamic_cast(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(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(message); +// printf("server receive %s: %lf, %u,\n", +// msg->getName(), msg->getTimeStamp(), msg->type, msg->plan_id.c_str()); +// } +// #endif +// return true; +// } + +#if 0 +int SurfaceSupportComm::retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList) +{ + int result = -1; + + DUNE::IMC::EntityParameters paramBattery; + paramBattery.name = "Battery"; + simulateGetEntityStatus(paramBattery, "Voltage", "3"); + simulateGetEntityStatus(paramBattery, "FuelLevel", "5"); + simulateGetEntityStatus(paramBattery, "Temp", "7"); + equipmentMsgList.msgs.push_back(paramBattery); + + DUNE::IMC::EntityParameters paramController; + paramController.name = "Controller"; + simulateGetEntityStatus(paramController, "Temp", "9"); + equipmentMsgList.msgs.push_back(paramController); + + DUNE::IMC::EntityParameters paramThrust; + paramThrust.name = "Thrust"; + simulateGetEntityStatus(paramThrust, "Rpm", "11"); + equipmentMsgList.msgs.push_back(paramThrust); + + DUNE::IMC::EntityParameters paramDVL; + paramDVL.name = "DVL"; + simulateGetEntityStatus(paramDVL, "Status", "1"); //取值范围{0, 1} + equipmentMsgList.msgs.push_back(paramDVL); + + DUNE::IMC::EntityParameters paramIndicatorLight; + paramIndicatorLight.name = "IndicatorLight"; + simulateGetEntityStatus(paramIndicatorLight, "Status", "1"); //取值范围{0, 1} + equipmentMsgList.msgs.push_back(paramIndicatorLight); + + DUNE::IMC::EntityParameters paramThrowingLoad; + paramThrowingLoad.name = "ThrowingLoad"; + simulateGetEntityStatus(paramThrowingLoad, "Status", "1"); //取值范围{0, 1} + equipmentMsgList.msgs.push_back(paramThrowingLoad); + + result = 0; + return result; +} +#endif + +int SurfaceSupportComm::retrieveEntityStatus(DUNE::IMC::MsgList& equipmentMsgList) +{ + int result = -1; + + DUNE::IMC::EntityParameters paramBattery; + paramBattery.name = "Battery"; + getEntityStatus(paramBattery, "Voltage", deviceStatus.batteryVoltage); + getEntityStatus(paramBattery, "FuelLevel", deviceStatus.batteryLevel); + getEntityStatus(paramBattery, "Temp", deviceStatus.batteryTemp); + equipmentMsgList.msgs.push_back(paramBattery); + + // DUNE::IMC::EntityParameters paramController; + // paramController.name = "Controller"; + // getEntityStatus(paramController, "Temp", deviceStatus.controllerTemp); + // equipmentMsgList.msgs.push_back(paramController); + + DUNE::IMC::EntityParameters paramThrust; + paramThrust.name = "Thrust"; + getEntityStatus(paramThrust, "Rpm", deviceStatus.thrustRpm); + equipmentMsgList.msgs.push_back(paramThrust); + + DUNE::IMC::EntityParameters paramDVL; + paramDVL.name = "DVL"; + getEntityStatus(paramDVL, "Status", deviceStatus.dvlStatus); //取值范围{0, 1} + equipmentMsgList.msgs.push_back(paramDVL); + + DUNE::IMC::EntityParameters paramIndicatorLight; + paramIndicatorLight.name = "IndicatorLight"; + getEntityStatus(paramIndicatorLight, "Status", deviceStatus.lightEnable); //取值范围{0, 1} + equipmentMsgList.msgs.push_back(paramIndicatorLight); + + DUNE::IMC::EntityParameters paramThrowingLoad; + paramThrowingLoad.name = "ThrowingLoad"; + getEntityStatus(paramThrowingLoad, "Status", deviceStatus.throwingLoadEnable ); //取值范围{0, 1} + equipmentMsgList.msgs.push_back(paramThrowingLoad); + + result = 0; + return result; +} + +template +inline int SurfaceSupportComm::getEntityStatus(DUNE::IMC::EntityParameters& entityParameter, std::string name, Type& value) +{ + int result = -1; + + DUNE::IMC::EntityParameter subMsg; + subMsg.name = name; + std::stringstream ss; + ss << value; + subMsg.value = ss.str(); + ss.clear(); + ss.str(""); + entityParameter.params.push_back(subMsg); + + result = 0; + return result; + +} + +void SurfaceSupportComm::tcpReceiveFromClient(char* buf, unsigned int cap, double timeout) +{ + // Poll for connections and client data + if (!m_poll.poll(timeout)) + return; + + // Check for new clients. + if (m_poll.wasTriggered(sock_tcp_receive)) + acceptNewClient(); + + // Check for client data + handleClients(buf, cap); +} + +void SurfaceSupportComm::tcpSendToClient(char* buf, unsigned int cap) +{ + ClientList::iterator itr = m_clients.begin(); + + while (itr != m_clients.end()) + { + try + { + itr->socket->write(buf, cap); + } + catch (std::runtime_error& e) + { + closeConnection(*itr, e); + itr = m_clients.erase(itr); + continue; + } + ++itr; + } +} + +void SurfaceSupportComm::acceptNewClient(void) +{ + Client c; + c.socket = 0; + try + { + c.socket = sock_tcp_receive.accept(&c.address, &c.port); + c.socket->setKeepAlive(true); + c.socket->setNoDelay(true); + c.socket->setReceiveTimeout(5); + c.socket->setSendTimeout(5); + m_poll.add(*c.socket); + m_clients.push_back(c); + } + catch (std::runtime_error& e) + { + if (c.socket) + delete c.socket; + } +} + +void SurfaceSupportComm::handleClients(char* buf, unsigned int cap) +{ + // Check for new data from clients. + ClientList::iterator itr = m_clients.begin(); + while (itr != m_clients.end()) + { + if (!m_poll.wasTriggered(*itr->socket)) + { + ++itr; + continue; + } + + int n; + + try + { + n = itr->socket->read(buf, cap); + } + catch (std::runtime_error& e) + { + closeConnection(*itr, e); + itr = m_clients.erase(itr); + continue; + } + + if (n > 0) + { + DUNE::IMC::Message * m = DUNE::IMC::Packet::deserialize((uint8_t *)buf, cap); + processMessage(m); + free(m); + } + ++itr; + } +} + +void SurfaceSupportComm::processMessage(DUNE::IMC::Message * message) +{ + int type = message->getId(); + + if (type == DUNE::IMC::PlanDB::getIdStatic()) + { + DUNE::IMC::PlanDB * msg = dynamic_cast(message); + printf("server receive %s: %lf, %u, %u, %s\n", \ + msg->getName(), msg->getTimeStamp(), msg->type, msg->op, msg->plan_id.c_str()); + if (msg->type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_SET) + { + std::string behaviorSpecString = msg->info; + std::cout << "BHV_Waypoint: " << behaviorSpecString << std::endl; + std::string err; + Json::Value behaviorSpecData; + std::istringstream iss(behaviorSpecString); + Json::CharReaderBuilder builder; + bool parsingResult = Json::parseFromStream(builder, iss, &behaviorSpecData, &err); + if (!parsingResult) { + std::cerr << "Failed to parse JSON string." << std::endl; + return; + } + + behaviorSpecData["boardStamp"] = MOOS::Time(); + BatchConvertCoordinate(behaviorSpecData); + + std::string queryMemberName = behaviorSpecData["taskName"].asString(); + std::cout << queryMemberName << std::endl; + struct stat info; + if (stat(planConfigPath.c_str(), &info) != 0) + { + std::ofstream ofs; + ofs.open(planConfigPath, std::ios::out); + Json::StreamWriterBuilder builder; + Json::Value saveJsonValue; + saveJsonValue[queryMemberName] = behaviorSpecData; + const std::string json_file = Json::writeString(builder, saveJsonValue); + std::cout << json_file << std::endl; + ofs << json_file << std::endl; + ofs.close(); + } + else + { + std::ifstream ifs; + ifs.open(planConfigPath, std::ios::in); + Json::Reader taskConfigureReader; + Json::Value tempJsonValue; + taskConfigureReader.parse(ifs, tempJsonValue); + ifs.close(); + + std::ofstream ofs; + ofs.open(planConfigPath, std::ios::out); + Json::StreamWriterBuilder builder; + tempJsonValue[queryMemberName] = behaviorSpecData; + const std::string json_file = Json::writeString(builder, tempJsonValue); + std::cout << json_file << std::endl; + ofs << json_file << std::endl; + ofs.close(); + } + } + if (msg->type == DUNE::IMC::PlanDB::DBT_REQUEST && msg->op == DUNE::IMC::PlanDB::DBOP_GET_STATE) + { + std::cout << "DUNE::IMC::PlanDB::DBT_REQUEST" << std::endl; + std::ifstream ifs; + ifs.open(planConfigPath, std::ios::in); + Json::Reader taskConfigureReader; + Json::Value inputJsonValue; + taskConfigureReader.parse(ifs, inputJsonValue); + ifs.close(); + + int taskCount = inputJsonValue.size(); + std::vector taskList = inputJsonValue.getMemberNames(); + Json::Value outputJsonValue; + for (int i=0; i(message); + printf("server receive %s: %lf\n", msg->getName(), msg->getTimeStamp()); + DUNE::IMC::MessageList::const_iterator iter = msg->params.begin(); + for(; iter < msg->params.end(); iter++) + { + DUNE::IMC::EntityParameter *subEntityParameter = static_cast(*iter); + + std::string err; + + Json::Value parameterValue; + std::istringstream iss(subEntityParameter->value); + Json::CharReaderBuilder builder; + bool parsingResult = Json::parseFromStream(builder, iss, ¶meterValue, &err); + if (!parsingResult) { + std::cerr << "Failed to parse JSON string." << std::endl; + return; + } + + std::string parentName = msg->name; + std::string childName = subEntityParameter->name; + std::string dataType = parameterValue["type"].asString(); + std::string dataValueTemp = parameterValue["value"].asString(); + if (dataType == "float") + { + std::stringstream ss(dataValueTemp); + float dataValue; + ss >> dataValue; + printf("%s, %s: %s, %f\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else if (dataType == "bool") + { + std::stringstream ss(dataValueTemp); + bool dataValue; + ss >> dataValue; + printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else if (dataType == "int") + { + std::stringstream ss(dataValueTemp); + int dataValue; + ss >> dataValue; + printf("%s, %s: %s, %d\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue); + } + else + { + std::stringstream ss(dataValueTemp); + std::string dataValue; + ss >> dataValue; + printf("%s, %s: %s, %s\n", parentName.c_str(), childName.c_str(), dataType.c_str(), dataValue.c_str()); + } + } + } + if (type == DUNE::IMC::PlanControl::getIdStatic()) + { + DUNE::IMC::PlanControl * msg = dynamic_cast(message); + printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->plan_id.c_str()); + if (msg->type == DUNE::IMC::PlanControl::TypeEnum::PC_REQUEST) + { + std::string action = ""; + if (msg->op == DUNE::IMC::PlanControl::OperationEnum::PC_START) + { + action = "start"; + } + if (msg->op == DUNE::IMC::PlanControl::OperationEnum::PC_STOP) + { + action = "stop"; + } + + Json::Value outputTaskValue; + outputTaskValue["taskName"] = msg->plan_id; + outputTaskValue["action"] = action; + Json::StreamWriterBuilder builder2; + std::string outputTaskString = Json::writeString(builder2, outputTaskValue); + std::cout << outputTaskString << std::endl; + if (action == "start") + { + std::ifstream ifs; + ifs.open(planConfigPath, std::ios::in); + Json::Reader taskConfigureReader; + Json::Value tempJsonValue; + taskConfigureReader.parse(ifs, tempJsonValue); + ifs.close(); + float originAlt = tempJsonValue[msg->plan_id]["origin"]["altitude"].asFloat(); + float originLat = tempJsonValue[msg->plan_id]["origin"]["lat"].asFloat(); + float originLon = tempJsonValue[msg->plan_id]["origin"]["lon"].asFloat(); + Json::Value outputOrginValue; + outputOrginValue["altitude"] = originAlt; + outputOrginValue["lat"] = originLat; + outputOrginValue["lon"] = originLon; + Json::StreamWriterBuilder builder1; + std::string outputOriginString = Json::writeString(builder1, outputOrginValue); + std::cout << outputOriginString << std::endl; + Notify("uMission_origin_cmd", outputOriginString); + MOOS::Pause(2000); + Notify("uMission_action_cmd", outputTaskString); + } + if (action == "stop") + { + Notify("uMission_action_cmd", outputTaskString); + } + + } + } + if (type == DUNE::IMC::VehicleCommand::getIdStatic()) + { + DUNE::IMC::VehicleCommand * msg = dynamic_cast(message); + printf("server receive %s: %lf, %u, %u\n", msg->getName(), msg->getTimeStamp(), msg->type, msg->command); + if (msg->type == DUNE::IMC::VehicleCommand::TypeEnum::VC_REQUEST) + { + if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_EXEC_MANEUVER) + { + Notify("uManual_enable_cmd", (double)msg->command); + } + if (msg->command == DUNE::IMC::VehicleCommand::CommandEnum::VC_STOP_MANEUVER) + { + Notify("uManual_enable_cmd", (double)msg->command); + } + } + } + if (type == DUNE::IMC::RemoteActions::getIdStatic()) + { + DUNE::IMC::RemoteActions * msg = dynamic_cast(message); + printf("server receive %s: %lf, %s\n", msg->getName(), msg->getTimeStamp(), msg->actions); + Notify("uManual_drive_cmd", msg->actions); + } +} + +void SurfaceSupportComm::ParsePlanConfig(Json::Value inputJsonValue) +{ + int taskCount = inputJsonValue.size(); + std::vector taskList = inputJsonValue.getMemberNames(); + std::cout << "taskCount: " << taskCount << std::endl; + double lastChangeStamp = 0; + std::string lastChangeSourceAddress; + std::string lastChangeSourceName; + for (int i=0; i lastChangeStamp) + {Json::Value inputJsonValue; + lastChangeStamp = boardStamp; + lastChangeSourceAddress = sourceAddress; + lastChangeSourceName = sourceName; + } + + std::cout << "taskName: " << taskName << std::endl; + std::cout << "taskId: " << taskId << std::endl; + std::cout << "originLon: " << originLon << std::endl; + std::cout << "originLat: " << originLat << std::endl; + std::cout << "originAlt: " << originAlt << std::endl; + std::cout << "sourceName: " << sourceName << std::endl; + std::cout << "sourceAddress: " << sourceAddress << std::endl; + std::cout << "boardStamp: " << boardStamp << std::endl; + std::cout << "clientStamp: " << clientStamp << std::endl; + std::cout << "priority: " << priority << std::endl; + std::cout << "duration: " << duration << std::endl; + std::cout << "closedLoop: " << closedLoop << std::endl; + std::cout << "constSpeed: " << constSpeed << std::endl; + std::cout << "repeat: " << repeat << std::endl; + std::cout << "perpetual: " << perpetual << std::endl; + std::cout << "minDepth: " << minDepth << std::endl; + std::cout << "maxDepth: " << maxDepth << std::endl; + + if (subTask["origin"].isArray()) + { + Json::Value array = subTask["origin"]; + if (array.size() == 3) + { + float origin_lon = array[0].asFloat(); + float origin_lat = array[1].asFloat(); + float origin_alt = array[2].asFloat(); + std::cout << "origin: [" << origin_lon << ", " + << origin_lat << ", " << origin_alt << "] " << std::endl; + } + } + + if (subTask["points"].isArray()) + { + Json::Value array = subTask["points"]; + for (int j = 0; j < array.size(); j++) + { + std::string name = array[j]["name"].asString(); + float lon = array[j]["lon"].asFloat(); + float lat = array[j]["lat"].asFloat(); + float depth = array[j]["depth"].asFloat(); + float speed = array[j]["speed"].asFloat(); + float north = array[j]["north"].asFloat(); + float east = array[j]["east"].asFloat(); + std::string type = array[j]["type"].asString(); + std::cout << "point " << j << ": " + << name << ", " + << lon << ", " + << lat << ", " + << north << ", " + << east << ", " + << depth << ", " + << speed << ", " + << type << ", " + << std::endl; + } + } + } +} + +void SurfaceSupportComm::BatchConvertCoordinate(Json::Value &object) +{ + if (!object["points"].isArray()) + { + return; + } + Json::Value& array = object["points"]; + std::vector reference = {object["origin"]["lon"].asFloat(), \ + object["origin"]["lat"].asFloat(), \ + object["origin"]["altitude"].asFloat()}; + for (int i = 0; i < array.size(); i++) + { + float landMarkLon = array[i]["lon"].asFloat(); + float landMarkLat = array[i]["lat"].asFloat(); + float landMarkDepth = array[i]["depth"].asFloat(); + std::vector current = {landMarkLon, landMarkLat, 0}; + + std::vector ned = {0, 0, 0}; + ConvertLLAToNED(reference, current, ned); + array[i]["north"] = ned.at(0); + array[i]["east"] = ned.at(1); + } +} + +void SurfaceSupportComm::closeConnection(Client& c, std::exception& e) +{ + // long unsigned int client_count = m_clients.size() - 1; + m_poll.remove(*c.socket); + delete c.socket; +} + +bool SurfaceSupportComm::OnStartUp() +{ + // AppCastingMOOSApp::OnStartUp(); + planConfigPath = ""; + m_MissionReader.GetValue("PlanConfigPath", planConfigPath); + + RegisterVariables(); + + // udpEvent.SetCallback(periodicCallback, NULL); + // udpEvent.SetPeriod(1); + // udpEvent.Start(); + + // tcpEvent.SetCallback(processMessageCallback, NULL); + // tcpEvent.SetPeriod(1); + // tcpEvent.Start(); + +#if 1 + sock_tcp_receive.bind(TCP_RECEIVE_PORT); + sock_tcp_receive.listen(5); + m_poll.add(sock_tcp_receive); +#endif + + TimeLast = MOOS::Time(); + + return(true); +} + +// bool SurfaceSupportComm::buildReport() +// { +// m_msgs << "buildReport:" << testFlag++ << endl; +// return true; +// } + +void SurfaceSupportComm::RegisterVariables() +{ + // AppCastingMOOSApp::RegisterVariables(); + Register("uDevice_monitor_fb", 0); + Register("uMission_task_fb", 0); +} + +void SurfaceSupportComm::ConvertLLAToENU(std::vector init_lla, + std::vector point_lla, + std::vector& 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 SurfaceSupportComm::ConvertENUToLLA(std::vector init_lla, + std::vector point_enu, + std::vector &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 SurfaceSupportComm::ConvertLLAToNED(std::vector init_lla, + std::vector point_lla, + std::vector& 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 SurfaceSupportComm::ConvertNEDToLLA(std::vector init_lla, + std::vector point_ned, + std::vector &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)); +} diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm.h b/src/pSurfaceSupportComm/SurfaceSupportComm.h new file mode 100644 index 0000000..456637a --- /dev/null +++ b/src/pSurfaceSupportComm/SurfaceSupportComm.h @@ -0,0 +1,140 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: SurfaceSupportComm.h */ +/* DATE: */ +/************************************************************/ + +#ifndef SurfaceSupportComm_HEADER +#define SurfaceSupportComm_HEADER + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "MOOS/libMOOS/MOOSLib.h" +#include "MOOS/libMOOS/Thirdparty/AppCasting/AppCastingMOOSApp.h" +#include +#include +#include +#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 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 init_lla, + std::vector point_lla, + std::vector& point_ned); + void ConvertNEDToLLA(std::vector init_lla, + std::vector point_ned, + std::vector &point_lla); + void ConvertLLAToENU(std::vector init_lla, + std::vector point_lla, + std::vector& point_enu); + void ConvertENUToLLA(std::vector init_lla, + std::vector point_enu, + std::vector &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 + 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 diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm_Info.cpp b/src/pSurfaceSupportComm/SurfaceSupportComm_Info.cpp new file mode 100644 index 0000000..6d92ece --- /dev/null +++ b/src/pSurfaceSupportComm/SurfaceSupportComm_Info.cpp @@ -0,0 +1,115 @@ +/****************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT Cambridge MA */ +/* FILE: SurfaceSupportComm_Info.cpp */ +/* DATE: Dec 29th 1963 */ +/****************************************************************/ + +#include +#include +#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","= "); + 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); +} + diff --git a/src/pSurfaceSupportComm/SurfaceSupportComm_Info.h b/src/pSurfaceSupportComm/SurfaceSupportComm_Info.h new file mode 100644 index 0000000..b553a89 --- /dev/null +++ b/src/pSurfaceSupportComm/SurfaceSupportComm_Info.h @@ -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 + diff --git a/src/pSurfaceSupportComm/main.cpp b/src/pSurfaceSupportComm/main.cpp new file mode 100644 index 0000000..560281c --- /dev/null +++ b/src/pSurfaceSupportComm/main.cpp @@ -0,0 +1,52 @@ +/************************************************************/ +/* NAME: Xiaobin Zeng */ +/* ORGN: MIT */ +/* FILE: main.cpp */ +/* DATE: */ +/************************************************************/ + +#include +#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 +#include +#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 sParams; + if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) + { + list::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 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 &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 &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); +} \ No newline at end of file diff --git a/src/pTaskManger/TaskManger.h b/src/pTaskManger/TaskManger.h new file mode 100644 index 0000000..fcb20b8 --- /dev/null +++ b/src/pTaskManger/TaskManger.h @@ -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 +#include +#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 &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 nodeList); + string getNodeName(const string node); + string getNode(list &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 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 taskList; + int taskCount; + string taskName; +//发送节点信息 + Json::Value node_rpt; +}; + +#endif diff --git a/src/pTaskManger/TaskMangerMain.cpp b/src/pTaskManger/TaskMangerMain.cpp new file mode 100644 index 0000000..9a8cab1 --- /dev/null +++ b/src/pTaskManger/TaskMangerMain.cpp @@ -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 +#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& /dev/null & + +uMAC -t $COMMUNITY.moos +kill -- -$$ diff --git a/src/pTaskManger/pTaskManger.moos b/src/pTaskManger/pTaskManger.moos new file mode 100644 index 0000000..1e790d3 --- /dev/null +++ b/src/pTaskManger/pTaskManger.moos @@ -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 +} + diff --git a/src/pTaskSend/CMakeLists.txt b/src/pTaskSend/CMakeLists.txt new file mode 100644 index 0000000..3362e8f --- /dev/null +++ b/src/pTaskSend/CMakeLists.txt @@ -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 +) + diff --git a/src/pTaskSend/TaskSend.cpp b/src/pTaskSend/TaskSend.cpp new file mode 100644 index 0000000..3580fb0 --- /dev/null +++ b/src/pTaskSend/TaskSend.cpp @@ -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 +#include +#include "MBUtils.h" +#include "TaskSend.h" +#include + +#include + +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 sParams; + if(m_MissionReader.GetConfiguration(GetAppName(), sParams)) { + list::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; +} diff --git a/src/pTaskSend/TaskSend.h b/src/pTaskSend/TaskSend.h new file mode 100644 index 0000000..e579b2f --- /dev/null +++ b/src/pTaskSend/TaskSend.h @@ -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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct Landmark { + float lon; + float lat; + float depth; + float speed; +}; + +struct WayPointBehavior +{ + std::string name; + std::string source; + int priority; + std::vector 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 diff --git a/src/pTaskSend/TaskSendMain.cpp b/src/pTaskSend/TaskSendMain.cpp new file mode 100644 index 0000000..7d87d87 --- /dev/null +++ b/src/pTaskSend/TaskSendMain.cpp @@ -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 +#include "TaskSend.h" +#include "display.h" +// #include +// #include +// #include + +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); +} + diff --git a/src/pTaskSend/build/.cmake/api/v1/query/client-vscode/query.json b/src/pTaskSend/build/.cmake/api/v1/query/client-vscode/query.json new file mode 100644 index 0000000..82bb964 --- /dev/null +++ b/src/pTaskSend/build/.cmake/api/v1/query/client-vscode/query.json @@ -0,0 +1 @@ +{"requests":[{"kind":"cache","version":2},{"kind":"codemodel","version":2},{"kind":"toolchains","version":1},{"kind":"cmakeFiles","version":1}]} \ No newline at end of file diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/cache-v2-d6d942c4138e4121aadf.json b/src/pTaskSend/build/.cmake/api/v1/reply/cache-v2-d6d942c4138e4121aadf.json new file mode 100644 index 0000000..964a989 --- /dev/null +++ b/src/pTaskSend/build/.cmake/api/v1/reply/cache-v2-d6d942c4138e4121aadf.json @@ -0,0 +1,1235 @@ +{ + "entries" : + [ + { + "name" : "CMAKE_ADDR2LINE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/addr2line" + }, + { + "name" : "CMAKE_AR", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/ar" + }, + { + "name" : "CMAKE_BACKWARDS_COMPATIBILITY", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "For backwards compatibility, what version of CMake commands and syntax should this version of CMake try to support." + } + ], + "type" : "STRING", + "value" : "2.4" + }, + { + "name" : "CMAKE_BUILD_TYPE", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "No help, variable specified on the command line." + } + ], + "type" : "STRING", + "value" : "Debug" + }, + { + "name" : "CMAKE_CACHEFILE_DIR", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "This is the directory where this CMakeCache.txt was created" + } + ], + "type" : "INTERNAL", + "value" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend/build" + }, + { + "name" : "CMAKE_CACHE_MAJOR_VERSION", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Major version of cmake used to create the current loaded cache" + } + ], + "type" : "INTERNAL", + "value" : "3" + }, + { + "name" : "CMAKE_CACHE_MINOR_VERSION", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Minor version of cmake used to create the current loaded cache" + } + ], + "type" : "INTERNAL", + "value" : "25" + }, + { + "name" : "CMAKE_CACHE_PATCH_VERSION", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Patch version of cmake used to create the current loaded cache" + } + ], + "type" : "INTERNAL", + "value" : "1" + }, + { + "name" : "CMAKE_COLOR_MAKEFILE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Enable/Disable color output during build." + } + ], + "type" : "BOOL", + "value" : "ON" + }, + { + "name" : "CMAKE_COMMAND", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Path to CMake executable." + } + ], + "type" : "INTERNAL", + "value" : "/usr/bin/cmake" + }, + { + "name" : "CMAKE_CPACK_COMMAND", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Path to cpack program executable." + } + ], + "type" : "INTERNAL", + "value" : "/usr/bin/cpack" + }, + { + "name" : "CMAKE_CTEST_COMMAND", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Path to ctest program executable." + } + ], + "type" : "INTERNAL", + "value" : "/usr/bin/ctest" + }, + { + "name" : "CMAKE_CXX_COMPILER", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "CXX compiler" + } + ], + "type" : "STRING", + "value" : "/usr/bin/g++" + }, + { + "name" : "CMAKE_CXX_COMPILER_AR", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "A wrapper around 'ar' adding the appropriate '--plugin' option for the GCC compiler" + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/gcc-ar-9" + }, + { + "name" : "CMAKE_CXX_COMPILER_RANLIB", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "A wrapper around 'ranlib' adding the appropriate '--plugin' option for the GCC compiler" + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/gcc-ranlib-9" + }, + { + "name" : "CMAKE_CXX_FLAGS", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the CXX compiler during all build types." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_CXX_FLAGS_DEBUG", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the CXX compiler during DEBUG builds." + } + ], + "type" : "STRING", + "value" : "-g" + }, + { + "name" : "CMAKE_CXX_FLAGS_MINSIZEREL", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the CXX compiler during MINSIZEREL builds." + } + ], + "type" : "STRING", + "value" : "-Os -DNDEBUG" + }, + { + "name" : "CMAKE_CXX_FLAGS_RELEASE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the CXX compiler during RELEASE builds." + } + ], + "type" : "STRING", + "value" : "-O3 -DNDEBUG" + }, + { + "name" : "CMAKE_CXX_FLAGS_RELWITHDEBINFO", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the CXX compiler during RELWITHDEBINFO builds." + } + ], + "type" : "STRING", + "value" : "-O2 -g -DNDEBUG" + }, + { + "name" : "CMAKE_C_COMPILER", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "C compiler" + } + ], + "type" : "STRING", + "value" : "/usr/bin/gcc" + }, + { + "name" : "CMAKE_C_COMPILER_AR", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "A wrapper around 'ar' adding the appropriate '--plugin' option for the GCC compiler" + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/gcc-ar-9" + }, + { + "name" : "CMAKE_C_COMPILER_RANLIB", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "A wrapper around 'ranlib' adding the appropriate '--plugin' option for the GCC compiler" + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/gcc-ranlib-9" + }, + { + "name" : "CMAKE_C_FLAGS", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the C compiler during all build types." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_C_FLAGS_DEBUG", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the C compiler during DEBUG builds." + } + ], + "type" : "STRING", + "value" : "-g" + }, + { + "name" : "CMAKE_C_FLAGS_MINSIZEREL", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the C compiler during MINSIZEREL builds." + } + ], + "type" : "STRING", + "value" : "-Os -DNDEBUG" + }, + { + "name" : "CMAKE_C_FLAGS_RELEASE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the C compiler during RELEASE builds." + } + ], + "type" : "STRING", + "value" : "-O3 -DNDEBUG" + }, + { + "name" : "CMAKE_C_FLAGS_RELWITHDEBINFO", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the C compiler during RELWITHDEBINFO builds." + } + ], + "type" : "STRING", + "value" : "-O2 -g -DNDEBUG" + }, + { + "name" : "CMAKE_DLLTOOL", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "CMAKE_DLLTOOL-NOTFOUND" + }, + { + "name" : "CMAKE_EDIT_COMMAND", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Path to cache edit program executable." + } + ], + "type" : "INTERNAL", + "value" : "/usr/bin/ccmake" + }, + { + "name" : "CMAKE_EXECUTABLE_FORMAT", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Executable file format" + } + ], + "type" : "INTERNAL", + "value" : "ELF" + }, + { + "name" : "CMAKE_EXE_LINKER_FLAGS", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during all build types." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_EXE_LINKER_FLAGS_DEBUG", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during DEBUG builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_EXE_LINKER_FLAGS_MINSIZEREL", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during MINSIZEREL builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_EXE_LINKER_FLAGS_RELEASE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during RELEASE builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during RELWITHDEBINFO builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_EXPORT_COMPILE_COMMANDS", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "No help, variable specified on the command line." + } + ], + "type" : "BOOL", + "value" : "TRUE" + }, + { + "name" : "CMAKE_EXTRA_GENERATOR", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Name of external makefile project generator." + } + ], + "type" : "INTERNAL", + "value" : "" + }, + { + "name" : "CMAKE_FIND_PACKAGE_REDIRECTS_DIR", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Value Computed by CMake." + } + ], + "type" : "STATIC", + "value" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend/build/CMakeFiles/pkgRedirects" + }, + { + "name" : "CMAKE_GENERATOR", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Name of generator." + } + ], + "type" : "INTERNAL", + "value" : "Unix Makefiles" + }, + { + "name" : "CMAKE_GENERATOR_INSTANCE", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Generator instance identifier." + } + ], + "type" : "INTERNAL", + "value" : "" + }, + { + "name" : "CMAKE_GENERATOR_PLATFORM", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Name of generator platform." + } + ], + "type" : "INTERNAL", + "value" : "" + }, + { + "name" : "CMAKE_GENERATOR_TOOLSET", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Name of generator toolset." + } + ], + "type" : "INTERNAL", + "value" : "" + }, + { + "name" : "CMAKE_HOME_DIRECTORY", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Source directory with the top level CMakeLists.txt file for this project" + } + ], + "type" : "INTERNAL", + "value" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend" + }, + { + "name" : "CMAKE_INSTALL_PREFIX", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Install path prefix, prepended onto install directories." + } + ], + "type" : "PATH", + "value" : "/usr/local" + }, + { + "name" : "CMAKE_INSTALL_SO_NO_EXE", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Install .so files without execute permission." + } + ], + "type" : "INTERNAL", + "value" : "1" + }, + { + "name" : "CMAKE_LINKER", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/ld" + }, + { + "name" : "CMAKE_MAKE_PROGRAM", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/make" + }, + { + "name" : "CMAKE_MODULE_LINKER_FLAGS", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of modules during all build types." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_MODULE_LINKER_FLAGS_DEBUG", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of modules during DEBUG builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of modules during MINSIZEREL builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_MODULE_LINKER_FLAGS_RELEASE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of modules during RELEASE builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of modules during RELWITHDEBINFO builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_NM", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/nm" + }, + { + "name" : "CMAKE_NUMBER_OF_MAKEFILES", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "number of local generators" + } + ], + "type" : "INTERNAL", + "value" : "1" + }, + { + "name" : "CMAKE_OBJCOPY", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/objcopy" + }, + { + "name" : "CMAKE_OBJDUMP", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/objdump" + }, + { + "name" : "CMAKE_PLATFORM_INFO_INITIALIZED", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Platform information initialized" + } + ], + "type" : "INTERNAL", + "value" : "1" + }, + { + "name" : "CMAKE_PROJECT_DESCRIPTION", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Value Computed by CMake" + } + ], + "type" : "STATIC", + "value" : "" + }, + { + "name" : "CMAKE_PROJECT_HOMEPAGE_URL", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Value Computed by CMake" + } + ], + "type" : "STATIC", + "value" : "" + }, + { + "name" : "CMAKE_PROJECT_NAME", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Value Computed by CMake" + } + ], + "type" : "STATIC", + "value" : "Project" + }, + { + "name" : "CMAKE_RANLIB", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/ranlib" + }, + { + "name" : "CMAKE_READELF", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/readelf" + }, + { + "name" : "CMAKE_ROOT", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Path to CMake installation." + } + ], + "type" : "INTERNAL", + "value" : "/usr/share/cmake-3.25" + }, + { + "name" : "CMAKE_SHARED_LINKER_FLAGS", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of shared libraries during all build types." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_SHARED_LINKER_FLAGS_DEBUG", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of shared libraries during DEBUG builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of shared libraries during MINSIZEREL builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_SHARED_LINKER_FLAGS_RELEASE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of shared libraries during RELEASE builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of shared libraries during RELWITHDEBINFO builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_SKIP_INSTALL_RPATH", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "If set, runtime paths are not added when installing shared libraries, but are added when building." + } + ], + "type" : "BOOL", + "value" : "NO" + }, + { + "name" : "CMAKE_SKIP_RPATH", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "If set, runtime paths are not added when using shared libraries." + } + ], + "type" : "BOOL", + "value" : "NO" + }, + { + "name" : "CMAKE_STATIC_LINKER_FLAGS", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of static libraries during all build types." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_STATIC_LINKER_FLAGS_DEBUG", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of static libraries during DEBUG builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of static libraries during MINSIZEREL builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_STATIC_LINKER_FLAGS_RELEASE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of static libraries during RELEASE builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Flags used by the linker during the creation of static libraries during RELWITHDEBINFO builds." + } + ], + "type" : "STRING", + "value" : "" + }, + { + "name" : "CMAKE_STRIP", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "Path to a program." + } + ], + "type" : "FILEPATH", + "value" : "/usr/bin/strip" + }, + { + "name" : "CMAKE_UNAME", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "uname command" + } + ], + "type" : "INTERNAL", + "value" : "/usr/bin/uname" + }, + { + "name" : "CMAKE_VERBOSE_MAKEFILE", + "properties" : + [ + { + "name" : "ADVANCED", + "value" : "1" + }, + { + "name" : "HELPSTRING", + "value" : "If this value is on, makefiles will be generated without the .SILENT directive, and all commands will be echoed to the console during the make. This is useful for debugging only. With Visual Studio IDE projects all commands are done without /nologo." + } + ], + "type" : "BOOL", + "value" : "FALSE" + }, + { + "name" : "EXECUTABLE_OUTPUT_PATH", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Single output directory for building all executables." + } + ], + "type" : "PATH", + "value" : "" + }, + { + "name" : "LIBRARY_OUTPUT_PATH", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Single output directory for building all libraries." + } + ], + "type" : "PATH", + "value" : "" + }, + { + "name" : "Project_BINARY_DIR", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Value Computed by CMake" + } + ], + "type" : "STATIC", + "value" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend/build" + }, + { + "name" : "Project_IS_TOP_LEVEL", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Value Computed by CMake" + } + ], + "type" : "STATIC", + "value" : "ON" + }, + { + "name" : "Project_SOURCE_DIR", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "Value Computed by CMake" + } + ], + "type" : "STATIC", + "value" : "/home/zjk/Desktop/project/moos-ivp-extend/src/pTaskSend" + }, + { + "name" : "_CMAKE_LINKER_PUSHPOP_STATE_SUPPORTED", + "properties" : + [ + { + "name" : "HELPSTRING", + "value" : "linker supports push/pop state" + } + ], + "type" : "INTERNAL", + "value" : "TRUE" + } + ], + "kind" : "cache", + "version" : + { + "major" : 2, + "minor" : 0 + } +} diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/cmakeFiles-v1-1b864ed9ef170e655086.json b/src/pTaskSend/build/.cmake/api/v1/reply/cmakeFiles-v1-1b864ed9ef170e655086.json new file mode 100644 index 0000000..5a6c27e --- /dev/null +++ b/src/pTaskSend/build/.cmake/api/v1/reply/cmakeFiles-v1-1b864ed9ef170e655086.json @@ -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 + } +} diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/codemodel-v2-8cafde00da2f94adfddf.json b/src/pTaskSend/build/.cmake/api/v1/reply/codemodel-v2-8cafde00da2f94adfddf.json new file mode 100644 index 0000000..db123dc --- /dev/null +++ b/src/pTaskSend/build/.cmake/api/v1/reply/codemodel-v2-8cafde00da2f94adfddf.json @@ -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 + } +} diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/directory-.-Debug-f5ebdc15457944623624.json b/src/pTaskSend/build/.cmake/api/v1/reply/directory-.-Debug-f5ebdc15457944623624.json new file mode 100644 index 0000000..3a67af9 --- /dev/null +++ b/src/pTaskSend/build/.cmake/api/v1/reply/directory-.-Debug-f5ebdc15457944623624.json @@ -0,0 +1,14 @@ +{ + "backtraceGraph" : + { + "commands" : [], + "files" : [], + "nodes" : [] + }, + "installers" : [], + "paths" : + { + "build" : ".", + "source" : "." + } +} diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/index-2023-09-21T07-09-11-0928.json b/src/pTaskSend/build/.cmake/api/v1/reply/index-2023-09-21T07-09-11-0928.json new file mode 100644 index 0000000..86cecfa --- /dev/null +++ b/src/pTaskSend/build/.cmake/api/v1/reply/index-2023-09-21T07-09-11-0928.json @@ -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 + } + } + ] + } + } + } +} diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/target-pTaskSend-Debug-fe070126a6f1fe44e26b.json b/src/pTaskSend/build/.cmake/api/v1/reply/target-pTaskSend-Debug-fe070126a6f1fe44e26b.json new file mode 100644 index 0000000..a2e86b0 --- /dev/null +++ b/src/pTaskSend/build/.cmake/api/v1/reply/target-pTaskSend-Debug-fe070126a6f1fe44e26b.json @@ -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" +} diff --git a/src/pTaskSend/build/.cmake/api/v1/reply/toolchains-v1-2781238ec0fa0b9996f9.json b/src/pTaskSend/build/.cmake/api/v1/reply/toolchains-v1-2781238ec0fa0b9996f9.json new file mode 100644 index 0000000..c8dfa23 --- /dev/null +++ b/src/pTaskSend/build/.cmake/api/v1/reply/toolchains-v1-2781238ec0fa0b9996f9.json @@ -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 + } +} diff --git a/src/pTaskSend/display.cpp b/src/pTaskSend/display.cpp new file mode 100644 index 0000000..d1bd223 --- /dev/null +++ b/src/pTaskSend/display.cpp @@ -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() +{ +} diff --git a/src/pTaskSend/display.h b/src/pTaskSend/display.h new file mode 100644 index 0000000..73cc899 --- /dev/null +++ b/src/pTaskSend/display.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#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 \ No newline at end of file diff --git a/src/pTaskSend/pTaskSend.moos b/src/pTaskSend/pTaskSend.moos new file mode 100644 index 0000000..8107445 --- /dev/null +++ b/src/pTaskSend/pTaskSend.moos @@ -0,0 +1,11 @@ +// MOOS file + +ServerHost = localhost +ServerPort = 9000 + +ProcessConfig = pTaskSend +{ + AppTick = 4 + CommsTick = 4 +} +