From 47d437abd585ab4ddca054ac80147c5d129d741d Mon Sep 17 00:00:00 2001 From: zjk <1553836110@qq.com> Date: Sun, 29 Jun 2025 14:15:33 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86=E5=B2=B8=E5=9F=BA?= =?UTF-8?q?=E9=85=8D=E7=BD=AE=E6=96=87=E4=BB=B6=EF=BC=8C=E4=BF=AE=E5=A4=8D?= =?UTF-8?q?=E4=BA=86=E8=BF=9E=E6=8E=A5bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 +- missions/alder/alder.moos | 4 +- missions/alpha/alpha.moos | 201 ++++++++++++++++++++++++++++ missions/alpha/alpha_shoreside.moos | 114 ++++++++++++++++ missions/alpha/alpha_test.bhv | 94 +++++++++++++ missions/alpha/alpha_vehicle.moos | 126 +++++++++++++++++ src/pAUV150/AUV150.cpp | 67 +++++----- 7 files changed, 572 insertions(+), 36 deletions(-) create mode 100644 missions/alpha/alpha.moos create mode 100644 missions/alpha/alpha_shoreside.moos create mode 100644 missions/alpha/alpha_test.bhv create mode 100644 missions/alpha/alpha_vehicle.moos diff --git a/CMakeLists.txt b/CMakeLists.txt index e3c922e..e93c78d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 3.0) PROJECT( IVP_EXTEND ) set (CMAKE_CXX_STANDARD 11) -set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/software/moos-ivp-main") +# set (MOOSIVP_SOURCE_TREE_BASE "/home/zjk/software/moos-ivp-main") #======================================================================= # Set the output directories for the binary and library files diff --git a/missions/alder/alder.moos b/missions/alder/alder.moos index fa71112..53fc980 100644 --- a/missions/alder/alder.moos +++ b/missions/alder/alder.moos @@ -21,7 +21,7 @@ ProcessConfig = ANTLER Run = pMarineViewer @ NewConsole = false Run = uProcessWatch @ NewConsole = false Run = pHelmIvP @ NewConsole = false - Run = pAUV150 @ NewConsole = false + Run = pAUV150 @ NewConsole = true //Run = pMotionControler @ NewConsole = false } @@ -164,7 +164,7 @@ ProcessConfig = pAUV150 //server_host = 10.127.0.18 //server_host = 127.0.0.1 - server_host = 172.24.224.1 + server_host = 192.168.137.1 LatOrigin = 43.825300 LongOrigin = -70.330400 diff --git a/missions/alpha/alpha.moos b/missions/alpha/alpha.moos new file mode 100644 index 0000000..8c73803 --- /dev/null +++ b/missions/alpha/alpha.moos @@ -0,0 +1,201 @@ +// Alder mission file +ServerHost = localhost +ServerPort = 9000 +Community = alder +MOOSTimeWarp = 1 + +LatOrigin = 43.825300 +LongOrigin = -70.330400 + +//------------------------------------------ +// Antler config block +ProcessConfig = ANTLER +{ + MSBetweenLaunches = 200 + + Run = MOOSDB @ NewConsole = false + //Run = uSimMarineV22 @ NewConsole = false + Run = pNodeReporter @ NewConsole = false + //Run = pLogger @ NewConsole = false + //Run = pMarinePIDV22 @ NewConsole = false + Run = pMarineViewer @ NewConsole = false + Run = uProcessWatch @ NewConsole = false + Run = pHelmIvP @ NewConsole = false + Run = pAUV150 @ NewConsole = true + //Run = pMotionControler @ NewConsole = false +} + +//------------------------------------------ +// uSimMarineV22 config block + +ProcessConfig = uSimMarineV22 +{ + 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 + + summary_wait = 5 + + nowatch = uXMS* + nowatch = uMAC* + nowatch = uPokeDB* + nowatch = uQueryDB* + nowatch = uTermCommand* + watch_all = true +} + +//------------------------------------------ +// pHelmIvP config block + +ProcessConfig = pHelmIvP +{ + AppTick = 4 + CommsTick = 4 + + Behaviors = alpha.bhv + Verbose = quiet + Domain = course:0:359:360 + Domain = speed:0:4:21 + + //IVP_BEHAVIOR_DIR = ../../lib + + ok_skew = any + start_in_drive = false +} + +//------------------------------------------ +// pMarinePIDV22 config block + +ProcessConfig = pMarinePIDV22 +{ + AppTick = 10 + CommsTick = 10 + + VERBOSE = true + DEPTH_CONTROL = false + + // Yaw PID controller + YAW_PID_KP = 1 + YAW_PID_KD = 1 + YAW_PID_KI = 1 + YAW_PID_INTEGRAL_LIMIT = 1 + + // 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 = 35 + MAXTHRUST = 35 + + // 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 + + platform_length = 1.5 // meters + //platform_beam = 0.2 // meters + platform_type = auv + platform_color = dodger_blue + //vessel_type = auv +} + +ProcessConfig = pAUV150 +{ + AppTick = 10 + CommsTick = 10 + + //server_host = 10.127.0.18 + //server_host = 127.0.0.1 + server_host = 192.168.137.1 + LatOrigin = 43.825300 + LongOrigin = -70.330400 + + ControlFrequency = 10 + +} + +ProcessConfig = pMotionControler +{ + AppTick = 10 + CommsTick = 10 + config_file = /home/zjk/project/moos-ivp-extend/missions/alder/ControlParam.json +} + +ProcessConfig = pLogger +{ + AppTick = 10 + CommsTick = 10 + + File = XLOG_SHORESIDE + PATH = ./ + SyncLog = true @ 0.2 + AsyncLog = true + FileTimeStamp = true + + LogAuxSrc = true + + // Log it all!!!!! + WildCardLogging = true + + WildCardOmitPattern = *_STATUS + WildCardOmitPattern = DB_VARSUMMARY + WildCardOmitPattern = DB_RWSUMMARY +} \ No newline at end of file diff --git a/missions/alpha/alpha_shoreside.moos b/missions/alpha/alpha_shoreside.moos new file mode 100644 index 0000000..c21204f --- /dev/null +++ b/missions/alpha/alpha_shoreside.moos @@ -0,0 +1,114 @@ +// Alder mission file +ServerHost = localhost +ServerPort = 9000 +Community = alpha +MOOSTimeWarp = 1 + +LatOrigin = 43.825300 +LongOrigin = -70.330400 + +//------------------------------------------ +// Antler config block +ProcessConfig = ANTLER +{ + MSBetweenLaunches = 200 + + Run = MOOSDB @ NewConsole = false + //Run = pLogger @ NewConsole = false + Run = pMarineViewer @ NewConsole = false + Run = uProcessWatch @ NewConsole = false + RUn = pShare @ NewConsole = false +} + +//------------------------------------------ +// pShare config block +ProcessConfig = pShare +{ + AppTick = 2 + CommsTick = 2 + + input = route = localhost:8085 + output = src_name=APPCAST_REQ, route=10.25.0.230:8081 + + //输出有两个端口，8081和8082，选择用1或者2 + //发送消息看以下格式 + //output = src_name=Y, dest_name=B, route=host:port + + output = src_name=uMission_action_cmd,route=10.25.0.230:8082 + output = DEPLOY, route=10.25.0.230:8082 + output = RETURN, route=10.25.0.230:8082 + output = MOOS_MANUAL_OVERIDE, route=10.25.0.230:8082 +} + +//------------------------------------------ +// uProcessWatch config block +ProcessConfig = uProcessWatch +{ + AppTick = 4 + CommsTick = 4 + + summary_wait = 5 + + nowatch = uXMS* + nowatch = uMAC* + nowatch = uPokeDB* + nowatch = uQueryDB* + nowatch = uTermCommand* + watch_all = true +} + +//------------------------------------------ +// 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 +} + + + +ProcessConfig = pLogger +{ + AppTick = 10 + CommsTick = 10 + + File = XLOG_SHORESIDE + PATH = ./ + SyncLog = true @ 0.2 + AsyncLog = true + FileTimeStamp = true + + LogAuxSrc = true + + // Log it all!!!!! + WildCardLogging = true + + WildCardOmitPattern = *_STATUS + WildCardOmitPattern = DB_VARSUMMARY + WildCardOmitPattern = DB_RWSUMMARY +} \ No newline at end of file diff --git a/missions/alpha/alpha_test.bhv b/missions/alpha/alpha_test.bhv new file mode 100644 index 0000000..c63240f --- /dev/null +++ b/missions/alpha/alpha_test.bhv @@ -0,0 +1,94 @@ +//-------- FILE: alpha.bhv ------------- + +initialize DEPLOY = false +initialize RETURN = false + +//---------------------------------------------- +Behavior = BHV_Waypoint +{ + name = waypt_survey + pwt = 100 + condition = RETURN = false + condition = DEPLOY = true + endflag = RETURN = true + + configflag = CRUISE_SPD = $[SPEED] + //configflag = OSPOS = $[OSX],$[OSY] + + activeflag = INFO=$[OWNSHIP] + activeflag = INFO=$[BHVNAME] + activeflag = INFO=$[BHVTYPE] + + cycleflag = CINFO=$[OSX],$[OSY] + + 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 + + speed_alt = 1.2 + use_alt_speed = true + lead = 8 + lead_damper = 1 + lead_to_start = true + speed = 12 // meters per second + capture_line = true + capture_radius = 5.0 + slip_radius = 15.0 + efficiency_measure = all + + // points = pts={107.5,-53.5:112,-43.8:112,-46.1:111.7,-49.3:108.3,-52.7:107.7,-53.3} + polygon = 60,-40 : 60,-160 : 150,-160 : 180,-100 : 150,-40 + + order = normal + //repeat = 100000 + 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_Waypoint +{ + name = waypt_return + pwt = 100 + condition = RETURN = true + condition = DEPLOY = true + condition = ((DEPLoy = true) or (alpha = one)) or (bravo = two) + 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 + } + +//---------------------------------------------- +Behavior=BHV_ConstantSpeed +{ + name = const_speed + pwt = 200 + condition = SPD=true + condition = DEPLOY = true + perpetual = true + updates = SPEED_UPDATE + endflag = SPD = false + + speed = 0.5 + duration = 10 + duration_reset = CONST_SPD_RESET=true + +} + diff --git a/missions/alpha/alpha_vehicle.moos b/missions/alpha/alpha_vehicle.moos new file mode 100644 index 0000000..79c0911 --- /dev/null +++ b/missions/alpha/alpha_vehicle.moos @@ -0,0 +1,126 @@ +// Alder mission file +ServerHost = localhost +ServerPort = 9000 +Community = alpha +MOOSTimeWarp = 1 + +LatOrigin = 43.825300 +LongOrigin = -70.330400 + +//------------------------------------------ +// Antler config block +ProcessConfig = ANTLER +{ + MSBetweenLaunches = 200 + + Run = MOOSDB @ NewConsole = false + Run = pNodeReporter @ NewConsole = false + //Run = pLogger @ NewConsole = false + //Run = pMarineViewer @ NewConsole = false + Run = uProcessWatch @ NewConsole = false + Run = pHelmIvP @ NewConsole = false + Run = pAUV150 @ NewConsole = false + Run = pShare @ NewConsole = false +} + +ProcessConfig = pShare +{ + AppTick = 2 + CommsTick = 2 + //UUV 信息 + output = src_name = NODE_REPORT*, route = 10.25.0.160:8085 + //App 信息 + output = src_name = APPCAST*, route = 10.25.0.160:8085 + //路径点信息 + output = src_name = VIEW*, route = 10.25.0.160:8085 + //调试端输入端口 + input = route = localhost:8082 +} + +//------------------------------------------ +// uProcessWatch config block + +ProcessConfig = uProcessWatch +{ + AppTick = 4 + CommsTick = 4 + + summary_wait = 5 + + nowatch = uXMS* + nowatch = uMAC* + nowatch = uPokeDB* + nowatch = uQueryDB* + nowatch = uTermCommand* + watch_all = true +} + +//------------------------------------------ +// pHelmIvP config block + +ProcessConfig = pHelmIvP +{ + AppTick = 4 + CommsTick = 4 + + Behaviors = alpha.bhv + Verbose = quiet + Domain = course:0:359:360 + Domain = speed:0:4:21 + + //IVP_BEHAVIOR_DIR = ../../lib + + ok_skew = any + start_in_drive = false +} + +//------------------------------------------ +// pNodeReporter config block + +ProcessConfig = pNodeReporter +{ + AppTick = 2 + CommsTick = 2 + + platform_length = 1.5 // meters + //platform_beam = 0.2 // meters + platform_type = auv + platform_color = dodger_blue + //vessel_type = auv +} + +ProcessConfig = pAUV150 +{ + AppTick = 10 + CommsTick = 10 + + //server_host = 10.127.0.18 + //server_host = 127.0.0.1 + server_host = 192.168.137.1 + LatOrigin = 43.825300 + LongOrigin = -70.330400 + + ControlFrequency = 10 + +} + +ProcessConfig = pLogger +{ + AppTick = 10 + CommsTick = 10 + + File = XLOG_SHORESIDE + PATH = ./ + SyncLog = true @ 0.2 + AsyncLog = true + FileTimeStamp = true + + LogAuxSrc = true + + // Log it all!!!!! + WildCardLogging = true + + WildCardOmitPattern = *_STATUS + WildCardOmitPattern = DB_VARSUMMARY + WildCardOmitPattern = DB_RWSUMMARY +} \ No newline at end of file diff --git a/src/pAUV150/AUV150.cpp b/src/pAUV150/AUV150.cpp index f77f9ec..adcdb1e 100644 --- a/src/pAUV150/AUV150.cpp +++ b/src/pAUV150/AUV150.cpp @@ -263,6 +263,7 @@ bool AUV150::ListenLoop() try { if((m_TcpSocket!=NULL) && m_bConnected) + // if((m_TcpSocket!=NULL)) { int count = m_TcpSocket->iReadMessageWithTimeOut(buffer, sizeof(FeedbackFrame_150AUV),3); if(count > 0) { @@ -304,40 +305,40 @@ bool AUV150::ListenLoop() m_status.heading_error = m_control.rtY.heading_error; m_status.pitch_error = m_control.rtY.pitch_error; // } - // 发送消息 - try - { - m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV)); - m_controlGap = MOOSTime() - m_desiredVarTime; - m_controlTime = MOOSTime(); - m_bConnected = true; - } - catch(XPCException &e) - { - m_bConnected = false; - std::cerr << "there was trouble sending message: " - << e.sGetException() << "\n"; - } - - // 计算控制频率 - m_samplingTime = MOOSTime(); - message_count++; - double elapsed_time = m_samplingTime - start_time; - if(elapsed_time >= 1.0) { // 每秒更新一次频率 - m_real_read_freq = message_count / elapsed_time; - message_count = 0; - start_time = MOOSTime(); - } - //正常情况下清除错误码 - m_faultCodes.erase(0x11); - m_faultCodes.erase(0x12); - m_faultCodes.erase(0x13); - } - else - { - m_faultCodes.insert(0x12); //无数据接收 - } } + // 发送消息 + try + { + m_TcpSocket->iSendMessage(&m_CommandFrame, sizeof(CommandFrame_150AUV)); + m_controlGap = MOOSTime() - m_desiredVarTime; + m_controlTime = MOOSTime(); + m_bConnected = true; + } + catch(XPCException &e) + { + m_bConnected = false; + std::cerr << "there was trouble sending message: " + << e.sGetException() << "\n"; + } + + // 计算控制频率 + m_samplingTime = MOOSTime(); + message_count++; + double elapsed_time = m_samplingTime - start_time; + if(elapsed_time >= 1.0) { // 每秒更新一次频率 + m_real_read_freq = message_count / elapsed_time; + message_count = 0; + start_time = MOOSTime(); + } + //正常情况下清除错误码 + m_faultCodes.erase(0x11); + m_faultCodes.erase(0x12); + m_faultCodes.erase(0x13); + } + else + { + m_faultCodes.insert(0x12); //无数据接收 + } } catch (XPCException & e) {