diff --git a/README.md b/README.md
index 16c35e58ce0f51a3f53317ba1c3dde6d5d6ce58e..ec4c94487e1b2efd8571ac1c7b230654b9061d14 100644
--- a/README.md
+++ b/README.md
@@ -31,7 +31,7 @@
1. 安装依赖
- ```bash
+ ```sh
sudo apt install -y ros-humble-gazebo-*
sudo apt install -y ros-humble-xacro
sudo apt install -y ros-humble-robot-state-publisher
@@ -51,11 +51,11 @@
2. 安装 [Livox SDK2](https://github.com/Livox-SDK/Livox-SDK2)
- ```bash
+ ```sh
sudo apt install cmake
```
- ```bash
+ ```sh
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd ./Livox-SDK2/
mkdir build
@@ -66,11 +66,11 @@
## 四. 编译运行
-```bash
+```sh
git clone --recursive https://gitee.com/SMBU-POLARBEAR/pb_rmsimulation --depth=1
```
-```bash
+```sh
cd pb_rmsimulation
./build.sh
```
@@ -109,7 +109,7 @@ cd pb_rmsimulation
- 示例：
- 边建图边导航
- ```bash
+ ```sh
ros2 launch rm_bringup bringup_sim.launch.py \
world:=RMUL \
mode:=mapping
@@ -117,7 +117,7 @@ cd pb_rmsimulation
- 已知全局地图导航
- ```bash
+ ```sh
ros2 launch rm_bringup bringup_sim.launch.py \
world:=RMUL \
mode:=nav \
@@ -129,7 +129,7 @@ cd pb_rmsimulation
- 示例：
- 边建图边导航
- ```bash
+ ```sh
ros2 launch rm_bringup bringup_real.launch.py \
world:=YOUR_WORLD_NAME \
mode:=mapping
@@ -143,18 +143,18 @@ cd pb_rmsimulation
- 已知全局地图导航
- ```bash
+ ```sh
ros2 launch rm_bringup bringup_real.launch.py \
world:=YOUR_WORLD_NAME \
mode:=nav \
localization:=slam_toolbox
```
- Tips: 栅格地图文件和 pcd 文件需具为相同名称，分别存放在 `src/rm_bringup/map` 和 `src/rm_bringup/PCD` 中，启动导航时 world 指定为文件名前缀即可。
+ Tips: 栅格地图文件和 pcd 文件需命名为相同名称，分别存放至 `src/rm_bringup/map` 和 `src/rm_bringup/PCD` 中，启动导航时 world 指定为文件名前缀即可。
### 小工具 - 键盘控制
-```bash
+```sh
ros2 run teleop_twist_keyboard teleop_twist_keyboard
```
@@ -166,28 +166,7 @@ Mid360 点云仿真：参考了 [livox_laser_simulation](https://github.com/Livo
非常非常感谢以上开源项目 / Issue 的帮助！！！
-## TODO
-
-1. ~~将湖工大地图模型文件修改为动态路径~~（2023.9.30 完成）
-
-2. ~~多线雷达~~（2023.10.8 完成）
-
-3. ~~适配为 ROS2 Humble 版本~~ (2023.10.13 完成)
-
-4. ~~完成 nav 的适配~~ （忘记记录时间了）
-
-5. ~~优雅的选择地图，并根据不同地图适配小车出生点~~（2023.11.25完成）
-
-6. 决策树
-
-7. 优化 livox_laser_simulation 性能问题
## 常见问题
-- Gazebo 无法正常启动？
-
- 答：执行命令 `source /usr/share/gazebo/setup.sh`。
-
-- Gazebo 没有生成小车？
-
- 答：执行命令 `killall -9 gzserver` 和 `killall -9 gzclient` 确保 Gazebo 后台进程已完全清除，再重新启动即可。
+详见 [Issue](https://gitee.com/SMBU-POLARBEAR/pb_rmsimulation/issues/I9427I)
diff --git a/src/rm_bringup/config/mapper_params_localization.yaml b/src/rm_bringup/config/mapper_params_localization.yaml
index 04f00857fe39a28855bc2a12a2365a4867cda29e..648a8bd0143f694a5730656814c9540effc8400f 100644
--- a/src/rm_bringup/config/mapper_params_localization.yaml
+++ b/src/rm_bringup/config/mapper_params_localization.yaml
@@ -38,7 +38,7 @@ slam_toolbox:
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
- do_loop_closing: true
+ do_loop_closing: false
loop_match_minimum_chain_size: 3
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
diff --git a/src/rm_decision/rm_FSM/Nav2_API.md b/src/rm_decision/rm_FSM/Nav2_API.md
deleted file mode 100644
index 12811e054d3d296e7af405454d6d461cad7cd4ac..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/Nav2_API.md
+++ /dev/null
@@ -1,81 +0,0 @@
-# Nav2 API (Python3)
-
-原版地址：https://github.com/ros-planning/navigation2/tree/humble/nav2_simple_commander
-
-## 概述
-
-此软件包的目标是为 Python3 用户提供“导航库”功能。我们提供了一个处理所有 ROS2 相关和 Action Server 相关事项的 API，以便您可以专注于构建利用 Nav2 能力的应用程序。我们还为您提供了使用 API 构建自主移动机器人的常见基本功能的演示和示例。
-
-该软件包由 [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) 在 [Samsung Research](https://www.sra.samsung.com/) 构建，最初为 [2021 ROS Developers Day](https://www.theconstructsim.com/ros-developers-day-2021/) 大会上的主题演讲准备了初版原型（代码可以在 [这里](https://github.com/SteveMacenski/nav2_rosdevday_2021)找到）。
-
-
-
-## API
-
-请查看 [API 指南页面](https://navigation.ros.org/commander_api/index.html) 以获取附加参数描述。
-
-基本导航器提供的方法如下，包括输入和预期返回值。如果服务器失败，它可能会引发异常或返回 `None` 对象，因此请务必正确包装您的导航调用以进行 try/catch 并检查结果是否为 `None` 类型。
-
-自 2023 年 9 月起：简单导航器构造函数将接受一个 `namespace` 字段，以支持多机器人应用程序或具有命名空间的 Nav2 启动。
-
-|机器人导航方法|描述|
-|-|-|
-|setInitialPose(initial_pose)|将机器人的初始姿势（`PoseStamped`）设置为定位。|
-|goThroughPoses(poses, behavior_tree='')|请求机器人穿过一组姿势（`PoseStamped` 列表）。|
-|goToPose(pose, behavior_tree='')|请求机器人驶向一个姿势（`PoseStamped`）。|
-|followWaypoints(poses)|请求机器人跟随一组路标（`PoseStamped` 列表）。这将在每个姿势处执行特定的 `TaskExecutor`。|
-|followPath(path, controller_id='', goal_checker_id='')|请求机器人从起始点到目标 `PoseStamped`、`nav_msgs/Path` 遵循路径。|
-|spin(spin_dist=1.57, time_allowance=10)|请求机器人执行给定角度的原地旋转。|
-|backup(backup_dist=0.15, backup_speed=0.025, time_allowance=10)|请求机器人倒车一定距离。|
-|cancelTask()|取消进行中的任务请求。|
-|isTaskComplete()|检查任务是否已完成，超时为 `100ms`。如果已完成，则返回 `True`，如果仍在进行中，则返回 `False`。|
-|getFeedback()|获取任务的反馈，返回动作服务器反馈对象。|
-|getResult()|获取任务的最终结果，在 `isTaskComplete` 返回 `True` 后调用。返回动作服务器结果对象。|
-|getPath(start, goal, planner_id='', use_start=False)|获取从起始点到目标 `PoseStamped`、`nav_msgs/Path` 的路径。|
-|getPathThroughPoses(start, goals, planner_id='', use_start=False)|获取通过起始点到一组目标点的路径，一组 `PoseStamped`、`nav_msgs/Path` 的列表。|
-|smoothPath(path, smoother_id='', max_duration=2.0, check_for_collision=False)|平滑给定的 `nav_msgs/msg/Path` 路径。|
-|changeMap(map_filepath)|请求从当前地图更改到 `map_filepath` 的 yaml。|
-|clearAllCostmaps()|清除全局和局部成本地图。|
-|clearLocalCostmap()|清除局部成本地图。|
-|clearGlobalCostmap()|清除全局成本地图。|
-|getGlobalCostmap()|返回全局成本地图，`nav2_msgs/Costmap`|
-|getLocalCostmap()|返回局部成本地图，`nav2_msgs/Costmap`|
-|waitUntilNav2Active(navigator='bt_navigator, localizer='amcl')|阻塞直到 Nav2 完全在线，并且生命周期节点处于活动状态。与自动启动或外部生命周期启动一起使用。可以指定自定义导航器和定位器节点|
-|lifecycleStartup()|发送请求到所有生命周期管理服务器，将它们带入活动状态，如果 `autostart` 为 `false`，并且您希望此程序控制 Nav2 的生命周期时使用。|
-|lifecycleShutdown()|发送请求到所有生命周期管理服务器，关闭它们。|
-|destroyNode()|释放对象使用的资源。|
-
-构建应用程序的一般模板如下：
-
-```python
-
-from rm_decision.robot_navigator import BasicNavigator
-import rclpy
-
-rclpy.init()
-
-nav = BasicNavigator()
-...
-nav.setInitialPose(init_pose)
-nav.waitUntilNav2Active() # 如果自动启动，则使用 `lifecycleStartup()`
-...
-path = nav.getPath(init_pose, goal_pose)
-smoothed_path = nav.smoothPath(path)
-...
-nav.goToPose(goal_pose)
-while not nav.isTaskComplete():
- feedback = nav.getFeedback()
- if feedback.navigation_duration > 600:
- nav.cancelTask()
-...
-result = nav.getResult()
-if result == TaskResult.SUCCEEDED:
- print('目标成功完成!')
-elif result == TaskResult.CANCELED:
- print('目标已取消!')
-elif result == TaskResult.FAILED:
- print('目标失败!')
-```
-
-
-
diff --git a/src/rm_decision/rm_FSM/README.md b/src/rm_decision/rm_FSM/README.md
deleted file mode 100644
index 6286392ab705a69746cca2d033979f17508311a3..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/README.md
+++ /dev/null
@@ -1,149 +0,0 @@
-# ATTENTION
-
-> 实验性功能，决策判断十分简陋，仍在开发中
-
-# rm_decision
-
-## 概述
-
-`rm_decision` 由 深圳北理莫斯科大学-北极熊战队 开发的哨兵决策模块。作为 RMUC 哨兵机器人的决策逻辑，根据机器人状态、视觉识别结果、队友位置等做出决策，从而在不同的状态之间切换，执行巡逻、跟随、攻击和逃跑等动作。代码参考了 [华农3V3哨兵决策](https://github.com/SCAU-RM-NAV/rm2022_auto_infantry_ws/tree/main/src/auto_nav/scripts)
-
-## 节点功能
-
-- 订阅来自其他节点的机器人状态、血量、友方位置、视觉识别等消息。
-
-- 根据接收到的消息更新状态机的状态。
-
-- 根据当前状态执行相应的任务，包括巡逻、跟随、攻击和逃跑等。
-
-- 使用 [Nav2_API](/src/rm_decision/Nav2_API.md) 发送导航信息，交由导航节点规划
-
-## 主要类和方法
-
-### AutoFSM类
-
-- `__init__`: 初始化方法，设置节点名称、配置参数、创建订阅器、初始化等。
-
-- `init_wait_for_messages`: 等待各种消息到达的初始化方法。
-
-- `thread_FSM_state`: 状态更新线程，根据接收到的消息更新状态机的状态。
-
-- `state_update`: 实际的状态更新逻辑，根据不同条件更新状态。
-
-- `thread_FSM`: 任务执行线程，根据状态机的状态执行相应任务。
-
-- `mission_random`: 随机巡逻任务，随机选择路径上的目标点进行巡逻。
-
-- `mission_follow`: 跟随任务，根据当前友方机器人的状态选择一个进行跟随。
-
-- `mission_attack`: 攻击任务，暂未实现。
-
-- `mission_escape`: 逃跑任务，根据事先定义的逃跑点进行逃跑（一键回城）
-
-### CallbackMsg类
-
-- `callback_AllRobotHP`: 处理 `AllRobotHP` 消息的回调函数，更新友方和敌方机器人的血量信息。
-
-- `callback_FriendLocation`: 处理 `FriendLocation` 消息的回调函数，更新友方机器人的位置信息。
-
-- `callback_RobotStatus`: 处理 `RobotStatus` 消息的回调函数，更新机器人的状态信息。
-
-- `callback_rm_vision`: 处理 `Armor` 消息（视觉识别）的回调函数，接收视觉部分识别到的装甲板信息，更新敌人距离信息。
-
-### Utility类
-
-- `yaml_read`: 从YAML文件中读取配置，包括目标点、路径和随机点列表。
-
-- `wait_for_message`: 等待指定消息到达的方法。
-
-- `decide_robot_to_follow`: 选择要跟随的机器人的方法，目前选择血量最高的友方机器人。
-
-- `goto_point`: 导航到指定目标点的方法，使用 Nav2 导航节点执行导航任务。
-
-## 使用方法
-
-### 启动有限状态机决策
-
-```Shell
-ros2 launch rm_auto_FSM rm_auto_fsm_launch.py
-```
-
-### 虚拟裁判系统和视觉部分，发布假消息
-
-- robot_status
-
- ```bash
- source install/setup.bash
- ros2 topic pub -r 5 /robot_status rm_decision_interfaces/msg/RobotStatus "{
- robot_id: 7,
- current_hp: 200,
- shooter_heat: 19,
- }"
- ```
-
-- robot_hp
-
- ```bash
- source install/setup.bash
- ros2 topic pub -r 5 /robot_hp rm_decision_interfaces/msg/AllRobotHP "{
- red_1_robot_hp: 200,
- red_2_robot_hp: 90,
- red_3_robot_hp: 100,
- red_4_robot_hp: 40,
- red_5_robot_hp: 200,
- red_7_robot_hp: 200,
- red_outpost_hp: 0,
- red_base_hp: 30,
- blue_1_robot_hp: 95,
- blue_2_robot_hp: 85,
- blue_3_robot_hp: 75,
- blue_4_robot_hp: 200,
- blue_5_robot_hp: 55,
- blue_7_robot_hp: 45,
- blue_outpost_hp: 35,
- blue_base_hp: 25
- }"
- ```
-
-- friend_location
-
- ```bash
- source install/setup.bash
- ros2 topic pub -r 5 /friend_location rm_decision_interfaces/msg/FriendLocation "{
- hero_x: 2.75, hero_y: 0.0,
- engineer_x: 10.36, engineer_y: -3.79,
- standard_3_x: 7.91, standard_3_y: 3.74,
- standard_4_x: 12.00, standard_4_y: 4.15,
- standard_5_x: 18.75, standard_5_y: 0.0
- }"
- ```
-
- `hero`: 己方哨兵出生点
- `engineer`: 能量机关（靠己方）
- `standard_3`: 己方环形高地
- `standard_4`: 敌方前哨站
- `standard_5`: 敌方基地
-
-- armors
-
- ```bash
- source install/setup.bash
- ros2 topic pub -r 5 /detector/armors rm_decision_interfaces/msg/Armor "{
- number: '1',
- type: 'small',
- distance_to_image_center: 0.0,
- pose: {
- position: {x: 0.0, y: 0.0, z: 0.0},
- orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
- }
- }"
- ```
-
-- RFID
-
- ```bash
- source install/setup.bash
- ros2 topic pub -r 5 /rfid rm_decision_interfaces/msg/RFID "{
- rfid_patrol_status: 0
- }"
- ```
diff --git a/src/rm_decision/rm_FSM/launch/rm_auto_fsm_launch.py b/src/rm_decision/rm_FSM/launch/rm_auto_fsm_launch.py
deleted file mode 100644
index 567f1cff884b728db51bbf01b601bcffc8686a90..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/launch/rm_auto_fsm_launch.py
+++ /dev/null
@@ -1,15 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-
-def generate_launch_description():
-
- # start the demo autonomy task
- demo_cmd = Node(
- package='rm_auto_FSM',
- executable='rm_auto_FSM',
- emulate_tty=True,
- output='screen')
-
- ld = LaunchDescription()
- ld.add_action(demo_cmd)
- return ld
diff --git a/src/rm_decision/rm_FSM/media/readme.gif b/src/rm_decision/rm_FSM/media/readme.gif
deleted file mode 100644
index d3a925a252cdf8401bfdddbaf712a5a11571daf1..0000000000000000000000000000000000000000
Binary files a/src/rm_decision/rm_FSM/media/readme.gif and /dev/null differ
diff --git a/src/rm_decision/rm_FSM/package.xml b/src/rm_decision/rm_FSM/package.xml
deleted file mode 100644
index 23deb78e70dfd4359544f320ac6949e6b0993743..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/package.xml
+++ /dev/null
@@ -1,25 +0,0 @@
-
-
-
- rm_auto_FSM
- 0.0.0
- A decision package for RMUC/RMYC sentinel robots implemented based on a finite state machine
- Lihan Chen
- MIT
-
- rclpy
- geometry_msgs
- nav2_msgs
- action_msgs
- lifecycle_msgs
- rm_decision_interfaces
-
- ament_copyright
- ament_flake8
- ament_pep257
- python3-pytest
-
-
- ament_python
-
-
diff --git a/src/rm_decision/rm_FSM/pytest.ini b/src/rm_decision/rm_FSM/pytest.ini
deleted file mode 100644
index 50d6d012576a92f3baee745f7521b79123b0bcb8..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/pytest.ini
+++ /dev/null
@@ -1,2 +0,0 @@
-[pytest]
-junit_family=xunit2
\ No newline at end of file
diff --git a/src/rm_decision/rm_FSM/resource/rm_auto_FSM b/src/rm_decision/rm_FSM/resource/rm_auto_FSM
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/rm_decision/rm_FSM/rm_auto_FSM/__init__.py b/src/rm_decision/rm_FSM/rm_auto_FSM/__init__.py
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/rm_decision/rm_FSM/rm_auto_FSM/callback_msg.py b/src/rm_decision/rm_FSM/rm_auto_FSM/callback_msg.py
deleted file mode 100644
index 6a328e11542675e3386ba6420440b5ff192e96d7..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/rm_auto_FSM/callback_msg.py
+++ /dev/null
@@ -1,127 +0,0 @@
-from rclpy.node import Node
-from rclpy import logging
-
-from enum import Enum
-
-class TeamColor(Enum):
- RED = 0
- BLUE = 1
-
-class CallbackMsg(Node):
- """
- 自定义消息回调类
- """
-
- def __init__(self):
- self.friend_color = 0 # 0-Red, 1-Blue
-
- # Robot HP （1-英雄 2-工程 3-步兵 4-步兵 5-步兵 7-哨兵 8-前哨站 9-基地）
- self.friend_robot_HP = [0] * 10
- self.enemy_robot_HP = [0] * 10
-
- # Robot Status
- self.maximum_hp = 400 # 最大血量
- self.shooter_heat_limit = 400 # 热量上限
-
- self.robot_id = None # 机器人ID
- self.current_hp = None # 当前血量
- self.shooter_heat = None # 发射机构的枪口热量
-
- # Friend Position
- self.friend_position = [[]]
-
- # RM Vision
- self.enemy_dis = None
-
- # RFID
- self.rfid_patrol_status = None
-
- # Custom Variables
- self.percent_hp = None
- self.high_heat_flag = None
-
- def callback_AllRobotHP(self, msg):
- if self.friend_color == TeamColor.RED:
- self.friend_robot_HP[1] = msg.red_1_robot_hp
- self.friend_robot_HP[2] = msg.red_2_robot_hp
- self.friend_robot_HP[3] = msg.red_3_robot_hp
- self.friend_robot_HP[4] = msg.red_4_robot_hp
- self.friend_robot_HP[5] = msg.red_5_robot_hp
- self.friend_robot_HP[7] = msg.red_7_robot_hp # 友方哨兵血量
- self.friend_robot_HP[8] = msg.red_outpost_hp
- self.friend_robot_HP[9] = msg.red_base_hp
- self.enemy_robot_HP[1] = msg.blue_1_robot_hp
- self.enemy_robot_HP[2] = msg.blue_2_robot_hp
- self.enemy_robot_HP[3] = msg.blue_3_robot_hp
- self.enemy_robot_HP[4] = msg.blue_4_robot_hp
- self.enemy_robot_HP[5] = msg.blue_5_robot_hp
- self.enemy_robot_HP[7] = msg.blue_7_robot_hp # 敌方哨兵血量
- self.enemy_robot_HP[8] = msg.blue_outpost_hp
- self.enemy_robot_HP[9] = msg.blue_base_hp
- elif self.friend_color == TeamColor.BLUE:
- self.friend_robot_HP[1] = msg.blue_1_robot_hp
- self.friend_robot_HP[2] = msg.blue_2_robot_hp
- self.friend_robot_HP[3] = msg.blue_3_robot_hp
- self.friend_robot_HP[4] = msg.blue_4_robot_hp
- self.friend_robot_HP[5] = msg.blue_5_robot_hp
- self.friend_robot_HP[7] = msg.blue_7_robot_hp # 友方哨兵血量
- self.friend_robot_HP[8] = msg.blue_outpost_hp
- self.friend_robot_HP[9] = msg.blue_base_hp
- self.enemy_robot_HP[1] = msg.red_1_robot_hp
- self.enemy_robot_HP[2] = msg.red_2_robot_hp
- self.enemy_robot_HP[3] = msg.red_3_robot_hp
- self.enemy_robot_HP[4] = msg.red_4_robot_hp
- self.enemy_robot_HP[5] = msg.red_5_robot_hp
- self.enemy_robot_HP[7] = msg.red_7_robot_hp # 敌方哨兵血量
- self.enemy_robot_HP[8] = msg.red_outpost_hp
- self.enemy_robot_HP[9] = msg.red_base_hp
- else :
- logging.get_logger('AllRobotHP').warn('Cannot identified Enemies and friends!!!')
-
- # logging.get_logger('AllRobotHP').info('Friend HP: ' + str(self.friend_robot_HP))
- # logging.get_logger('AllRobotHP').info('Enemy HP: ' + str(self.enemy_robot_HP))
-
- def callback_FriendLocation(self, msg):
- self.friend_position = [[], # 下标索引从1开始
- [msg.hero_x, msg.hero_y],
- [msg.engineer_x, msg.engineer_y],
- [msg.standard_3_x, msg.standard_3_y],
- [msg.standard_4_x, msg.standard_4_y],
- [msg.standard_5_x, msg.standard_5_y]]
-
- def callback_RobotStatus(self, msg):
- self.robot_id = msg.robot_id
- self.current_hp = msg.current_hp
- self.shooter_heat = msg.shooter_heat
-
- self.percent_hp = self.current_hp / self.maximum_hp
-
- if self.shooter_heat > (self.shooter_heat_limit - 5):
- self.high_heat_flag = 1
- else:
- self.high_heat_flag = 0
-
- '''
- 根据哨兵机器人ID，判断我方阵营颜色
- '''
- if 1 <= self.robot_id <= 7:
- self.friend_color = TeamColor.RED
- # logging.get_logger('RobotStatus').info('We are: RED')
- elif 101 <= self.robot_id <= 107:
- self.friend_color = TeamColor.BLUE
- # logging.get_logger('RobotStatus').info('We are: BLUE')
- else:
- logging.get_logger('RobotStatus').warn('Cannot identify team color')
-
- # DEBUG
- # logging.get_logger('RobotStatus').info('We are: ' + str(self.friend_color.name))
- # logging.get_logger('RobotStatus').info('self.robot_id: ' + str(self.robot_id))
- # logging.get_logger('RobotStatus').info('self.current_hp: ' + str(self.current_hp))
- # logging.get_logger('RobotStatus').info('self.shooter_heat: ' + str(self.shooter_heat))
- # logging.get_logger('RobotStatus').info(' ------------------------------------- ')
-
- def callback_rm_vision(self, msg):
- self.enemy_dis = msg.distance_to_image_center
-
- def callback_RFID(self, msg):
- self.rfid_patrol_status = msg.rfid_patrol_status
\ No newline at end of file
diff --git a/src/rm_decision/rm_FSM/rm_auto_FSM/config/goal.yaml b/src/rm_decision/rm_FSM/rm_auto_FSM/config/goal.yaml
deleted file mode 100644
index 5bc661e21f75b41f17d82bc749ca7d1061292979..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/rm_auto_FSM/config/goal.yaml
+++ /dev/null
@@ -1,13 +0,0 @@
-1: [2.75,0.0] # 己方哨兵出生点
-2: [10.36,-3.79] # 己方前哨站
-3: [12.00,4.15] # 敌方前哨站
-4: [12.3,0.96] # 能量机关（靠敌方）
-5: [9.86,-1.17] # 能量机关（靠己方）
-6: [7.91,3.74] # 己方环形高地
-7: [14.89,-5.62] # 敌方银矿
-8: [6.70,6.15] # 己方银矿
-9: [18.75,0.0] # 敌方基地
-
-path: [1,2,3,4,5,6,7,8,9]
-
-random_list: [1,2,4,5,6,7,8,9,9,9,3,3,3]
\ No newline at end of file
diff --git a/src/rm_decision/rm_FSM/rm_auto_FSM/rm_auto_FSM.py b/src/rm_decision/rm_FSM/rm_auto_FSM/rm_auto_FSM.py
deleted file mode 100644
index 1ce03c39c73ffd1de4fed1808a68aa69b57594e2..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/rm_auto_FSM/rm_auto_FSM.py
+++ /dev/null
@@ -1,242 +0,0 @@
-import os
-import inspect
-import threading
-import random
-import time
-
-import rclpy
-from rclpy.node import Node
-from rclpy.executors import MultiThreadedExecutor
-from rclpy.qos import QoSProfile
-from rclpy import qos
-
-from geometry_msgs.msg import Twist, PoseStamped
-from rm_decision_interfaces.msg import RobotStatus, FriendLocation, AllRobotHP, Armor, RFID
-
-from rm_auto_FSM.callback_msg import CallbackMsg
-from rm_auto_FSM.robot_navigator import BasicNavigator
-from rm_auto_FSM.utility import Utility
-
-class AutoFSM(Node):
- """
- 深圳北理莫斯科大学 北极熊战队 哨兵决策部分
- Attention: 开发中, 有点乱！
- """
-
- def __init__(self):
- super().__init__('auto_fsm_node')
-
- # Set Config
- self.set_unhealth_point_percent = 0.3
- self.set_enemy_max_dis = 5.0
- self.set_attack_blacklist = [1, 1, 1, 1, 1, 1, 1, 1]
- self.yaml_name = '/goal.yaml'
- self.yaml_path = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) + '/config' + self.yaml_name
- self.goal_dict = dict()
- self.goal_path = []
- self.nav_goal_name = []
- self.random_list = []
- self.nav_timeout = 10.0
- self.interval_time = 1.0
- self.follow_robot_id = None
- self.out_patrol_time = 0
- self.msg_callback = CallbackMsg()
- self.FSM_state = 2
-
- # Create subscription
- self.robot_hp_sub = self.create_subscription(AllRobotHP,
- '/robot_hp',
- self.msg_callback.callback_AllRobotHP,
- QoSProfile(depth=1))
- self.enemy_locate_sub = self.create_subscription(FriendLocation,
- '/friend_location',
- self.msg_callback.callback_FriendLocation,
- QoSProfile(depth=1))
- self.robot_states_sub = self.create_subscription(RobotStatus,
- '/robot_status',
- self.msg_callback.callback_RobotStatus,
- QoSProfile(depth=1))
- self.rm_vision_sub = self.create_subscription(Armor,
- '/detector/armors',
- self.msg_callback.callback_rm_vision,
- qos.qos_profile_sensor_data)
- self.RFID_sub = self.create_subscription(RFID,
- '/rfid',
- self.msg_callback.callback_RFID,
- QoSProfile(depth=1))
-
- # Set rate
- self.rate = self.create_rate(0.5)
- self.rate_nav = self.create_rate(100)
- self.rate_RFID = self.create_rate(1)
-
- # Init function
- Utility.yaml_read(self)
- self.init_wait_for_messages()
-
- # Connect to Navigation
- self.navigator = BasicNavigator()
- self.navigator.get_logger().info('等待 navigator 服务器连接...')
- self.navigator.waitUntilNav2Active()
- self.navigator.get_logger().info('成功连接导航服务')
-
- time.sleep(3) # 停滞三秒，让所有 msg 都被正常接收
-
- # Create Threads
- self.thread_FSM_ = threading.Thread(target=self.thread_FSM)
- self.thread_FSM_.start()
-
- self.thread_FSM_state_ = threading.Thread(target=self.thread_FSM_state)
- self.thread_FSM_state_.start()
-
- self.thread_RFID_ = threading.Thread(target=self.thread_RFID)
- self.thread_RFID_.start()
-
- def init_wait_for_messages(self):
- message_topics = [
- (RobotStatus, '/robot_status'),
- (AllRobotHP, '/robot_hp'),
- (FriendLocation, '/friend_location'),
- (Armor, '/detector/armors'),
- (RFID, '/rfid')
- ]
- for message_type, topic in message_topics:
- self.get_logger().info(f'等待 {topic} 消息')
- Utility.wait_for_message(self, message_type, topic)
-
- def thread_RFID(self):
- while rclpy.ok():
- if self.msg_callback.rfid_patrol_status == 0:
- # Not detected patrol_RFID, the robot is not in the patrol area, timer counts in seconds
- self.out_patrol_time += 1
- self.get_logger().info('out_patrol_time = ' + str(self.out_patrol_time))
- else:
- # Detected patrol_RFID, reset the timer
- self.get_logger().info('Detected patrol RFID')
- self.out_patrol_time = 0
- self.rate_RFID.sleep()
-
- def thread_FSM_state(self):
- while rclpy.ok():
- self.state_update()
- self.rate_nav.sleep()
-
- def state_update(self):
- if self.msg_callback.percent_hp < self.set_unhealth_point_percent or self.msg_callback.high_heat_flag == 1 or self.out_patrol_time > 30:
- '''
- 血量百分比低于设定值 或 枪口热量过高 或 哨兵离开巡逻区过长，进入逃跑状态
- '''
- self.FSM_state = 4
-
- elif self.msg_callback.enemy_dis != 0 and self.msg_callback.enemy_dis <= self.set_enemy_max_dis and self.msg_callback.high_heat_flag == 0 :
- '''
- 视觉识别到敌人，进入攻击状态
- '''
- self.FSM_state = 3
-
- elif self.msg_callback.friend_robot_HP[8] == 0 and self.msg_callback.high_heat_flag == 0:
- '''
- 己方前哨站被击毁，进入跟随状态
- '''
- self.FSM_state = 2
-
- elif self.msg_callback.friend_robot_HP[8] != 0 and self.msg_callback.high_heat_flag == 0:
- '''
- 己方前哨站存活，进入巡逻状态
- '''
- self.FSM_state = 1
-
-
- # elif self.game_state !=4:
- # self.FSM_state =0 #休眠状态
- # self.get_logger().info('进入休眠状态')
-
- def thread_FSM(self):
- self.get_logger().info('准备完成, 等待比赛开始')
- while rclpy.ok():
- if self.FSM_state == 1:
- self.mission_random() # 巡逻模式
- elif self.FSM_state == 2:
- self.mission_follow() # 跟随模式
- elif self.FSM_state == 3:
- self.mission_attack() # 攻击模式
- elif self.FSM_state == 4:
- self.mission_escape() # 逃跑模式
-
- def mission_random(self):
- self.get_logger().info("开始随机跑点")
-
- while self.FSM_state == 1 and rclpy.ok():
- random_num = random.randint(0, len(self.random_list) - 1)
- reach_result = Utility.goto_point(self, self.goal_dict[self.random_list[random_num]], self.nav_timeout)
- if reach_result == 1:
- self.get_logger().info("到达目标点 " + str(self.random_list[random_num]))
- reach_result = 0
- self.rate.sleep()
- self.get_logger().info("退出随机跑点")
-
- def mission_follow(self):
- self.get_logger().info("开始跟随")
-
- while self.FSM_state == 2 and self.msg_callback.percent_hp > 0.3 and rclpy.ok():
- follow_robot_id = Utility.decide_robot_to_follow(self)
-
- # 检查是否出现了不同的follow_robot_id
- if follow_robot_id != self.follow_robot_id:
- self.get_logger().info("切换到跟随" + str(follow_robot_id) + "号机器人")
- self.navigator.cancelTask()
-
- self.follow_robot_id = follow_robot_id
-
- goal_pose = PoseStamped()
- goal_pose.header.frame_id = 'map'
- goal_pose.header.stamp = self.get_clock().now().to_msg()
- goal_pose.pose.position.x = self.msg_callback.friend_position[follow_robot_id][0]
- goal_pose.pose.position.y = self.msg_callback.friend_position[follow_robot_id][1]
- goal_pose.pose.orientation.w = 0.0
-
- self.navigator.goToPose(goal_pose)
-
- self.rate.sleep()
-
- self.get_logger().info("退出跟随")
-
- def mission_attack(self):
- self.get_logger().info("开始攻击")
- while self.FSM_state == 3 and rclpy.ok():
- self.get_logger().info("正在攻击状态中...")
- self.rate_nav.sleep()
- self.get_logger().info("退出攻击")
-
- def mission_escape(self):
- self.get_logger().info("开始逃跑")
- while self.FSM_state == 4 and rclpy.ok():
- self.get_logger().info("正在逃跑状态中...")
-
- goal_pose = PoseStamped()
- goal_pose.header.frame_id = 'map'
- goal_pose.header.stamp = self.get_clock().now().to_msg()
- goal_pose.pose.position.x = self.goal_dict[self.random_list[0]][0]
- goal_pose.pose.position.y = self.goal_dict[self.random_list[0]][1]
- goal_pose.pose.orientation.w = 0.0
-
- self.navigator.goToPose(goal_pose)
- self.rate.sleep()
- self.get_logger().info("退出逃跑")
-
-def main():
- rclpy.init()
- auto_fsm = AutoFSM()
-
- executor = MultiThreadedExecutor()
- executor.add_node(auto_fsm)
-
- try:
- executor.spin()
- finally:
- executor.shutdown()
- auto_fsm.destroy_node()
- rclpy.shutdown()
-
-if __name__ == '__main__':
- main()
diff --git a/src/rm_decision/rm_FSM/rm_auto_FSM/robot_navigator.py b/src/rm_decision/rm_FSM/rm_auto_FSM/robot_navigator.py
deleted file mode 100644
index 0f7e2232c66ab1f9f46a0a791babec97db29f6d4..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/rm_auto_FSM/robot_navigator.py
+++ /dev/null
@@ -1,593 +0,0 @@
-#! /usr/bin/env python3
-# Copyright 2021 Samsung Research America
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-
-from enum import Enum
-import time
-
-from action_msgs.msg import GoalStatus
-from builtin_interfaces.msg import Duration
-from geometry_msgs.msg import Point
-from geometry_msgs.msg import PoseStamped
-from geometry_msgs.msg import PoseWithCovarianceStamped
-from lifecycle_msgs.srv import GetState
-from nav2_msgs.action import AssistedTeleop, BackUp, Spin
-from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose
-from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose
-from nav2_msgs.action import SmoothPath
-from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes
-
-import rclpy
-from rclpy.action import ActionClient
-from rclpy.duration import Duration as rclpyDuration
-from rclpy.node import Node
-from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy
-from rclpy.qos import QoSProfile, QoSReliabilityPolicy
-
-
-class TaskResult(Enum):
- UNKNOWN = 0
- SUCCEEDED = 1
- CANCELED = 2
- FAILED = 3
-
-
-class BasicNavigator(Node):
-
- def __init__(self, node_name='basic_navigator', namespace=''):
- super().__init__(node_name=node_name, namespace=namespace)
- self.initial_pose = PoseStamped()
- self.initial_pose.header.frame_id = 'map'
- self.goal_handle = None
- self.result_future = None
- self.feedback = None
- self.status = None
-
- amcl_pose_qos = QoSProfile(
- durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
- reliability=QoSReliabilityPolicy.RELIABLE,
- history=QoSHistoryPolicy.KEEP_LAST,
- depth=1)
-
- self.initial_pose_received = False
- self.nav_through_poses_client = ActionClient(self,
- NavigateThroughPoses,
- 'navigate_through_poses')
- self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
- self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')
- self.follow_path_client = ActionClient(self, FollowPath, 'follow_path')
- self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose,
- 'compute_path_to_pose')
- self.compute_path_through_poses_client = ActionClient(self, ComputePathThroughPoses,
- 'compute_path_through_poses')
- self.smoother_client = ActionClient(self, SmoothPath, 'smooth_path')
- self.spin_client = ActionClient(self, Spin, 'spin')
- self.backup_client = ActionClient(self, BackUp, 'backup')
- self.assisted_teleop_client = ActionClient(self, AssistedTeleop, 'assisted_teleop')
- self.localization_pose_sub = self.create_subscription(PoseWithCovarianceStamped,
- 'amcl_pose',
- self._amclPoseCallback,
- amcl_pose_qos)
- self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped,
- 'initialpose',
- 10)
- self.change_maps_srv = self.create_client(LoadMap, 'map_server/load_map')
- self.clear_costmap_global_srv = self.create_client(
- ClearEntireCostmap, 'global_costmap/clear_entirely_global_costmap')
- self.clear_costmap_local_srv = self.create_client(
- ClearEntireCostmap, 'local_costmap/clear_entirely_local_costmap')
- self.get_costmap_global_srv = self.create_client(GetCostmap, 'global_costmap/get_costmap')
- self.get_costmap_local_srv = self.create_client(GetCostmap, 'local_costmap/get_costmap')
-
- def destroyNode(self):
- self.destroy_node()
-
- def destroy_node(self):
- self.nav_through_poses_client.destroy()
- self.nav_to_pose_client.destroy()
- self.follow_waypoints_client.destroy()
- self.follow_path_client.destroy()
- self.compute_path_to_pose_client.destroy()
- self.compute_path_through_poses_client.destroy()
- self.smoother_client.destroy()
- self.spin_client.destroy()
- self.backup_client.destroy()
- super().destroy_node()
-
- def setInitialPose(self, initial_pose):
- """Set the initial pose to the localization system."""
- self.initial_pose_received = False
- self.initial_pose = initial_pose
- self._setInitialPose()
-
- def goThroughPoses(self, poses, behavior_tree=''):
- """Send a `NavThroughPoses` action request."""
- self.debug("Waiting for 'NavigateThroughPoses' action server")
- while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0):
- self.info("'NavigateThroughPoses' action server not available, waiting...")
-
- goal_msg = NavigateThroughPoses.Goal()
- goal_msg.poses = poses
- goal_msg.behavior_tree = behavior_tree
-
- self.info(f'Navigating with {len(goal_msg.poses)} goals....')
- send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg,
- self._feedbackCallback)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error(f'Goal with {len(poses)} poses was rejected!')
- return False
-
- self.result_future = self.goal_handle.get_result_async()
- return True
-
- def goToPose(self, pose, behavior_tree=''):
- """Send a `NavToPose` action request."""
- self.debug("Waiting for 'NavigateToPose' action server")
- while not self.nav_to_pose_client.wait_for_server(timeout_sec=1.0):
- self.info("'NavigateToPose' action server not available, waiting...")
-
- goal_msg = NavigateToPose.Goal()
- goal_msg.pose = pose
- goal_msg.behavior_tree = behavior_tree
-
- self.info('Navigating to goal: ' + str(pose.pose.position.x) + ' ' +
- str(pose.pose.position.y) + '...')
- send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg,
- self._feedbackCallback)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error('Goal to ' + str(pose.pose.position.x) + ' ' +
- str(pose.pose.position.y) + ' was rejected!')
- return False
-
- self.result_future = self.goal_handle.get_result_async()
- return True
-
- def followWaypoints(self, poses):
- """Send a `FollowWaypoints` action request."""
- self.debug("Waiting for 'FollowWaypoints' action server")
- while not self.follow_waypoints_client.wait_for_server(timeout_sec=1.0):
- self.info("'FollowWaypoints' action server not available, waiting...")
-
- goal_msg = FollowWaypoints.Goal()
- goal_msg.poses = poses
-
- self.info(f'Following {len(goal_msg.poses)} goals....')
- send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg,
- self._feedbackCallback)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error(f'Following {len(poses)} waypoints request was rejected!')
- return False
-
- self.result_future = self.goal_handle.get_result_async()
- return True
-
- def spin(self, spin_dist=1.57, time_allowance=10):
- self.debug("Waiting for 'Spin' action server")
- while not self.spin_client.wait_for_server(timeout_sec=1.0):
- self.info("'Spin' action server not available, waiting...")
- goal_msg = Spin.Goal()
- goal_msg.target_yaw = spin_dist
- goal_msg.time_allowance = Duration(sec=time_allowance)
-
- self.info(f'Spinning to angle {goal_msg.target_yaw}....')
- send_goal_future = self.spin_client.send_goal_async(goal_msg, self._feedbackCallback)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error('Spin request was rejected!')
- return False
-
- self.result_future = self.goal_handle.get_result_async()
- return True
-
- def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10):
- self.debug("Waiting for 'Backup' action server")
- while not self.backup_client.wait_for_server(timeout_sec=1.0):
- self.info("'Backup' action server not available, waiting...")
- goal_msg = BackUp.Goal()
- goal_msg.target = Point(x=float(backup_dist))
- goal_msg.speed = backup_speed
- goal_msg.time_allowance = Duration(sec=time_allowance)
-
- self.info(f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....')
- send_goal_future = self.backup_client.send_goal_async(goal_msg, self._feedbackCallback)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error('Backup request was rejected!')
- return False
-
- self.result_future = self.goal_handle.get_result_async()
- return True
-
- def assistedTeleop(self, time_allowance=30):
- self.debug("Wainting for 'assisted_teleop' action server")
- while not self.assisted_teleop_client.wait_for_server(timeout_sec=1.0):
- self.info("'assisted_teleop' action server not available, waiting...")
- goal_msg = AssistedTeleop.Goal()
- goal_msg.time_allowance = Duration(sec=time_allowance)
-
- self.info("Running 'assisted_teleop'....")
- send_goal_future = \
- self.assisted_teleop_client.send_goal_async(goal_msg, self._feedbackCallback)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error('Assisted Teleop request was rejected!')
- return False
-
- self.result_future = self.goal_handle.get_result_async()
- return True
-
- def followPath(self, path, controller_id='', goal_checker_id=''):
- """Send a `FollowPath` action request."""
- self.debug("Waiting for 'FollowPath' action server")
- while not self.follow_path_client.wait_for_server(timeout_sec=1.0):
- self.info("'FollowPath' action server not available, waiting...")
-
- goal_msg = FollowPath.Goal()
- goal_msg.path = path
- goal_msg.controller_id = controller_id
- goal_msg.goal_checker_id = goal_checker_id
-
- self.info('Executing path...')
- send_goal_future = self.follow_path_client.send_goal_async(goal_msg,
- self._feedbackCallback)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error('Follow path was rejected!')
- return False
-
- self.result_future = self.goal_handle.get_result_async()
- return True
-
- def cancelTask(self):
- """Cancel pending task request of any type."""
- self.info('Canceling current task.')
- if self.result_future:
- future = self.goal_handle.cancel_goal_async()
- rclpy.spin_until_future_complete(self, future)
- return
-
- def isTaskComplete(self):
- """Check if the task request of any type is complete yet."""
- if not self.result_future:
- # task was cancelled or completed
- return True
- rclpy.spin_until_future_complete(self, self.result_future, timeout_sec=0.10)
- if self.result_future.result():
- self.status = self.result_future.result().status
- if self.status != GoalStatus.STATUS_SUCCEEDED:
- self.debug(f'Task with failed with status code: {self.status}')
- return True
- else:
- # Timed out, still processing, not complete yet
- return False
-
- self.debug('Task succeeded!')
- return True
-
- def getFeedback(self):
- """Get the pending action feedback message."""
- return self.feedback
-
- def getResult(self):
- """Get the pending action result message."""
- if self.status == GoalStatus.STATUS_SUCCEEDED:
- return TaskResult.SUCCEEDED
- elif self.status == GoalStatus.STATUS_ABORTED:
- return TaskResult.FAILED
- elif self.status == GoalStatus.STATUS_CANCELED:
- return TaskResult.CANCELED
- else:
- return TaskResult.UNKNOWN
-
- def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
- """Block until the full navigation system is up and running."""
- # self._waitForNodeToActivate(localizer)
- # if localizer == 'amcl':
- # self._waitForInitialPose()
- self._waitForNodeToActivate(navigator)
- self.info('Nav2 is ready for use!')
- return
-
- def _getPathImpl(self, start, goal, planner_id='', use_start=False):
- """
- Send a `ComputePathToPose` action request.
-
- Internal implementation to get the full result, not just the path.
- """
- self.debug("Waiting for 'ComputePathToPose' action server")
- while not self.compute_path_to_pose_client.wait_for_server(timeout_sec=1.0):
- self.info("'ComputePathToPose' action server not available, waiting...")
-
- goal_msg = ComputePathToPose.Goal()
- goal_msg.start = start
- goal_msg.goal = goal
- goal_msg.planner_id = planner_id
- goal_msg.use_start = use_start
-
- self.info('Getting path...')
- send_goal_future = self.compute_path_to_pose_client.send_goal_async(goal_msg)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error('Get path was rejected!')
- return None
-
- self.result_future = self.goal_handle.get_result_async()
- rclpy.spin_until_future_complete(self, self.result_future)
- self.status = self.result_future.result().status
- if self.status != GoalStatus.STATUS_SUCCEEDED:
- self.warn(f'Getting path failed with status code: {self.status}')
- return None
-
- return self.result_future.result().result
-
- def getPath(self, start, goal, planner_id='', use_start=False):
- """Send a `ComputePathToPose` action request."""
- rtn = self._getPathImpl(start, goal, planner_id, use_start)
- if not rtn:
- return None
- else:
- return rtn.path
-
- def getPathThroughPoses(self, start, goals, planner_id='', use_start=False):
- """Send a `ComputePathThroughPoses` action request."""
- self.debug("Waiting for 'ComputePathThroughPoses' action server")
- while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0):
- self.info("'ComputePathThroughPoses' action server not available, waiting...")
-
- goal_msg = ComputePathThroughPoses.Goal()
- goal_msg.start = start
- goal_msg.goals = goals
- goal_msg.planner_id = planner_id
- goal_msg.use_start = use_start
-
- self.info('Getting path...')
- send_goal_future = self.compute_path_through_poses_client.send_goal_async(goal_msg)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error('Get path was rejected!')
- return None
-
- self.result_future = self.goal_handle.get_result_async()
- rclpy.spin_until_future_complete(self, self.result_future)
- self.status = self.result_future.result().status
- if self.status != GoalStatus.STATUS_SUCCEEDED:
- self.warn(f'Getting path failed with status code: {self.status}')
- return None
-
- return self.result_future.result().result.path
-
- def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
- """
- Send a `SmoothPath` action request.
-
- Internal implementation to get the full result, not just the path.
- """
- self.debug("Waiting for 'SmoothPath' action server")
- while not self.smoother_client.wait_for_server(timeout_sec=1.0):
- self.info("'SmoothPath' action server not available, waiting...")
-
- goal_msg = SmoothPath.Goal()
- goal_msg.path = path
- goal_msg.max_smoothing_duration = rclpyDuration(seconds=max_duration).to_msg()
- goal_msg.smoother_id = smoother_id
- goal_msg.check_for_collisions = check_for_collision
-
- self.info('Smoothing path...')
- send_goal_future = self.smoother_client.send_goal_async(goal_msg)
- rclpy.spin_until_future_complete(self, send_goal_future)
- self.goal_handle = send_goal_future.result()
-
- if not self.goal_handle.accepted:
- self.error('Smooth path was rejected!')
- return None
-
- self.result_future = self.goal_handle.get_result_async()
- rclpy.spin_until_future_complete(self, self.result_future)
- self.status = self.result_future.result().status
- if self.status != GoalStatus.STATUS_SUCCEEDED:
- self.warn(f'Getting path failed with status code: {self.status}')
- return None
-
- return self.result_future.result().result
-
- def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
- """Send a `SmoothPath` action request."""
- rtn = self._smoothPathImpl(
- self, path, smoother_id, max_duration, check_for_collision)
- if not rtn:
- return None
- else:
- return rtn.path
-
- def changeMap(self, map_filepath):
- """Change the current static map in the map server."""
- while not self.change_maps_srv.wait_for_service(timeout_sec=1.0):
- self.info('change map service not available, waiting...')
- req = LoadMap.Request()
- req.map_url = map_filepath
- future = self.change_maps_srv.call_async(req)
- rclpy.spin_until_future_complete(self, future)
- status = future.result().result
- if status != LoadMap.Response().RESULT_SUCCESS:
- self.error('Change map request failed!')
- else:
- self.info('Change map request was successful!')
- return
-
- def clearAllCostmaps(self):
- """Clear all costmaps."""
- self.clearLocalCostmap()
- self.clearGlobalCostmap()
- return
-
- def clearLocalCostmap(self):
- """Clear local costmap."""
- while not self.clear_costmap_local_srv.wait_for_service(timeout_sec=1.0):
- self.info('Clear local costmaps service not available, waiting...')
- req = ClearEntireCostmap.Request()
- future = self.clear_costmap_local_srv.call_async(req)
- rclpy.spin_until_future_complete(self, future)
- return
-
- def clearGlobalCostmap(self):
- """Clear global costmap."""
- while not self.clear_costmap_global_srv.wait_for_service(timeout_sec=1.0):
- self.info('Clear global costmaps service not available, waiting...')
- req = ClearEntireCostmap.Request()
- future = self.clear_costmap_global_srv.call_async(req)
- rclpy.spin_until_future_complete(self, future)
- return
-
- def getGlobalCostmap(self):
- """Get the global costmap."""
- while not self.get_costmap_global_srv.wait_for_service(timeout_sec=1.0):
- self.info('Get global costmaps service not available, waiting...')
- req = GetCostmap.Request()
- future = self.get_costmap_global_srv.call_async(req)
- rclpy.spin_until_future_complete(self, future)
- return future.result().map
-
- def getLocalCostmap(self):
- """Get the local costmap."""
- while not self.get_costmap_local_srv.wait_for_service(timeout_sec=1.0):
- self.info('Get local costmaps service not available, waiting...')
- req = GetCostmap.Request()
- future = self.get_costmap_local_srv.call_async(req)
- rclpy.spin_until_future_complete(self, future)
- return future.result().map
-
- def lifecycleStartup(self):
- """Startup nav2 lifecycle system."""
- self.info('Starting up lifecycle nodes based on lifecycle_manager.')
- for srv_name, srv_type in self.get_service_names_and_types():
- if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
- self.info(f'Starting up {srv_name}')
- mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
- while not mgr_client.wait_for_service(timeout_sec=1.0):
- self.info(f'{srv_name} service not available, waiting...')
- req = ManageLifecycleNodes.Request()
- req.command = ManageLifecycleNodes.Request().STARTUP
- future = mgr_client.call_async(req)
-
- # starting up requires a full map->odom->base_link TF tree
- # so if we're not successful, try forwarding the initial pose
- while True:
- rclpy.spin_until_future_complete(self, future, timeout_sec=0.10)
- if not future:
- self._waitForInitialPose()
- else:
- break
- self.info('Nav2 is ready for use!')
- return
-
- def lifecycleShutdown(self):
- """Shutdown nav2 lifecycle system."""
- self.info('Shutting down lifecycle nodes based on lifecycle_manager.')
- for srv_name, srv_type in self.get_service_names_and_types():
- if srv_type[0] == 'nav2_msgs/srv/ManageLifecycleNodes':
- self.info(f'Shutting down {srv_name}')
- mgr_client = self.create_client(ManageLifecycleNodes, srv_name)
- while not mgr_client.wait_for_service(timeout_sec=1.0):
- self.info(f'{srv_name} service not available, waiting...')
- req = ManageLifecycleNodes.Request()
- req.command = ManageLifecycleNodes.Request().SHUTDOWN
- future = mgr_client.call_async(req)
- rclpy.spin_until_future_complete(self, future)
- future.result()
- return
-
- def _waitForNodeToActivate(self, node_name):
- # Waits for the node within the tester namespace to become active
- self.debug(f'Waiting for {node_name} to become active..')
- node_service = f'{node_name}/get_state'
- state_client = self.create_client(GetState, node_service)
- while not state_client.wait_for_service(timeout_sec=1.0):
- self.info(f'{node_service} service not available, waiting...')
-
- req = GetState.Request()
- state = 'unknown'
- while state != 'active':
- self.debug(f'Getting {node_name} state...')
- future = state_client.call_async(req)
- rclpy.spin_until_future_complete(self, future)
- if future.result() is not None:
- state = future.result().current_state.label
- self.debug(f'Result of get_state: {state}')
- time.sleep(2)
- return
-
- def _waitForInitialPose(self):
- while not self.initial_pose_received:
- self.info('Setting initial pose')
- self._setInitialPose()
- self.info('Waiting for amcl_pose to be received')
- rclpy.spin_once(self, timeout_sec=1.0)
- return
-
- def _amclPoseCallback(self, msg):
- self.debug('Received amcl pose')
- self.initial_pose_received = True
- return
-
- def _feedbackCallback(self, msg):
- self.debug('Received action feedback message')
- self.feedback = msg.feedback
- return
-
- def _setInitialPose(self):
- msg = PoseWithCovarianceStamped()
- msg.pose.pose = self.initial_pose.pose
- msg.header.frame_id = self.initial_pose.header.frame_id
- msg.header.stamp = self.initial_pose.header.stamp
- self.info('Publishing Initial Pose')
- self.initial_pose_pub.publish(msg)
- return
-
- def info(self, msg):
- self.get_logger().info(msg)
- return
-
- def warn(self, msg):
- self.get_logger().warn(msg)
- return
-
- def error(self, msg):
- self.get_logger().error(msg)
- return
-
- def debug(self, msg):
- self.get_logger().debug(msg)
- return
diff --git a/src/rm_decision/rm_FSM/rm_auto_FSM/utility.py b/src/rm_decision/rm_FSM/rm_auto_FSM/utility.py
deleted file mode 100644
index 415fb277e6c1139b3ba02f5e0645b30a1d6e0611..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/rm_auto_FSM/utility.py
+++ /dev/null
@@ -1,110 +0,0 @@
-import rclpy
-from rclpy.duration import Duration
-from geometry_msgs.msg import PoseStamped
-from rm_auto_FSM.robot_navigator import TaskResult
-import time
-import yaml
-
-
-class Utility:
-
- def __init__(self):
- self.goal_dict = None
- self.goal_path = None
- self.nav_goal_name = None
- self.random_list = None
- self.msg_callback = None
- self.yaml_path = None
-
- def yaml_read(self):
- with open(self.yaml_path, 'r', encoding='utf-8') as f:
- cfg = f.read()
- self.goal_dict = yaml.safe_load(cfg)
- self.nav_goal_name = list(self.goal_dict.keys())
- self.goal_path = self.goal_dict['path']
- self.random_list = self.goal_dict['random_list']
-
- @staticmethod
- def wait_for_message(node, topic_type, topic):
- class _vfm(object):
- def __init__(self):
- self.msg = None
-
- def cb(self, msg):
- self.msg = msg
-
- vfm = _vfm()
- subscription = node.create_subscription(topic_type, topic, vfm.cb, 1)
- while rclpy.ok():
- if vfm.msg is not None:
- return vfm.msg
- rclpy.spin_once(node)
- time.sleep(0.001)
- # unsubscription
- subscription.destroy()
-
- def decide_robot_to_follow(self):
- """
- 选择要跟随的机器人。
- """
- # 寻找血量最高的友方机器人
- max_hp = 0
- robot_id = 0
- for i in [1, 3, 4, 5]: # 仅考虑跟随英雄, 三号步兵, 四号步兵, 五号步兵
- if self.msg_callback.friend_robot_HP[i] > max_hp:
- max_hp = self.msg_callback.friend_robot_HP[i]
- robot_id = i
- self.get_logger().info('跟随 ' + str(robot_id) + ' 号机器人')
- return robot_id
-
- def goto_point(self, goal_point, nav_timeout):
- """
- 导航到给定的目标点。
-
- Args:
- - goal_point: 目标点的坐标
- - nav_timeout: 导航超时时间
-
- Returns:
- - 1-到达目标, 0-失败
- """
- # 设置 Nav 目标点
- goal_pose = PoseStamped()
- goal_pose.header.frame_id = 'map'
- goal_pose.header.stamp = self.get_clock().now().to_msg()
- goal_pose.pose.position.x = goal_point[0]
- goal_pose.pose.position.y = goal_point[1]
- goal_pose.pose.orientation.w = 0.0
-
- # 发送 Nav 目标点
- self.navigator.goToPose(goal_pose)
-
- self.last_FSM_state = self.FSM_state
-
- while not self.navigator.isTaskComplete():
-
- # 处理反馈
- feedback = self.navigator.getFeedback()
- if Duration.from_msg(feedback.navigation_time) > Duration(seconds=nav_timeout):
- self.get_logger().info('导航超时')
- self.navigator.cancelTask()
- return 0
-
- if self.last_FSM_state != self.FSM_state:
- self.navigator.cancelTask()
- self.get_logger().info('状态更换, 导航取消')
- return 0
-
- # 根据返回代码执行某些操作
- result = self.navigator.getResult()
- if result == TaskResult.SUCCEEDED:
- print('Goal succeeded!')
- return 1
- elif result == TaskResult.CANCELED:
- print('Goal was canceled!')
- return 0
- elif result == TaskResult.FAILED:
- print('Goal failed!')
- return 0
- else:
- print('Goal has an invalid return status!')
diff --git a/src/rm_decision/rm_FSM/setup.cfg b/src/rm_decision/rm_FSM/setup.cfg
deleted file mode 100644
index 47e0fbc0ff09a996bfa6f5456b0b96de16dccc78..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/setup.cfg
+++ /dev/null
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/rm_auto_FSM
-[install]
-install_scripts=$base/lib/rm_auto_FSM
diff --git a/src/rm_decision/rm_FSM/setup.py b/src/rm_decision/rm_FSM/setup.py
deleted file mode 100644
index bb2c71e32c77704b01a383a4d6381dcdd465c521..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/setup.py
+++ /dev/null
@@ -1,30 +0,0 @@
-from glob import glob
-import os
-
-from setuptools import setup
-
-package_name = 'rm_auto_FSM'
-
-setup(
- name=package_name,
- version='1.0.0',
- packages=[package_name],
- data_files=[
- ('share/ament_index/resource_index/packages',
- ['resource/' + package_name]),
- ('share/' + package_name, ['package.xml']),
- (os.path.join('share', package_name), glob('launch/*')),
- ],
- install_requires=['setuptools'],
- zip_safe=True,
- maintainer='LihanChen',
- maintainer_email='1120220476@smbu.edu.cn',
- description='A decision package for RMUC/RMUL sentinel robots implemented based on a finite state machine',
- license='MIT',
- tests_require=['pytest'],
- entry_points={
- 'console_scripts': [
- 'rm_auto_FSM = rm_auto_FSM.rm_auto_FSM:main',
- ],
- },
-)
diff --git a/src/rm_decision/rm_FSM/test/test_copyright.py b/src/rm_decision/rm_FSM/test/test_copyright.py
deleted file mode 100644
index cc8ff03f79abb8eb061827a24f2a331e052c5c37..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/test/test_copyright.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_copyright.main import main
-import pytest
-
-
-@pytest.mark.copyright
-@pytest.mark.linter
-def test_copyright():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found errors'
diff --git a/src/rm_decision/rm_FSM/test/test_flake8.py b/src/rm_decision/rm_FSM/test/test_flake8.py
deleted file mode 100644
index 27ee1078ff077cc3a0fec75b7d023101a68164d1..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/test/test_flake8.py
+++ /dev/null
@@ -1,25 +0,0 @@
-# Copyright 2017 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_flake8.main import main_with_errors
-import pytest
-
-
-@pytest.mark.flake8
-@pytest.mark.linter
-def test_flake8():
- rc, errors = main_with_errors(argv=[])
- assert rc == 0, \
- 'Found %d code style errors / warnings:\n' % len(errors) + \
- '\n'.join(errors)
diff --git a/src/rm_decision/rm_FSM/test/test_line_iterator.py b/src/rm_decision/rm_FSM/test/test_line_iterator.py
deleted file mode 100644
index 9c07e4cf9ea17ea82892e151e82d5d15dcb6bee2..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/test/test_line_iterator.py
+++ /dev/null
@@ -1,107 +0,0 @@
-# Copyright 2022 Afif Swaidan
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-import unittest
-from cmath import sqrt
-from rm_auto_FSM.line_iterator import LineIterator
-
-
-class TestLineIterator(unittest.TestCase):
-
- def test_type_error(self):
- # Test if a type error raised when passing invalid arguements types
- self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1')
-
- def test_value_error(self):
- # Test if a value error raised when passing negative or zero step_size
- self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2)
- # Test if a value error raised when passing zero length line
- self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1)
-
- def test_get_xy(self):
- # Test if the initial and final coordinates are returned correctly
- lt = LineIterator(0, 0, 5, 5, 1)
- self.assertEqual(lt.getX0(), 0)
- self.assertEqual(lt.getY0(), 0)
- self.assertEqual(lt.getX1(), 5)
- self.assertEqual(lt.getY1(), 5)
-
- def test_line_length(self):
- # Test if the line length is calculated correctly
- lt = LineIterator(0, 0, 5, 5, 1)
- self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2)))
-
- def test_straight_line(self):
- # Test if the calculations are correct for y = x
- lt = LineIterator(0, 0, 5, 5, 1)
- i = 0
- while lt.isValid():
- self.assertEqual(lt.getX(), lt.getX0() + i)
- self.assertEqual(lt.getY(), lt.getY0() + i)
- lt.advance()
- i += 1
-
- # Test if the calculations are correct for y = 2x (positive slope)
- lt = LineIterator(0, 0, 5, 10, 1)
- i = 0
- while lt.isValid():
- self.assertEqual(lt.getX(), lt.getX0() + i)
- self.assertEqual(lt.getY(), lt.getY0() + (i*2))
- lt.advance()
- i += 1
-
- # Test if the calculations are correct for y = -2x (negative slope)
- lt = LineIterator(0, 0, 5, -10, 1)
- i = 0
- while lt.isValid():
- self.assertEqual(lt.getX(), lt.getX0() + i)
- self.assertEqual(lt.getY(), lt.getY0() + (-i*2))
- lt.advance()
- i += 1
-
- def test_hor_line(self):
- # Test if the calculations are correct for y = 0x+b (horizontal line)
- lt = LineIterator(0, 10, 5, 10, 1)
- i = 0
- while lt.isValid():
- self.assertEqual(lt.getX(), lt.getX0() + i)
- self.assertEqual(lt.getY(), lt.getY0())
- lt.advance()
- i += 1
-
- def test_ver_line(self):
- # Test if the calculations are correct for x = n (vertical line)
- lt = LineIterator(5, 0, 5, 10, 1)
- i = 0
- while lt.isValid():
- self.assertEqual(lt.getX(), lt.getX0())
- self.assertEqual(lt.getY(), lt.getY0() + i)
- lt.advance()
- i += 1
-
- def test_clamp(self):
- # Test if the increments are clamped to avoid crossing the final points
- # when step_size is large with respect to line length
- lt = LineIterator(0, 0, 5, 5, 10)
- self.assertEqual(lt.getX(), 0)
- self.assertEqual(lt.getY(), 0)
- lt.advance()
- while lt.isValid():
- self.assertEqual(lt.getX(), 5)
- self.assertEqual(lt.getY(), 5)
- lt.advance()
-
-
-if __name__ == '__main__':
- unittest.main()
diff --git a/src/rm_decision/rm_FSM/test/test_pep257.py b/src/rm_decision/rm_FSM/test/test_pep257.py
deleted file mode 100644
index b234a3840f4c5bd38f043638c8622b8f240e1185..0000000000000000000000000000000000000000
--- a/src/rm_decision/rm_FSM/test/test_pep257.py
+++ /dev/null
@@ -1,23 +0,0 @@
-# Copyright 2015 Open Source Robotics Foundation, Inc.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from ament_pep257.main import main
-import pytest
-
-
-@pytest.mark.linter
-@pytest.mark.pep257
-def test_pep257():
- rc = main(argv=['.', 'test'])
- assert rc == 0, 'Found code style errors / warnings'
diff --git a/src/rm_decision_interfaces/CMakeLists.txt b/src/rm_decision_interfaces/CMakeLists.txt
deleted file mode 100644
index 110503b2beb0b8fd0f13018ea7c6c13d3970f51d..0000000000000000000000000000000000000000
--- a/src/rm_decision_interfaces/CMakeLists.txt
+++ /dev/null
@@ -1,29 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-project(rm_decision_interfaces)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-
-find_package(std_msgs REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-
-rosidl_generate_interfaces(${PROJECT_NAME}
- "msg/AllRobotHP.msg"
- "msg/RobotStatus.msg"
- "msg/FriendLocation.msg"
- "msg/Armor.msg"
- "msg/RFID.msg"
-
- DEPENDENCIES
- std_msgs
- geometry_msgs
- builtin_interfaces
-)
-
-ament_package()
\ No newline at end of file
diff --git a/src/rm_decision_interfaces/msg/AllRobotHP.msg b/src/rm_decision_interfaces/msg/AllRobotHP.msg
deleted file mode 100644
index da08b498dcbe017f477f7accb4f0b59fe942c033..0000000000000000000000000000000000000000
--- a/src/rm_decision_interfaces/msg/AllRobotHP.msg
+++ /dev/null
@@ -1,19 +0,0 @@
-# v1.6 0x0003 机器人血量数据
-
-uint16 red_1_robot_hp
-uint16 red_2_robot_hp
-uint16 red_3_robot_hp
-uint16 red_4_robot_hp
-uint16 red_5_robot_hp
-uint16 red_7_robot_hp
-uint16 red_outpost_hp
-uint16 red_base_hp
-
-uint16 blue_1_robot_hp
-uint16 blue_2_robot_hp
-uint16 blue_3_robot_hp
-uint16 blue_4_robot_hp
-uint16 blue_5_robot_hp
-uint16 blue_7_robot_hp
-uint16 blue_outpost_hp
-uint16 blue_base_hp
\ No newline at end of file
diff --git a/src/rm_decision_interfaces/msg/Armor.msg b/src/rm_decision_interfaces/msg/Armor.msg
deleted file mode 100644
index 723f523723556e1e1d5b13eebde5da8f505c99ef..0000000000000000000000000000000000000000
--- a/src/rm_decision_interfaces/msg/Armor.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-string number
-string type
-float32 distance_to_image_center
-geometry_msgs/Pose pose
\ No newline at end of file
diff --git a/src/rm_decision_interfaces/msg/FriendLocation.msg b/src/rm_decision_interfaces/msg/FriendLocation.msg
deleted file mode 100644
index 1398497ad6a3f517268d5146834aaae7a4189679..0000000000000000000000000000000000000000
--- a/src/rm_decision_interfaces/msg/FriendLocation.msg
+++ /dev/null
@@ -1,16 +0,0 @@
-# v1.6 0x020B 地面机器人位置数据
-
-float64 hero_x
-float64 hero_y
-
-float64 engineer_x
-float64 engineer_y
-
-float64 standard_3_x
-float64 standard_3_y
-
-float64 standard_4_x
-float64 standard_4_y
-
-float64 standard_5_x
-float64 standard_5_y
diff --git a/src/rm_decision_interfaces/msg/RFID.msg b/src/rm_decision_interfaces/msg/RFID.msg
deleted file mode 100644
index 6b13aed58db2d856f7d3c8acf35ac77a5ffa7ca1..0000000000000000000000000000000000000000
--- a/src/rm_decision_interfaces/msg/RFID.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-# v1.6 0x0209 机器人RFID状态（此处仅保留我方哨兵巡逻区）
-bool rfid_patrol_status
\ No newline at end of file
diff --git a/src/rm_decision_interfaces/msg/RobotStatus.msg b/src/rm_decision_interfaces/msg/RobotStatus.msg
deleted file mode 100644
index 851fc865c3b41a442dc923306a7805bb1305d0cd..0000000000000000000000000000000000000000
--- a/src/rm_decision_interfaces/msg/RobotStatus.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-# v1.6 0x0201 机器人性能体系数据
-uint8 robot_id # 机器人ID （1~7->红，101~107->蓝）
-uint16 current_hp # 血量
-
-# v1.6 0x0202 实时功率热量数据
-uint16 shooter_heat # 枪口热量
diff --git a/src/rm_decision_interfaces/package.xml b/src/rm_decision_interfaces/package.xml
deleted file mode 100644
index f6d4aed18eaaa779e1f72a100f8a72d9b508bfdb..0000000000000000000000000000000000000000
--- a/src/rm_decision_interfaces/package.xml
+++ /dev/null
@@ -1,24 +0,0 @@
-
-
-
- rm_decision_interfaces
- 0.0.0
- TODO: Package description
- Lihan Chen
- MIT
-
- ament_cmake
-
- rosidl_default_generators
- rosidl_default_runtime
- rosidl_interface_packages
-
- std_msgs
- geometry_msgs
- nav_msgs
- builtin_interfaces
-
-
- ament_cmake
-
-