├── src ├── fake_msg_publisher │ ├── fake_msg_publisher │ │ ├── __init__.py │ │ ├── __pycache__ │ │ │ ├── __init__.cpython-38.pyc │ │ │ └── fake_msg_publisher_node.cpython-38.pyc │ │ └── fake_msg_publisher_node.py │ ├── resource │ │ └── fake_msg_publisher │ ├── setup.cfg │ ├── setup.py │ ├── package.xml │ └── test │ │ ├── test_copyright.py │ │ ├── test_pep257.py │ │ └── test_flake8.py ├── global_interface │ ├── msg │ │ ├── Point2f.msg │ │ ├── CarPos.msg │ │ ├── ObjHP.msg │ │ ├── DetectionArray.msg │ │ ├── StrikeLicensing.msg │ │ ├── GameInfo.msg │ │ ├── ModeSet.msg │ │ ├── Detection.msg │ │ ├── Decision.msg │ │ ├── Serial.msg │ │ └── Autoaim.msg │ ├── package.xml │ └── CMakeLists.txt └── robot_decision │ ├── resources │ └── RMUL.png │ ├── include │ ├── robot_decision │ │ ├── public.h │ │ ├── structs.h │ │ └── RobotDecision.h │ ├── robot_decision_node.hpp │ └── Json │ │ └── json-forwards.h │ ├── JsonFile │ ├── config.json │ ├── waypoints_0.json │ ├── waypoints.json │ ├── waypoints_1.json │ ├── decisions.json │ └── decisions111.json │ ├── launch │ └── decision_node_launch.py │ ├── package.xml │ ├── sample │ ├── waypoints.json │ └── decisions.json │ ├── CMakeLists.txt │ └── src │ ├── robot_decision │ └── RobotDecision.cpp │ └── robot_decision_node.cpp ├── .gitignore ├── tools ├── resource │ └── RMUL.png └── python │ └── getlocation.py ├── image └── README │ ├── 1681056559470.png │ └── 1681128909059.png ├── README.md └── LICENSE /src/fake_msg_publisher/fake_msg_publisher/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/fake_msg_publisher/resource/fake_msg_publisher: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /src/global_interface/msg/Point2f.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y -------------------------------------------------------------------------------- /src/global_interface/msg/CarPos.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Point2f[12] pos -------------------------------------------------------------------------------- /src/global_interface/msg/ObjHP.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint16[16] hp -------------------------------------------------------------------------------- /src/global_interface/msg/DetectionArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Detection[] detections -------------------------------------------------------------------------------- /src/global_interface/msg/StrikeLicensing.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float32[6] weights -------------------------------------------------------------------------------- /src/global_interface/msg/GameInfo.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint16 timestamp 3 | uint16 game_stage -------------------------------------------------------------------------------- /src/global_interface/msg/ModeSet.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint16 mode 3 | float32 x 4 | float32 y -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build/ 3 | install/ 4 | log/ 5 | src/fake_msg_publisher/fake_msg_publisher/__pycache__/ -------------------------------------------------------------------------------- /src/global_interface/msg/Detection.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float32 conf 3 | string type 4 | geometry_msgs/Pose center -------------------------------------------------------------------------------- /tools/resource/RMUL.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tup-robomaster/JsonFileBased_RobotDecision/HEAD/tools/resource/RMUL.png -------------------------------------------------------------------------------- /image/README/1681056559470.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tup-robomaster/JsonFileBased_RobotDecision/HEAD/image/README/1681056559470.png -------------------------------------------------------------------------------- /image/README/1681128909059.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tup-robomaster/JsonFileBased_RobotDecision/HEAD/image/README/1681128909059.png -------------------------------------------------------------------------------- /src/global_interface/msg/Decision.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 decision_id 3 | float32 x 4 | float32 y 5 | float32 theta 6 | uint8 mode -------------------------------------------------------------------------------- /src/global_interface/msg/Serial.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | sensor_msgs/Imu imu 3 | uint8 mode 4 | float32 theta 5 | float32 bullet_speed -------------------------------------------------------------------------------- /src/fake_msg_publisher/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/fake_msg_publisher 3 | [install] 4 | install_scripts=$base/lib/fake_msg_publisher 5 | -------------------------------------------------------------------------------- /src/robot_decision/resources/RMUL.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tup-robomaster/JsonFileBased_RobotDecision/HEAD/src/robot_decision/resources/RMUL.png -------------------------------------------------------------------------------- /src/fake_msg_publisher/fake_msg_publisher/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tup-robomaster/JsonFileBased_RobotDecision/HEAD/src/fake_msg_publisher/fake_msg_publisher/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /src/fake_msg_publisher/fake_msg_publisher/__pycache__/fake_msg_publisher_node.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tup-robomaster/JsonFileBased_RobotDecision/HEAD/src/fake_msg_publisher/fake_msg_publisher/__pycache__/fake_msg_publisher_node.cpython-38.pyc -------------------------------------------------------------------------------- /src/global_interface/msg/Autoaim.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint8 mode 3 | string key 4 | uint16 hp 5 | float64 period 6 | float64 bullet_speed 7 | int64 timestamp 8 | Point2f[4] point2d 9 | # float64 w 10 | bool clockwise 11 | bool is_spinning 12 | bool is_controlled 13 | bool is_target_lost 14 | bool target_switched 15 | bool spinning_switched 16 | bool is_still_spinning 17 | geometry_msgs/Quaternion quat_imu 18 | geometry_msgs/Point aiming_point_world 19 | geometry_msgs/Point aiming_point_cam -------------------------------------------------------------------------------- /src/robot_decision/include/robot_decision/public.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "opencv2/opencv.hpp" 23 | #include "../../include/Json/json.h" 24 | 25 | #define OBJHP_NUM (int)8 26 | #define CARPOS_NUM (int)6 -------------------------------------------------------------------------------- /src/robot_decision/JsonFile/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "config" : { 3 | "Debug" : true, 4 | "WayPointsPATH" : "waypoints_0.json", 5 | "DecisionsPATH" : "decisions.json", 6 | "MAP_PATH" : "RMUL.png", 7 | "REAL_WIDTH" : 25.0, 8 | "REAL_HEIGHT": 15.0, 9 | "GOAL_TIME_THR_SEC" : 2, 10 | "TIME_THR" : 3, 11 | "CAR_SEEK_FOV" : 70.0, 12 | "INIT_DISTANCE_THR" : 1.0, 13 | "INIT_SEEK_THR" : 10.0, 14 | "INIT_ISBLUE" : true, 15 | "INIT_IFSHOWUI" : true, 16 | "INIT_IFUSEMANUAL" : true, 17 | "INIT_SELFINDEX" : 5, 18 | "INIT_FRIENDOUTPOSTINDEX" : 6, 19 | "INIT_FRIENDBASEINDEX" : 7, 20 | "STEP_DISTANCE" : 0.1, 21 | "GAME_TIME" : 420 22 | } 23 | } -------------------------------------------------------------------------------- /src/fake_msg_publisher/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'fake_msg_publisher' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='ninefish', 17 | maintainer_email='iratecat1@outlook.com', 18 | description='TODO: Package description', 19 | license='TODO: License declaration', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | 'fake_msg_publisher_node = fake_msg_publisher.fake_msg_publisher_node:main' 24 | ], 25 | }, 26 | ) 27 | -------------------------------------------------------------------------------- /src/fake_msg_publisher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | fake_msg_publisher 5 | 0.0.0 6 | make fake msgs 7 | ninefish 8 | TODO: License declaration 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | rclpy 16 | 17 | std_msgs 18 | geometry_msgs 19 | global_interface 20 | 21 | 22 | ament_python 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/fake_msg_publisher/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /src/fake_msg_publisher/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /src/robot_decision/launch/decision_node_launch.py: -------------------------------------------------------------------------------- 1 | from argparse import Namespace 2 | from launch import LaunchDescription 3 | from launch_ros.actions import Node 4 | from launch.actions import DeclareLaunchArgument 5 | from launch.substitutions import LaunchConfiguration 6 | 7 | 8 | from ament_index_python.packages import get_package_share_directory 9 | 10 | import os 11 | 12 | 13 | def generate_launch_description(): 14 | ld = LaunchDescription() 15 | 16 | robot_decision_node = Node( 17 | name="robot_decision", 18 | package="robot_decision", 19 | executable="robot_decision_node", 20 | parameters=[{'distance_thr', "1.0", 21 | 'seek_thr', "5.0", 22 | 'IsBlue', "False", 23 | 'IfShowUI', "False"}], 24 | respawn=True, 25 | output="screen" 26 | ) 27 | 28 | ld.add_action(robot_decision_node) 29 | 30 | return ld 31 | -------------------------------------------------------------------------------- /src/robot_decision/JsonFile/waypoints_0.json: -------------------------------------------------------------------------------- 1 | { 2 | "data": [ 3 | { 4 | "id": 0, 5 | "name": "FrontBase", 6 | "type": 0, 7 | "x": 5.5, 8 | "y": 7.4, 9 | "angle": 0.0, 10 | "connect": [ 11 | 1 12 | ], 13 | "enemyWeights": [ 14 | -1, 15 | -1, 16 | -1, 17 | -1, 18 | -1 19 | ] 20 | }, 21 | { 22 | "id": 1, 23 | "name": "FirePoint", 24 | "type": 0, 25 | "x": 4, 26 | "y": 3, 27 | "angle": 0.0, 28 | "connect": [ 29 | 0 30 | ], 31 | "enemyWeights": [ 32 | -1, 33 | -1, 34 | -1, 35 | -1, 36 | -1 37 | ] 38 | } 39 | ] 40 | } 41 | 42 | -------------------------------------------------------------------------------- /tools/python/getlocation.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | 4 | real_width = 12. 5 | real_height = 8. 6 | 7 | img = cv2.imread( 8 | "/home/ninefish/nine-fish/JsonFileBased_RobotDecision/tools/resource/RMUL.png") 9 | 10 | def on_EVENT_LBUTTONDOWN(event, x, y, flags, param): 11 | if event == cv2.EVENT_LBUTTONDOWN: 12 | xy = "%f,%f" % (float( 13 | x)/float(img.shape[1]) * real_width, float(y)/float(img.shape[0]) * real_height) 14 | print(xy) 15 | cv2.circle(img, (x, y), 1, (255, 0, 0), thickness=-1) 16 | cv2.putText(img, xy, (x, y), cv2.FONT_HERSHEY_PLAIN, 17 | 1.0, (0, 0, 255), thickness=1) 18 | cv2.imshow("image", img) 19 | 20 | 21 | cv2.namedWindow("image", cv2.WINDOW_GUI_NORMAL) 22 | cv2.setMouseCallback("image", on_EVENT_LBUTTONDOWN) 23 | cv2.imshow("image", img) 24 | while (True): 25 | try: 26 | cv2.waitKey(100) 27 | except Exception: 28 | cv2.destroyWindow("image") 29 | break 30 | -------------------------------------------------------------------------------- /src/fake_msg_publisher/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /src/global_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | global_interface 5 | 0.0.0 6 | TODO: Package description 7 | ninefish 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | std_msgs 14 | geometry_msgs 15 | sensor_msgs 16 | 17 | ament_lint_auto 18 | ament_lint_common 19 | 20 | rosidl_default_generators 21 | rosidl_default_runtime 22 | rosidl_interface_packages 23 | builtin_interfaces 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/robot_decision/JsonFile/waypoints.json: -------------------------------------------------------------------------------- 1 | { 2 | "data": [ 3 | { 4 | "id": 0, 5 | "name": "zero", 6 | "type": 0, 7 | "x": 3.13, 8 | "y": 3.56, 9 | "angle": 0.0, 10 | "connect": [ 11 | 1 12 | ], 13 | "enemyWeights": [ 14 | -1, 15 | -1, 16 | -1, 17 | -1, 18 | -1 19 | ] 20 | }, 21 | { 22 | "id": 1, 23 | "name": "one", 24 | "type": 0, 25 | "x": 4.55, 26 | "y": 6.31, 27 | "angle": 0.0, 28 | "connect": [ 29 | 0, 30 | 2 31 | ], 32 | "enemyWeights": [ 33 | -1, 34 | -1, 35 | -1, 36 | -1, 37 | -1 38 | ] 39 | }, 40 | { 41 | "id": 2, 42 | "name": "two", 43 | "type": 0, 44 | "x": 5.78, 45 | "y": 4.29, 46 | "angle": 0.0, 47 | "connect": [ 48 | 1 49 | ], 50 | "enemyWeights": [ 51 | -1, 52 | -1, 53 | -1, 54 | -1, 55 | -1 56 | ] 57 | } 58 | ] 59 | } -------------------------------------------------------------------------------- /src/robot_decision/JsonFile/waypoints_1.json: -------------------------------------------------------------------------------- 1 | { 2 | "data": [ 3 | { 4 | "id": 0, 5 | "name": "supply", 6 | "type": 0, 7 | "x": 0.5, 8 | "y": 0.5, 9 | "angle": 0.0, 10 | "connect": [ 11 | 1 12 | ], 13 | "enemyWeights": [ 14 | -1, 15 | -1, 16 | -1, 17 | -1, 18 | -1 19 | ] 20 | }, 21 | { 22 | "id": 1, 23 | "name": "INIT", 24 | "type": 0, 25 | "x": 3.18, 26 | "y": 4.91, 27 | "angle": 0.0, 28 | "connect": [ 29 | 0, 30 | 2 31 | ], 32 | "enemyWeights": [ 33 | -1, 34 | -1, 35 | -1, 36 | -1, 37 | -1 38 | ] 39 | }, 40 | { 41 | "id": 2, 42 | "name": "FirePoint", 43 | "type": 0, 44 | "x": 3.82, 45 | "y": 7.0, 46 | "angle": 0.0, 47 | "connect": [ 48 | 1 49 | ], 50 | "enemyWeights": [ 51 | -1, 52 | -1, 53 | -1, 54 | -1, 55 | -1 56 | ] 57 | } 58 | ] 59 | } -------------------------------------------------------------------------------- /src/robot_decision/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_decision 5 | 0.0.0 6 | THE FUCTION FOR ROBOT DECISION 7 | ninefish 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | geometry_msgs 13 | nav2_util 14 | nav2_lifecycle_manager 15 | nav2_msgs 16 | nav_msgs 17 | std_msgs 18 | rclcpp 19 | rclcpp_components 20 | global_interface 21 | rclcpp_lifecycle 22 | message_filters 23 | robot_state_publisher 24 | tf2 25 | tf2_ros 26 | tf2_geometry_msgs 27 | 28 | rosidl_default_generators 29 | rosidl_default_runtime 30 | rosidl_interface_packages 31 | ament_lint_auto 32 | ament_lint_common 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /src/robot_decision/JsonFile/decisions.json: -------------------------------------------------------------------------------- 1 | { 2 | "data" : [ 3 | { 4 | "id" : 0, 5 | "name" : "move_FrontBase", 6 | "wayPointID" : [-1], 7 | "weight" : 400, 8 | "start_time" : -1, 9 | "end_time" : 420, 10 | "robot_mode" : -1, 11 | "minHP" : 0, 12 | "maxHP" : -1, 13 | "decide_mode" : 6, 14 | "decide_wayPoint" : 0, 15 | "out_post_HP_min": -1, 16 | "out_post_HP_max": -1, 17 | "base_HP_min": -1, 18 | "if_succession" : false, 19 | "if_reverse" : false, 20 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]], 21 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]] 22 | }, 23 | { 24 | "id" : 1, 25 | "name" : "stay_FrontBase", 26 | "wayPointID" : [0], 27 | "weight" : 500, 28 | "start_time" : -1, 29 | "end_time" : 420, 30 | "robot_mode" : -1, 31 | "minHP" : 0, 32 | "maxHP" : -1, 33 | "decide_mode" : 7, 34 | "decide_wayPoint" : 0, 35 | "out_post_HP_min": -1, 36 | "out_post_HP_max": -1, 37 | "base_HP_min": -1, 38 | "if_succession" : false, 39 | "if_reverse" : false, 40 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]], 41 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]] 42 | } 43 | ] 44 | } -------------------------------------------------------------------------------- /src/global_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(global_interface) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic -O3) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rosidl_default_generators REQUIRED) 11 | find_package(builtin_interfaces REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(std_msgs REQUIRED) 14 | find_package(geometry_msgs REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | # uncomment the following section in order to fill in 17 | # further dependencies manually. 18 | # find_package( REQUIRED) 19 | 20 | if(BUILD_TESTING) 21 | find_package(ament_lint_auto REQUIRED) 22 | # the following line skips the linter which checks for copyrights 23 | # uncomment the line when a copyright and license is not present in all source files 24 | #set(ament_cmake_copyright_FOUND TRUE) 25 | # the following line skips cpplint (only works in a git repo) 26 | # uncomment the line when this package is not in a git repo 27 | #set(ament_cmake_cpplint_FOUND TRUE) 28 | ament_lint_auto_find_test_dependencies() 29 | endif() 30 | 31 | rosidl_generate_interfaces(${PROJECT_NAME} 32 | "msg/ObjHP.msg" 33 | "msg/CarPos.msg" 34 | "msg/GameInfo.msg" 35 | "msg/Point2f.msg" 36 | "msg/Serial.msg" 37 | "msg/Detection.msg" 38 | "msg/DetectionArray.msg" 39 | "msg/Decision.msg" 40 | "msg/Autoaim.msg" 41 | "msg/ModeSet.msg" 42 | "msg/StrikeLicensing.msg" 43 | DEPENDENCIES 44 | builtin_interfaces 45 | std_msgs 46 | geometry_msgs 47 | sensor_msgs 48 | ) 49 | 50 | ament_export_dependencies(rosidl_default_runtime) 51 | ament_package() 52 | -------------------------------------------------------------------------------- /src/robot_decision/include/robot_decision/structs.h: -------------------------------------------------------------------------------- 1 | #ifndef _RD_STRUCTS_H 2 | #define _RD_STRUCTS_H 3 | 4 | #include "./public.h" 5 | 6 | namespace rdsys 7 | { 8 | /** 9 | * @brief 机器人位置信息 10 | */ 11 | typedef struct RobotPosition 12 | { 13 | int robot_id = -1; 14 | float x; 15 | float y; 16 | RobotPosition(){}; 17 | RobotPosition(int id, float x, float y) 18 | { 19 | this->robot_id = id; 20 | this->x = x; 21 | this->y = y; 22 | }; 23 | } RobotPosition; 24 | 25 | /** 26 | * @brief 路径点信息 27 | */ 28 | typedef struct WayPoint 29 | { 30 | int id; 31 | int type; 32 | float x; 33 | float y; 34 | double theta; 35 | std::map enemyWeights; 36 | std::vector connection; 37 | } WayPoint; 38 | 39 | /** 40 | * @brief 决策信息 41 | */ 42 | typedef struct Decision 43 | { 44 | bool if_auto = true; 45 | int id; 46 | const char *name; 47 | std::vector wayPointID; 48 | int weight; 49 | // condition 50 | int robot_mode; 51 | int start_time; 52 | int end_time; 53 | int _minHP; 54 | int _maxHP; 55 | int out_post_HP_min; 56 | int out_post_HP_max; 57 | int base_HP_min; 58 | std::vector> enemy_position; 59 | std::vector> friend_position; 60 | // decision 61 | int decide_mode; 62 | int decide_wayPoint; 63 | bool if_succession; 64 | bool if_reverse; 65 | } Decision; 66 | 67 | enum Mode 68 | { 69 | AUTOAIM = 8, 70 | MANUAL_ATTACK = 8, 71 | MANUAL_BACKDEFENSE = 11 72 | }; 73 | 74 | enum GameStage 75 | { 76 | COMPETITON_NOT_STARTED = 0, 77 | PREPARATION_STAGE = 1, 78 | SELF_INSPECTION_STAGE = 2, 79 | FIVE_S_COUNTDOWN = 3, 80 | IN_BATTLE = 4, 81 | END = 5 82 | }; 83 | } 84 | 85 | #endif -------------------------------------------------------------------------------- /src/robot_decision/sample/waypoints.json: -------------------------------------------------------------------------------- 1 | { 2 | "data": [ 3 | { 4 | "id": 0, 5 | "name": "test1", 6 | "type": 0, 7 | "x": 1.0, 8 | "y": 1.0, 9 | "angle": 0.0, 10 | "connect": [ 11 | 1 12 | ], 13 | "enemyWeights": [ 14 | -1, 15 | -1, 16 | -1, 17 | -1, 18 | -1 19 | ] 20 | }, 21 | { 22 | "id": 1, 23 | "name": "test2", 24 | "type": 0, 25 | "x": 1.0, 26 | "y": 2.0, 27 | "angle": 0.0, 28 | "connect": [ 29 | 0, 30 | 2, 31 | 4 32 | ], 33 | "enemyWeights": [ 34 | -1, 35 | -1, 36 | -1, 37 | -1, 38 | -1 39 | ] 40 | }, 41 | { 42 | "id": 2, 43 | "name": "test3", 44 | "type": 0, 45 | "x": 2.0, 46 | "y": 2.0, 47 | "angle": 0.0, 48 | "connect": [ 49 | 1, 50 | 3 51 | ], 52 | "enemyWeights": [ 53 | -1, 54 | -1, 55 | -1, 56 | -1, 57 | -1 58 | ] 59 | }, 60 | { 61 | "id": 3, 62 | "name": "test4", 63 | "type": 0, 64 | "x": 2.0, 65 | "y": 1.0, 66 | "angle": 0.0, 67 | "connect": [ 68 | 2 69 | ], 70 | "enemyWeights": [ 71 | -1, 72 | -1, 73 | -1, 74 | -1, 75 | -1 76 | ] 77 | }, 78 | { 79 | "id": 4, 80 | "name": "test4", 81 | "type": 0, 82 | "x": 4.0, 83 | "y": 7.0, 84 | "angle": 0.0, 85 | "connect": [ 86 | 1 87 | ], 88 | "enemyWeights": [ 89 | -1, 90 | -1, 91 | -1, 92 | -1, 93 | -1 94 | ] 95 | } 96 | ] 97 | } -------------------------------------------------------------------------------- /src/robot_decision/JsonFile/decisions111.json: -------------------------------------------------------------------------------- 1 | { 2 | "data" : [ 3 | { 4 | "id" : 0, 5 | "name" : "move_FrontBase", 6 | "wayPointID" : [-1], 7 | "weight" : 400, 8 | "start_time" : -1, 9 | "end_time" : 420, 10 | "robot_mode" : -1, 11 | "minHP" : 0, 12 | "maxHP" : -1, 13 | "decide_mode" : 6, 14 | "decide_wayPoint" : 0, 15 | "out_post_HP_min": -1, 16 | "out_post_HP_max": 300, 17 | "base_HP_min": -1, 18 | "if_succession" : false, 19 | "if_reverse" : false, 20 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]], 21 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]] 22 | }, 23 | { 24 | "id" : 1, 25 | "name" : "stay_FrontBase", 26 | "wayPointID" : [0], 27 | "weight" : 500, 28 | "start_time" : -1, 29 | "end_time" : 420, 30 | "robot_mode" : -1, 31 | "minHP" : 0, 32 | "maxHP" : -1, 33 | "decide_mode" : 7, 34 | "decide_wayPoint" : 0, 35 | "out_post_HP_min": -1, 36 | "out_post_HP_max": 300, 37 | "base_HP_min": -1, 38 | "if_succession" : false, 39 | "if_reverse" : false, 40 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]], 41 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]] 42 | }, 43 | { 44 | "id" : 2, 45 | "name" : "move_FirePoint", 46 | "wayPointID" : [-1], 47 | "weight" : 400, 48 | "start_time" : -1, 49 | "end_time" : 420, 50 | "robot_mode" : -1, 51 | "minHP" : 0, 52 | "maxHP" : -1, 53 | "decide_mode" : 6, 54 | "decide_wayPoint" : 1, 55 | "out_post_HP_min": 300, 56 | "out_post_HP_max": -1, 57 | "base_HP_min": -1, 58 | "if_succession" : false, 59 | "if_reverse" : false, 60 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]], 61 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]] 62 | }, 63 | { 64 | "id" : 3, 65 | "name" : "stay_FirePoint", 66 | "wayPointID" : [1], 67 | "weight" : 500, 68 | "start_time" : -1, 69 | "end_time" : 420, 70 | "robot_mode" : -1, 71 | "minHP" : 0, 72 | "maxHP" : -1, 73 | "decide_mode" : 7, 74 | "decide_wayPoint" : 1, 75 | "out_post_HP_min": 300, 76 | "out_post_HP_max": -1, 77 | "base_HP_min": -1, 78 | "if_succession" : false, 79 | "if_reverse" : false, 80 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]], 81 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]] 82 | } 83 | ] 84 | } -------------------------------------------------------------------------------- /src/robot_decision/sample/decisions.json: -------------------------------------------------------------------------------- 1 | { 2 | "data" : [ 3 | { 4 | "id" : 0, 5 | "name" : "test1", 6 | "wayPointID" : [-1], 7 | "weight" : 500, 8 | "start_time" : -1, 9 | "end_time" : 420, 10 | "robot_mode" : 0, 11 | "minHP" : -1, 12 | "maxHP" : -1, 13 | "decide_mode" : 0, 14 | "decide_wayPoint" : 0, 15 | "out_post_HP_min": 0, 16 | "if_succession" : false, 17 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1]], 18 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1]] 19 | }, 20 | { 21 | "id" : 1, 22 | "name" : "test2", 23 | "wayPointID" : [0], 24 | "weight" : 1000, 25 | "start_time" : -1, 26 | "end_time" : 420, 27 | "robot_mode" : 0, 28 | "minHP" : -1, 29 | "maxHP" : -1, 30 | "decide_mode" : 0, 31 | "decide_wayPoint" : 1, 32 | "out_post_HP_min": 0, 33 | "if_succession" : false, 34 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1]], 35 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1]] 36 | }, 37 | { 38 | "id" : 2, 39 | "name" : "test3", 40 | "wayPointID" : [1], 41 | "weight" : 1000, 42 | "start_time" : -1, 43 | "end_time" : 420, 44 | "robot_mode" : 0, 45 | "minHP" : -1, 46 | "maxHP" : -1, 47 | "decide_mode" : 0, 48 | "decide_wayPoint" : 2, 49 | "out_post_HP_min": 0, 50 | "if_succession" : false, 51 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1]], 52 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1]] 53 | }, 54 | { 55 | "id" : 3, 56 | "name" : "test4", 57 | "wayPointID" : [2], 58 | "weight" : 1000, 59 | "start_time" : -1, 60 | "end_time" : 420, 61 | "robot_mode" : 0, 62 | "minHP" : -1, 63 | "maxHP" : -1, 64 | "decide_mode" : 0, 65 | "decide_wayPoint" : 3, 66 | "out_post_HP_min": 0, 67 | "if_succession" : false, 68 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1]], 69 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1]] 70 | }, 71 | { 72 | "id" : 4, 73 | "name" : "test4", 74 | "wayPointID" : [3], 75 | "weight" : 1200, 76 | "start_time" : -1, 77 | "end_time" : 420, 78 | "robot_mode" : 0, 79 | "minHP" : -1, 80 | "maxHP" : -1, 81 | "decide_mode" : 0, 82 | "decide_wayPoint" : 0, 83 | "out_post_HP_min": 0, 84 | "if_succession" : true, 85 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1]], 86 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1]] 87 | } 88 | ] 89 | } -------------------------------------------------------------------------------- /src/robot_decision/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_decision) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic -O3) 10 | endif() 11 | 12 | # find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(nav2_util REQUIRED) 16 | find_package(nav2_lifecycle_manager REQUIRED) 17 | find_package(nav2_msgs REQUIRED) 18 | find_package(nav2_behavior_tree REQUIRED) 19 | find_package(nav_msgs REQUIRED) 20 | find_package(rclcpp_components REQUIRED) 21 | find_package(geometry_msgs REQUIRED) 22 | find_package(rclcpp_lifecycle REQUIRED) 23 | find_package(global_interface REQUIRED) 24 | find_package(message_filters REQUIRED) 25 | find_package(std_msgs REQUIRED) 26 | find_package(robot_state_publisher REQUIRED) 27 | find_package(rosidl_default_generators REQUIRED) 28 | find_package(tf2 REQUIRED) 29 | find_package(tf2_ros REQUIRED) 30 | find_package(OpenCV REQUIRED) 31 | find_package(tf2_geometry_msgs REQUIRED) 32 | 33 | 34 | include_directories( 35 | include 36 | ) 37 | 38 | set(node_plugins "") 39 | 40 | add_library(${PROJECT_NAME} SHARED 41 | src/Json/jsoncpp.cpp 42 | src/${PROJECT_NAME}/RobotDecision.cpp 43 | src/${PROJECT_NAME}_node.cpp 44 | ) 45 | 46 | set(dependencies 47 | rclcpp 48 | rclcpp_components 49 | nav2_util 50 | nav2_lifecycle_manager 51 | nav2_msgs 52 | nav_msgs 53 | std_msgs 54 | geometry_msgs 55 | global_interface 56 | rclcpp_lifecycle 57 | message_filters 58 | robot_state_publisher 59 | nav2_behavior_tree 60 | rosidl_default_generators 61 | tf2_ros 62 | tf2 63 | tf2_geometry_msgs 64 | OpenCV 65 | ) 66 | 67 | ament_target_dependencies( 68 | ${PROJECT_NAME} 69 | ${dependencies} 70 | ) 71 | 72 | add_executable(${PROJECT_NAME}_node 73 | src/${PROJECT_NAME}_node.cpp 74 | src/robot_decision/RobotDecision.cpp 75 | src/Json/jsoncpp.cpp 76 | ) 77 | 78 | target_link_libraries(${PROJECT_NAME}_node 79 | ${PROJECT_NAME} 80 | ) 81 | 82 | target_compile_definitions(${PROJECT_NAME} 83 | PRIVATE "COMPOSITION_BUILDING_DLL" 84 | ) 85 | 86 | rclcpp_components_register_nodes(${PROJECT_NAME} 87 | PLUGIN "robotdecisionsystem::decision_process" 88 | EXECUTABLE ${PROJECT_NAME}_node 89 | ) 90 | 91 | install(TARGETS ${PROJECT_NAME} 92 | EXPORT ${PROJECT_NAME} 93 | LIBRARY DESTINATION lib 94 | ARCHIVE DESTINATION lib 95 | RUNTIME DESTINATION bin 96 | INCLUDES DESTINATION include 97 | ) 98 | 99 | install(TARGETS 100 | ${PROJECT_NAME}_node 101 | DESTINATION lib/${PROJECT_NAME} 102 | ) 103 | 104 | install( 105 | DIRECTORY include/ 106 | DESTINATION include 107 | ) 108 | 109 | install( 110 | DIRECTORY launch JsonFile resources 111 | DESTINATION share/${PROJECT_NAME} 112 | ) 113 | 114 | if(BUILD_TESTING) 115 | find_package(ament_lint_auto REQUIRED) 116 | # the following line skips the linter which checks for copyrights 117 | # uncomment the line when a copyright and license is not present in all source files 118 | #set(ament_cmake_copyright_FOUND TRUE) 119 | # the following line skips cpplint (only works in a git repo) 120 | # uncomment the line when this package is not in a git repo 121 | #set(ament_cmake_cpplint_FOUND TRUE) 122 | ament_lint_auto_find_test_dependencies() 123 | endif() 124 | 125 | ament_package() 126 | -------------------------------------------------------------------------------- /src/fake_msg_publisher/fake_msg_publisher/fake_msg_publisher_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from ast import arg 3 | import rclpy 4 | from rclpy.node import Node 5 | from global_interface.msg import ObjHP as ObjHPMsg 6 | from global_interface.msg import CarPos as CarPosMsg 7 | from global_interface.msg import GameInfo as GameInfoMsg 8 | from global_interface.msg import Serial as SerialMsg 9 | from global_interface.msg import Point2f 10 | from global_interface.msg import ModeSet 11 | import random 12 | 13 | 14 | class Lier(Node): 15 | 16 | def __init__(self): 17 | super().__init__('fake_msg_publisher') 18 | self.publisher_CarHP = self.create_publisher( 19 | ObjHPMsg, '/obj_hp', 10) 20 | self.publisher_CarPosMsg = self.create_publisher( 21 | CarPosMsg, '/car_pos', 10) 22 | self.publisher_GameInfoMsg = self.create_publisher( 23 | GameInfoMsg, '/game_info', 10) 24 | self.publisher_SerialMsg = self.create_publisher( 25 | SerialMsg, '/serial_msg', 10) 26 | self.publisher_ModeSet = self.create_publisher( 27 | ModeSet, '/mode_set', 10) 28 | timer_period = 0.1 # seconds 29 | self.timer = self.create_timer(timer_period, self.timer_callback) 30 | self.carPos_msg = CarPosMsg() 31 | self.w = 12. 32 | self.h = 8. 33 | for i in range(-1, 11): 34 | temp_pos2 = Point2f() 35 | temp_pos2.x = 6. 36 | temp_pos2.y = 4. 37 | self.carPos_msg.pos[i] = temp_pos2 38 | 39 | def timer_callback(self): 40 | self.make_fake() 41 | temp_pos = Point2f() 42 | # temp_pos.x = 4.6 43 | # temp_pos.y = 6.4 44 | temp_pos.x = 3.82 45 | temp_pos.y = 7.0 46 | modeSet_msg = ModeSet() 47 | modeSet_msg.mode = 1 48 | modeSet_msg.x = 3. 49 | modeSet_msg.y = 4. 50 | self.carPos_msg.pos[5] = temp_pos 51 | self.publisher_CarHP.publish(self.objHP_msg) 52 | self.publisher_CarPosMsg.publish(self.carPos_msg) 53 | self.publisher_GameInfoMsg.publish(self.gameInfo_msg) 54 | self.publisher_SerialMsg.publish(self.serial_msg) 55 | self.publisher_ModeSet.publish(modeSet_msg) 56 | self.get_logger().info("Publish Fake Msgs") 57 | 58 | def make_fake(self): 59 | self.objHP_msg = ObjHPMsg() 60 | self.objHP_msg.header.stamp = self.get_clock().now().to_msg() 61 | self.objHP_msg.hp[5] = 500 62 | self.objHP_msg.hp[6] = 600 63 | self.objHP_msg.hp[7] = 600 64 | for i in range(-1, 11): 65 | temp_pos2 = Point2f() 66 | aim_x = random.uniform(self.carPos_msg.pos[i].x - 0.1, self.carPos_msg.pos[i].x + 0.1) 67 | aim_y = random.uniform(self.carPos_msg.pos[i].y - 0.1, self.carPos_msg.pos[i].y + 0.1) 68 | # aim_x = 0. 69 | # aim_y = 0. 70 | if aim_x < 0.: 71 | aim_x = 0. 72 | if aim_x > self.w: 73 | aim_x = self.w 74 | if aim_y < 0.: 75 | aim_y = 0. 76 | if aim_y > self.h: 77 | aim_y = self.h 78 | temp_pos2.x = aim_x 79 | temp_pos2.y = aim_y 80 | self.carPos_msg.pos[i] = temp_pos2 81 | self.carPos_msg.header.stamp = self.get_clock().now().to_msg() 82 | self.gameInfo_msg = GameInfoMsg() 83 | self.gameInfo_msg.header.stamp = self.get_clock().now().to_msg() 84 | self.gameInfo_msg.game_stage = 4 85 | self.serial_msg = SerialMsg() 86 | self.serial_msg.header.stamp = self.get_clock().now().to_msg() 87 | 88 | 89 | def main(args=None): 90 | rclpy.init(args=args) 91 | minimal_publisher = Lier() 92 | rclpy.spin(minimal_publisher) 93 | minimal_publisher.destroy_node() 94 | rclpy.shutdown() 95 | 96 | 97 | if __name__ == '__main__': 98 | main() 99 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # JsonFileBased_RobotDecision 2 | 3 | 基于Json文件解析的行为树，为机器人宏观决策设计 4 | 5 | 基于Json文件的机器人决策系统（JsonFileBased RobotDecision），为RoboMaster参赛机器人宏观决策设计。通过修改指定Json文件，快速修改机器人运行逻辑并约束移动区域，提供高自由度、高定制化的机器人决策逻辑定制功能。带有可视化GUI，可直观查看机器人决策状态。另有虚假消息发布者，以模拟虚拟环境来提供离线调试功能。程序运行在ROS2(Robot Operating System 2) Galactic框架下。 6 | 7 | # Version：1.2a 8 | 9 | # **节点（Node） 介绍：** 10 | 11 | ## robot_decision: 12 | 13 | ros2 run robot_decision robot_decision_node 14 | 15 | ### 订阅： 16 | 17 | | 话题 | 消息 | 描述 | 18 | | --------------------------------------- | -------------------------------------------------------------- | --------------------- | 19 | | /obj_hp | global_interface::msg::ObjHP | 场上兵种/设施血量 | 20 | | /car_pos | global_interface::msg::CarPos | 场上各兵种位置 | 21 | | /game_info | global_interface::msg::GameInfo | 比赛相关信息 | 22 | | /serial_msg | global_interface::msg::Serial | 车辆模式、弹速等信息 | 23 | | /joint_states | sensor_msgs::msg::JointState | 云台yaw、pitch轴角度 | 24 | | perception_detector/perception_array | global_interface::msg::DetectionArray | 感知识别信息 | 25 | | navigate_through_poses/_action/feedback | nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage | Nav2 Action反馈信息 | 26 | | navigate_through_poses/_action/status | action_msgs::msg::GoalStatusArray | Nav2 Action Goal 状态 | 27 | | /armor_detector/armor_msg | global_interface::msg::Autoaim | 自瞄检测信息 | 28 | 29 | ### 发布： 30 | 31 | | 话题 | 消息 | 描述 | 32 | | ----------------------- | ------------------------------- | ---------------- | 33 | | robot_decision/aim_yaw | std_msgs::msg::Float32 | 车辆目标朝向偏角 | 34 | | robot_decision/decision | global_interface::msg::Decision | 车辆当前决策信息 | 35 | 36 | ### 动作: 37 | 38 | | Name | Action | 描述 | 39 | | ---------------------- | --------------------------------------- | -------------------- | 40 | | navigate_through_poses | nav2_msgs::action::NavigateThroughPoses | 发送Nav2路径导航动作 | 41 | 42 | ### 参数： 43 | 44 | | Name | 类型 | 描述 | 45 | | ------------------ | ----- | -------------------- | 46 | | distance_thr | float | 路径点计算距离阈值 | 47 | | seek_thr | float | 感知索敌距离阈值 | 48 | | IsRed | bool | 红蓝方标志位 | 49 | | IfShowUI | bool | 是否显示可视化GUI | 50 | | SelfIndex | int | 车辆自身ID（索引） | 51 | | friendOutPostIndex | int | 我方前哨站ID（索引） | 52 | 53 | ## fake_msg_publisher: 54 | 55 | ros2 run fake_msg_publisher fake_msg_publisher_node 56 | 57 | ### 发布： 58 | 59 | | 话题 | 消息 | 描述 | 60 | | ----------- | ------------------------------- | -------------------- | 61 | | /obj_hp | global_interface::msg::ObjHP | 场上兵种/设施血量 | 62 | | /car_pos | global_interface::msg::CarPos | 场上各兵种位置 | 63 | | /game_info | global_interface::msg::GameInfo | 比赛相关信息 | 64 | | /serial_msg | global_interface::msg::Serial | 车辆模式、弹速等信息 | 65 | 66 | # 节点关系图 67 | 68 | ![1681056559470](image/README/1681056559470.png) 69 | 70 | # 项目结构 （OUT-OF-DATA） 71 | 72 | ├── images //图片目录 73 | 74 | ├── LICENSE //开源协议 75 | 76 | ├── README.md //项目自述文件 77 | 78 | └── src //项目源码目录 79 | 80 | ├── fake_msg_publisher //假消息发布者功能包目录 81 | 82 | │ ├── fake_msg_publisher 83 | 84 | │ │ ├── fake_msg_publisher_node.py //节点源码 85 | 86 | │ │ ├── init.py 87 | 88 | │ │ └── pycache //python缓存 89 | 90 | │ │ ├── fake_msg_publisher_node.cpython-38.pyc 91 | 92 | │ │ └── init.cpython-38.pyc 93 | 94 | │ ├── package.xml //功能包依赖xml文件 95 | 96 | │ ├── resource //功能包资源文件 97 | 98 | │ │ └── fake_msg_publisher 99 | │ ├── setup.cfg //ROS2 setup.cfg 100 | 101 | │ ├── setup.py //ROS2 setup.py 102 | 103 | │ └── test 104 | │ ├── test_copyright.py 105 | 106 | │ ├── test_flake8.py 107 | 108 | │ └── test_pep257.py 109 | 110 | ├── global_interface //定义全局通用消息文件 111 | 112 | │ ├── CMakeLists.txt //功能包CMakeLists.txt 113 | 114 | │ ├── msg //消息目录 115 | 116 | │ │ ├── CarPos.msg //车辆位置消息 117 | 118 | │ │ ├── Decision.msg //决策消息 119 | 120 | │ │ ├── DetectionArray.msg //感知识别消息 121 | 122 | │ │ ├── Detection.msg //自瞄识别消息 123 | 124 | │ │ ├── GameInfo.msg //比赛信息消息 125 | 126 | │ │ ├── ObjHP.msg //血量消息 127 | 128 | │ │ ├── Point2f.msg //2D点消息 129 | 130 | │ │ └── Serial.msg //下位机通讯消息 131 | 132 | │ └── package.xml //功能包依赖xml文件 133 | 134 | └── robot_decision //决策功能包 135 | 136 | ├── CMakeLists.txt //功能包CMakeLists.txt 137 | 138 | ├── include //头文件目录 139 | 140 | │ ├── Json //Json处理库头文件 141 | 142 | │ │ ├── json-forwards.h 143 | 144 | │ │ └── json.h 145 | 146 | │ ├── robot_decision //决策系统头文件目录 147 | 148 | │ │ ├── public.h //公共头文件 149 | 150 | │ │ ├── RobotDecision.h //决策系统头文件 151 | 152 | │ │ └── structs.h //结构体定义 153 | 154 | │ └── robot_decision_node.hpp //决策节点头文件 155 | 156 | ├── launch //launch目录 157 | 158 | │ └── decision_node_launch.py //带参数节点启动launch文件 159 | 160 | ├── package.xml //功能包依赖xml文件 161 | 162 | ├── resources //资源目录 163 | 164 | │ └── RMUL.png //RMUL赛场障碍图 165 | 166 | ├── sample //Json样例目录 167 | 168 | │ ├── decisions.json //决策Json样例 169 | 170 | │ └── waypoints.json //路径点Json样例 171 | 172 | └── src //源码目录 173 | 174 | ├── Json //Json处理库源码 175 | 176 | │ └── jsoncpp.cpp 177 | 178 | 179 | ├── robot_decision //决策系统源码 180 | 181 | │ └── RobotDecision.cpp 182 | 183 | └── robot_decision_node.cpp //决策节点源码 184 | 185 | # 决策主要流程图 186 | 187 | ```mermaid 188 | graph LR 189 | A[订阅者消息回调] --> B{消息是否合法?}; 190 | B -- NO ---> C[终止本次决策]; 191 | B -- YES ---> D[处理车辆位置信息]; 192 | D --> E{是否存在感知目标?}; 193 | E -- YES ---> F[使用感知信息更新车辆位置]; 194 | F --> G[提取自身位置并分离敌我车辆位置]; 195 | E -- NO --->G; 196 | G --> H{是否存在TF2坐标系转换关系}; 197 | H -- YES ---> I[使用TF2坐标系转换更新自身位置]; 198 | I --> J{自身位置是否合法?}; 199 | H -- NO ---> K{是否存在Nav2反馈信息}; 200 | K -- YES ---> L[使用Nav2反馈信息更新自身位置]; 201 | L --> J; 202 | K -- NO ---> J; 203 | J -- NO ----->M[终止本次决策]; 204 | J -- YES ---->N[根据敌方位置决策车辆朝向]; 205 | N --> O[根据当前条件选取当前决策]; 206 | O --> P{检查上次决策是否执行完成}; 207 | P -- NO --> Q{当前决策权重是否高于上次决策}; 208 | Q -- YES ---> R[取消上次决策执行]; 209 | Q -- NO --> V; 210 | R --> T[根据决策计算路径]; 211 | P -- YES ---> T; 212 | T --> U[执行\发布决策]; 213 | U --> V[更新GUI]; 214 | ``` 215 | 216 | 程序采用时间相近协议同步接收 /obj_hp 、/car_pos 、/game_info 、/serial_msg 话题的消息，异步接收 /joint_states 、 perception_detector/perception_array 、navigate_through_poses/_action/feedback 、 navigate_through_poses/_action/status的消息，每次回调固定处理车辆朝向和目标决策。 217 | 218 | 决策由车辆当前位置、车辆当前模式、当前比赛时间、己方前哨站当前血量、己方车辆位置、敌方车辆位置共同决定，在有多个决策符合条件的情况下，取权重最高的决策。 219 | 220 | 机器人移动路径点由决策给出，分为直达和连续两种情况。直达情况下，直接给出最终路径点，连续情况下，由图深度优先搜索算法计算一系列连续路径点，可适当缓解Nav2导航问题。 221 | 222 | 车辆朝向由敌方车辆位置决定，通过计算索敌圈内最近敌人方位，得出视野外敌方角度，被障碍遮挡但仍被检测到的敌方位置通过间段采样的方法滤除。 223 | 224 | # GUI 225 | 226 | ![1681128909059](image/README/1681128909059.png) 227 | 228 | GUI可直观显示车辆当前状态、路径点、决策状态、敌我位置与索敌情况。路径点分为当前、符合条件但未被选择、符合条件且被选择和不符合条件四种状态。路径点上的敌我接近情况也被显示。（PS：图上敌我位置由虚假消息发布者随机给出） 229 | 230 | # 模板 231 | 232 | ## 决策： 233 | 234 | { 235 | 236 | "data" : [ 237 | 238 | { 239 | 240 | "id" : 0, //决策ID 241 | 242 | "name" : "test1", //决策命名 243 | 244 | "wayPointID" : [-1], //决策所属路径点，填写多个时，只要当前路径点在列表内，便通过判断，-1不作判断 245 | 246 | "weight" : 500, //决策权重，符合条件的决策中优先取权重大的，大权重决策可覆盖正在执行的决策 247 | 248 | "start_time" : -1, //决策容许时间范围起始，-1不作判断 249 | 250 | "end_time" : 420, //决策容许时间范围结束，-1不作判断 251 | 252 | "robot_mode" : 0, //当前机器人模式，-1不作判断 253 | 254 | "minHP" : -1, //决策容许血量范围起始，-1不作判断 255 | 256 | "maxHP" : -1, //决策容许血量范围结束，-1不作判断 257 | 258 | "decide_mode" : 0, //决策决定机器人模式[6：正常巡航，7：扭腰巡航，8：自瞄] 259 | 260 | "decide_wayPoint" : 0, //决策决定目标路径点（id） 261 | 262 | "out_post_HP_min": 0, //决策容许前哨站血量线，-1不作判断 263 | 264 | "if_succession" : false, //决策路径是否连续，否为直达 265 | 266 | "if_reverse" : true, //是否返程 267 | 268 | "enemyPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]], //敌方车辆当前路径点ID，填写多个时，只要目标单位所在路径点在列表内，便通过判断，-1不作判断 顺序： 英雄 工程 3，4，5步兵，哨兵（UNTESTED） 269 | 270 | "friendPosition" : [[-1],[-1],[-1],[-1],[-1],[-1]] //友方车辆当前路径点ID，填写多个时，只要目标单位所在路径点在列表内，便通过判断，-1不作判断 顺序： 英雄 工程 3，4，5步兵，哨兵（UNTESTED） 271 | 272 | }, 273 | 274 | ... 275 | 276 | ] 277 | 278 | } 279 | 280 | ## 路径点： 281 | 282 | { 283 | 284 | "data": [ 285 | 286 | { 287 | 288 | "id": 0, //路径点ID 289 | 290 | "name": "test1", //路径点命名 291 | 292 | "type": 0, //路径点类型（UNUSED） 293 | 294 | "x": 1.0, //路径点坐标x（真实坐标） 295 | 296 | "y": 1.0, //路径点坐标y（真实坐标） 297 | 298 | "angle": 0.0, //路径点上车辆朝向（弧度制）（UNTESTED） 299 | 300 | "connect": [ //路径点邻接表（配合决策if_succession使用） 301 | 302 | 1 303 | 304 | ], 305 | 306 | "enemyWeights": [ //路径点上敌方选择权重，-1不作判断（UNUSED） 307 | 308 | -1, 309 | 310 | -1, 311 | 312 | -1, 313 | 314 | -1, 315 | 316 | -1 317 | 318 | ] 319 | 320 | }, 321 | 322 | ... 323 | 324 | ] 325 | 326 | } 327 | 328 | ## CONFIG 配置文件： 329 | 330 | "Debug" : false, //是否Debug（UNUSED） 331 | 332 | "WayPointsPATH" : "waypoints.json", //shared/JsonFile文件夹下路径点文件命名 333 | 334 | "DecisionsPATH" : "decisions.json", //shared/JsonFile文件夹下决策文件命名 335 | 336 | "MAP_PATH" : "RMUL.png", //shared/resources文件夹下map命名 337 | 338 | "REAL_WIDTH" : 12.0, //场地真实宽度 339 | 340 | "REAL_HEIGHT": 8.0, //场地真实高度 341 | 342 | "GOAL_TIME_THR_SEC" : 2, //目标执行时间阈值（UNUSED） 343 | 344 | "TIME_THR" : 1, //消息有效时间（秒） 345 | 346 | "CAR_SEEK_FOV" : 70.0, //车辆自瞄FOV 347 | 348 | "INIT_DISTANCE_THR" : 1.0, //路径点判断用距离阈值（米） 349 | 350 | "INIT_SEEK_THR" : 5.0, //索敌圈半径（米） 351 | 352 | "INIT_ISBLUE" : false, //红蓝方判断标志位 *赛前修改 ！important* 353 | 354 | "INIT_IFSHOWUI" : true, //是否显示UI 355 | 356 | "INIT_SELFINDEX" : 5, //车辆当前索引（0-5）[英雄 工程 3，4，5步兵 哨兵] 357 | 358 | "INIT_FRIENDOUTPOSTINDEX" : 6, //前哨站当前索引（0-7）[英雄 工程 3，4，5步兵 哨兵 前哨站 基地] 359 | 360 | "INIT_FRIENDBASEINDEX" : 7, //基地当前索引（0-7）[英雄 工程 3，4，5步兵 哨兵 前哨站 基地] 361 | 362 | "STEP_DISTANCE" : 0.1, //障碍判断步进（米） 363 | 364 | "GAME_TIME" : 420 //比赛持续时间（秒） 365 | 366 | ## 决策、路径点文件编写注意事项： 367 | 368 | 1.对于红蓝方，路径点文件应编写两个以适应场地换向 369 | 370 | 2.不论决策还是路径点文件，决策与路径点的ID应唯一 371 | 372 | 3.应尽量少的使用-1来跳过判断，避免出现意料外的情况 373 | 374 | 4.决策中，决策所属路径点与目标路径点一致时可达成原地驻留的效果 375 | 376 | 5.血量区间与时间区间应尽量减少重合 377 | 378 | 6.同一执行级别下的决策权重应保持一致 379 | -------------------------------------------------------------------------------- /src/robot_decision/include/robot_decision_node.hpp: -------------------------------------------------------------------------------- 1 | #include "./robot_decision/RobotDecision.h" 2 | 3 | #include "global_interface/msg/obj_hp.hpp" 4 | #include "global_interface/msg/car_pos.hpp" 5 | #include "global_interface/msg/game_info.hpp" 6 | #include "global_interface/msg/serial.hpp" 7 | #include "global_interface/msg/detection_array.hpp" 8 | #include "global_interface/msg/decision.hpp" 9 | #include "global_interface/msg/autoaim.hpp" 10 | #include "global_interface/msg/mode_set.hpp" 11 | #include "global_interface/msg/strike_licensing.hpp" 12 | 13 | #include "rclcpp_action/rclcpp_action.hpp" 14 | #include "geometry_msgs/msg/pose.hpp" 15 | #include "std_msgs/msg/float32.hpp" 16 | #include "nav2_util/geometry_utils.hpp" 17 | #include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp" 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace rdsys 31 | { 32 | using namespace std::chrono_literals; 33 | 34 | class RobotDecisionNode : public rclcpp::Node 35 | { 36 | public: 37 | bool _Debug = false; 38 | bool _auto_mode = true; 39 | 40 | double delta_yaw = 0.; 41 | 42 | private: 43 | std::string _WayPointsPath; 44 | std::string _DecisionsPath; 45 | float _INIT_DISTANCE_THR; 46 | float _INIT_SEEK_THR; 47 | bool _INIT_IsBlue; 48 | bool _INIT_IFSHOWUI; 49 | bool _INIT_IFUSEMANUAL; 50 | int _INIT_SELFINDEX; 51 | int _INIT_FRIENDOUTPOSTINDEX; 52 | int _INIT_FRIENDBASEINDEX; 53 | int _GAME_TIME; 54 | int _TIME_THR; 55 | 56 | std::string _MAP_PATH; 57 | float _REAL_WIDTH; 58 | float _REAL_HEIGHT; 59 | float _STEP_DISTANCE; 60 | float _CAR_SEEK_FOV; 61 | 62 | private: 63 | // RealParamValues: 64 | int _selfIndex = 5; 65 | int _friendOutPostIndex = 6; 66 | int _friendBaseIndex = 7; 67 | bool _IfShowUI = false; 68 | bool _IsBlue = false; 69 | 70 | // TempParams: 71 | float _distance_THR_Temp = 0.5; 72 | float _seek_THR_Temp = 5.0; 73 | bool _IfShowUI_Temp = false; 74 | bool _IsBlue_Temp = false; 75 | 76 | const std::map type_id = {std::map::value_type("R1", 0), 77 | std::map::value_type("R2", 1), 78 | std::map::value_type("R3", 2), 79 | std::map::value_type("R4", 3), 80 | std::map::value_type("R5", 4), 81 | std::map::value_type("R7", 5), 82 | std::map::value_type("B1", 6), 83 | std::map::value_type("B2", 7), 84 | std::map::value_type("B3", 8), 85 | std::map::value_type("B4", 9), 86 | std::map::value_type("B5", 10), 87 | std::map::value_type("B7", 11)}; 88 | 89 | const std::vector _car_ids = {0, 1, 2, 3, 4, 6}; 90 | 91 | private: 92 | message_filters::Subscriber objHP_sub_; 93 | message_filters::Subscriber carPos_sub_; 94 | message_filters::Subscriber gameInfo_sub_; 95 | message_filters::Subscriber serial_sub_; 96 | 97 | rclcpp::Subscription::SharedPtr joint_state_sub_; 98 | rclcpp::Subscription::SharedPtr autoaim_sub_; 99 | rclcpp::Subscription::SharedPtr detectionArray_sub_; 100 | rclcpp::Subscription::SharedPtr modeSet_sub_; 101 | 102 | typedef message_filters::sync_policies::ApproximateTime 106 | ApproximateSyncPolicy; 107 | std::unique_ptr> TS_sync_; 108 | 109 | std::shared_ptr myRDS; 110 | 111 | rclcpp_action::Client::SharedPtr nav_through_poses_action_client_; 112 | 113 | std::vector acummulated_poses_; 114 | std::chrono::milliseconds server_timeout_; 115 | 116 | nav2_msgs::action::NavigateThroughPoses::Goal nav_through_poses_goal_; 117 | 118 | std::shared_ptr, (void *)nullptr>> timer_; 119 | 120 | rclcpp::Subscription::SharedPtr 121 | nav_through_poses_feedback_sub_; 122 | rclcpp::Subscription::SharedPtr 123 | nav_through_poses_goal_status_sub_; 124 | 125 | rclcpp::Publisher::SharedPtr 126 | decision_pub_; 127 | rclcpp::Publisher::SharedPtr 128 | strikeLicensing_pub_; 129 | 130 | std::shared_timed_mutex myMutex_status; 131 | std::shared_timed_mutex myMutex_NTP_FeedBack; 132 | std::shared_timed_mutex myMutex_joint_states; 133 | std::shared_timed_mutex myMutex_detectionArray; 134 | std::shared_timed_mutex myMutex_modeSet; 135 | std::shared_timed_mutex myMutex_autoaim; 136 | 137 | int8_t goal_status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN; 138 | nav2_msgs::action::NavigateThroughPoses_FeedbackMessage::SharedPtr current_NTP_FeedBack_msg = nullptr; 139 | 140 | sensor_msgs::msg::JointState::SharedPtr joint_states_msg = nullptr; 141 | global_interface::msg::Autoaim::SharedPtr autoaim_msg = nullptr; 142 | global_interface::msg::DetectionArray::SharedPtr detectionArray_msg = nullptr; 143 | global_interface::msg::ModeSet::SharedPtr modeSet_msg = nullptr; 144 | 145 | std::shared_ptr excuting_decision = nullptr; 146 | 147 | std::unique_ptr tf_buffer_ = std::make_unique(this->get_clock()); 148 | std::shared_ptr transform_listener_ = std::make_shared(*tf_buffer_); 149 | 150 | std::shared_ptr _transformStamped = nullptr; 151 | 152 | private: 153 | /** 154 | * @brief 向目标缓冲区添加目标点 155 | * @param x 156 | * 目标x坐标 157 | * @param y 158 | * 目标y坐标 159 | * @param theta 160 | * 目标theta角 161 | */ 162 | void makeNewGoal(double x, double y, double theta); 163 | /** 164 | * @brief 将消息位置转换成RobotPosition结构体 165 | * @param pos 166 | * 位置集合 167 | * @return 168 | * RobotPosition集合 169 | */ 170 | std::vector point2f2Position(std::array pos); 171 | /** 172 | * @brief 消息回调，由message_filters处理 173 | */ 174 | void messageCallBack(const std::shared_ptr &carHP_msg_, 175 | const std::shared_ptr &carPos_msg_, 176 | const std::shared_ptr &gameInfo_msg_, 177 | const std::shared_ptr &serial_sub_); 178 | /** 179 | * @brief joint_states消息回调 180 | */ 181 | void jointStateCallBack(const sensor_msgs::msg::JointState::SharedPtr msg); 182 | /** 183 | * @brief armor_info消息回调 184 | */ 185 | void autoaimCallBack(const global_interface::msg::Autoaim::SharedPtr msg); 186 | /** 187 | * @brief 检测目标消息回调 188 | */ 189 | void detectionArrayCallBack(const global_interface::msg::DetectionArray::SharedPtr msg); 190 | /** 191 | * @brief 云台手模式设置消息回调 192 | */ 193 | void modeSetCallBack(const global_interface::msg::ModeSet::SharedPtr msg); 194 | /** 195 | * @brief nav2反馈回调 196 | */ 197 | void nav2FeedBackCallBack(const nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage::SharedPtr msg); 198 | /** 199 | * @brief nav2目标状态回调 200 | */ 201 | void nav2GoalStatusCallBack(const action_msgs::msg::GoalStatusArray::SharedPtr msg); 202 | /** 203 | * @brief 周期性参数回调 204 | */ 205 | void respond(); 206 | /** 207 | * @brief 决策处理一次 208 | * @param _HP 209 | * 当前血量 210 | * @param mode 211 | * 当前模式 212 | * @param _x 213 | * 当前x轴坐标 214 | * @param _y 215 | * 当前y轴坐标 216 | * @param time 217 | * 比赛时间 218 | * @param now_out_post_HP 219 | * 友方前哨站当前血量 220 | * @param friendPositions 221 | * 友方位置集合 222 | * @param enemyPositions 223 | * 敌方位置集合 224 | * @return 225 | * 处理是否成功 226 | */ 227 | bool process_once(int &_HP, int &mode, float &_x, float &_y, int &time, int &now_out_post_HP, int &now_base_HP, std::vector &friendPositions, std::vector &enemyPositions, geometry_msgs::msg::TransformStamped::SharedPtr transformStamped); 228 | /** 229 | * @brief 根据决策创建消息 230 | * @param decision 231 | * 决策 232 | * @param theta 233 | * 车辆朝向（弧度制）（偏角） 234 | * @return 235 | * 决策消息 236 | */ 237 | global_interface::msg::Decision makeDecisionMsg(std::shared_ptr decision, double &theta); 238 | /** 239 | * @brief 手动创建决策消息 240 | * @param mode 241 | * 决策模式 242 | * @param theta 243 | * 车辆朝向（弧度制）（偏角） 244 | * @param _x 245 | * 决策x坐标 246 | * @param _y 247 | * 决策y坐标 248 | * @return 249 | * 决策消息 250 | */ 251 | global_interface::msg::Decision makeDecisionMsg(int mode, double theta, float _x, float _y); 252 | /** 253 | * @brief 载入配置信息 254 | * @return 255 | * 是否成功 256 | */ 257 | bool decodeConfig(); 258 | /** 259 | * @brief 清空目标 260 | */ 261 | void clearGoals(); 262 | 263 | public: 264 | RobotDecisionNode(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); 265 | ~RobotDecisionNode(); 266 | 267 | public: 268 | /** 269 | * @brief 初始化 270 | * @param waypointsPath 271 | * 路径点Json文件路径 272 | * @param decisionsPath 273 | * 决策Json文件路径 274 | */ 275 | void init(); 276 | }; 277 | } -------------------------------------------------------------------------------- /src/robot_decision/include/robot_decision/RobotDecision.h: -------------------------------------------------------------------------------- 1 | #ifndef _RD_DECISION_H 2 | #define _RD_DECISION_H 3 | 4 | #include "./structs.h" 5 | 6 | namespace rdsys 7 | { 8 | /** 9 | * @brief 裁判系统信息处理类 10 | * 处理存储比赛信息 11 | */ 12 | class GameHandler 13 | { 14 | private: 15 | std::chrono::time_point lastUpdateTime; 16 | int gameTime = -1; 17 | 18 | public: 19 | GameHandler(); 20 | ~GameHandler(); 21 | 22 | void update(int &gameTime); 23 | }; 24 | /** 25 | * @brief 决策系统类 26 | * 提供机器人决策相关处理接口 27 | */ 28 | class RobotDecisionSys 29 | { 30 | private: 31 | std::string _MAP_PATH; 32 | float _REAL_WIDTH; 33 | float _REAL_HEIGHT; 34 | float _STEP_DISTANCE; 35 | float _CAR_SEEK_FOV; 36 | 37 | private: 38 | bool IfShowUI = false; 39 | bool IfUIInited = false; 40 | 41 | public: 42 | /** 43 | * @brief 获取UI显示标志位 44 | * @return 45 | * 当前UI显示标志位 46 | */ 47 | bool getIfShowUI(); 48 | /** 49 | * @brief 设置UI显示标志位 50 | * @param ifShowUI 51 | * 目标UI显示标志位 52 | */ 53 | void setIfShowUI(bool ifShowUI); 54 | 55 | private: 56 | std::vector> wayPointMap; 57 | std::vector> decisions; 58 | 59 | float _distance_THR = 0.; 60 | float _seek_THR = 5.0; 61 | 62 | private: 63 | std::map> connection_map; 64 | std::shared_ptr myGameHandler = nullptr; 65 | 66 | public: 67 | /** 68 | * @brief 计算角度 69 | * @param x1 70 | * 点1 x坐标 71 | * @param y1 72 | * 点1 y坐标 73 | * @param x2 74 | * 点2 x坐标 75 | * @param y2 76 | * 点2 y坐标 77 | * @return 78 | * 角度值（弧度制） 79 | */ 80 | double calculateAngle(double x1, double y1, double x2, double y2); 81 | /** 82 | * @brief 计算角度 83 | * @param p1 84 | * 点1 85 | * @param p2 86 | * 点2 87 | * @return 88 | * 角度值（弧度制） 89 | */ 90 | double calculateAngle(cv::Point2d p1, cv::Point2d p2); 91 | 92 | private: 93 | /** 94 | * @brief 计算机器人当前所在路径点 95 | * @param pos 96 | * 当前机器人坐标信息 97 | */ 98 | int calculatePosition(RobotPosition &pos); 99 | /** 100 | * @brief 通过角度获取终止点 101 | * @param x1 102 | * 点1 x坐标 103 | * @param y1 104 | * 点1 y坐标 105 | * @param theta 106 | * 角度（弧度制） 107 | * @param length 108 | * 两点距离 109 | * @return 110 | * 结束点 111 | */ 112 | cv::Point2i createEndPointByTheta(double x1, double y1, double theta, int length); 113 | /** 114 | * @brief 通过角度获取终止点 115 | * @param start 116 | * 起点 117 | * @param theta 118 | * 角度（弧度制） 119 | * @param length 120 | * 两点距离 121 | * @return 122 | * 结束点 123 | */ 124 | cv::Point2i createEndPointByTheta(cv::Point start, double theta, int length); 125 | /** 126 | * @brief 通过角度与起终点判断线上是否有障碍 127 | * @param start 128 | * 起点 129 | * @param theta 130 | * 角度（弧度制） 131 | * @param distance 132 | * 终点距离 133 | * @return 134 | * 是否有阻碍 135 | */ 136 | bool checkBlock(cv::Point start, double theta, int distance); 137 | 138 | public: 139 | RobotDecisionSys(float &_distance_THR, float &_seek_THR, float &_REAL_WIDTH, float &_REAL_HEIGHT, std::string &_MAP_PATH, float &_STEP_DISTANCE, float &_CAR_SEEK_FOV); 140 | ~RobotDecisionSys(); 141 | 142 | /** 143 | * @brief 解码路径点Json文件 144 | * @param filePath 145 | * 路径点Json文件地址 146 | * @return 147 | * 是否成功 148 | */ 149 | bool decodeWayPoints(std::string &filePath); 150 | /** 151 | * @brief 解码决策Json文件 152 | * @param filePath 153 | * 决策Json文件地址 154 | * @return 155 | * 是否成功 156 | */ 157 | bool decodeDecisions(std::string &filePath); 158 | 159 | /** 160 | * @brief 检查机器人当前所在路径点 161 | * @param x 162 | * 机器人坐标x 163 | * @param y 164 | * 机器人坐标y 165 | * @return 166 | * 路径点ID 167 | */ 168 | int checkNowWayPoint(float x, float y); 169 | /** 170 | * @brief 检查机器人当前所在路径点 171 | * @param pos 172 | * 当前机器人坐标信息 173 | * @return 174 | * 路径点ID 175 | */ 176 | int checkNowWayPoint(RobotPosition pos); 177 | /** 178 | * @brief 机器人决策 179 | * @param wayPointID 180 | * 当前机器人路径点ID 181 | * @param robot_mode 182 | * 当前机器人模式 183 | * @param _HP 184 | * 当前机器人血量 185 | * @param nowtime 186 | * 当前比赛剩余时间 187 | * @param now_out_post_HP 188 | * 当前前哨站血量 189 | * @param now_base_HP 190 | * 当前基地血量 191 | * @param friendPositions 192 | * 友方机器人位置（真实坐标） 193 | * @param enemtPositions 194 | * 敌方机器人位置（真实坐标） 195 | * @param availableDecisionID 196 | * 输入输出符合条件的决策ID 197 | * @param id_pos_f 198 | * 友方位置（路径点） 199 | * @param id_pos_e 200 | * 敌方位置（路径点） 201 | * @return 202 | * 决策 203 | */ 204 | std::shared_ptr decide(int wayPointID, int robot_mode, int _HP, int nowtime, int now_out_post_HP, int now_base_HP, std::vector &friendPositions, std::vector &enemyPositions, std::vector &availableDecisionID, std::map &id_pos_f, std::map &id_pos_e); 205 | /** 206 | * @brief 计算路径路线 207 | * @param startWapPointID 208 | * 起点路径点ID 209 | * @param endWapPointID 210 | * 终点路径点ID 211 | * @return 212 | * 路径点集合 213 | */ 214 | std::vector> calculatePath(int startWapPointID, int endWapPointID); 215 | /** 216 | * @brief 根据ID获取路径点 217 | * @param id 218 | * 路径点ID 219 | * @return 220 | * 路径点 221 | */ 222 | std::shared_ptr getWayPointByID(int id); 223 | /** 224 | * @brief 根据ID获取决策 225 | * @param id 226 | * 决策ID 227 | * @return 228 | * 决策 229 | */ 230 | std::shared_ptr getDecisionByID(int id); 231 | 232 | /** 233 | * @brief 决策打击目标 234 | * @param _IsBlue 235 | * 是否是蓝方 236 | * @param mypos 237 | * 当前机器人位置 238 | * @param enemyPositions 239 | * 敌方机器人位置 240 | * @param detectedEnemy 241 | * 检测到的敌方目标ID 242 | * @param enemyHP 243 | * 敌方血量(仅填入车辆血量) 244 | * @param distance_weight 245 | * 距离权重 246 | * @param hp_weight 247 | * 血量权重 248 | * @return 249 | * 打击权重 250 | */ 251 | std::map decideAimTarget(bool _IsBlue, RobotPosition &mypos, std::vector &enemyPositions, std::vector &enemyHP, float distance_weight_ratio, float hp_weight_ratio, bool enemyOutpostDown); 252 | /** 253 | * @brief 决策yaw轴角度，通用坐标系 254 | * @param _x 255 | * 当前机器人x轴坐标（真实坐标） 256 | * @param _y 257 | * 当前机器人y轴坐标（真实坐标） 258 | * @param enemyPositions 259 | * 敌方机器人位置（真实坐标） 260 | * @return 261 | * yaw轴角度 262 | */ 263 | double decideAngleByEnemyPos(float _x, float _y, std::vector &enemyPositions); 264 | 265 | /** 266 | * @brief 获取距离阈值，用于计算路径点 267 | * @return 268 | * 距离阈值 269 | */ 270 | float getDistanceTHR(); 271 | /** 272 | * @brief 设置距离阈值，用于计算路径点 273 | * @param thr 274 | * 距离阈值 275 | */ 276 | void setDistanceTHR(float thr); 277 | /** 278 | * @brief 获取距离阈值，用于索敌 279 | * @return 280 | * 距离阈值 281 | */ 282 | float getSeekTHR(); 283 | /** 284 | * @brief 设置距离阈值，用于索敌 285 | * @param thr 286 | * 距离阈值 287 | */ 288 | void setSeekTHR(float thr); 289 | 290 | private: 291 | cv::Mat decisionMap; 292 | cv::Mat decisionMap_Gray; 293 | 294 | public: 295 | /** 296 | * @brief 更新路径状态图UI 297 | * @param activateDecisionID 298 | * 激活决策ID 299 | * @param availableDecisionID 300 | * 符合条件的决策ID 301 | * @param nowWayPoint 302 | * 当前所在路径点 303 | * @param yaw 304 | * 当前云台yaw轴角度（弧度制） 305 | * @param car_center 306 | * 当前车辆坐标（真实坐标） 307 | * @param car_orientation 308 | * 当前车辆朝向角度（弧度制） 309 | * @param aim_yaw 310 | * 目标云台yaw轴角度（弧度制） 311 | * @param friendPositions 312 | * 友方机器人位置（真实坐标） 313 | * @param enemtPositions 314 | * 敌方机器人位置（真实坐标） 315 | * @param id_pos_f 316 | * 友方位置（路径点） 317 | * @param id_pos_e 318 | * 敌方位置（路径点） 319 | * @param aimWayPoints 320 | * 决策路径点的集合 321 | */ 322 | void UpdateDecisionMap(int &activateDecisionID, std::vector &availableDecisionID, int &nowWayPoint, double yaw, cv::Point2f car_center, double car_orientation, double aim_yaw, std::vector &friendPositions, std::vector &enemyPositions, std::map &id_pos_f, std::map &id_pos_e); 323 | 324 | private: 325 | /** 326 | * @brief 绘制路径点 327 | * @param img 328 | * 目标绘制图像 329 | * @param center 330 | * 路径点坐标（图像坐标） 331 | * @param id 332 | * 路径点ID 333 | * @param type 334 | * 路径点状态（0未激活-灰色，1符合条件-蓝色，2激活-绿色，3当前-黄色） 335 | */ 336 | void drawWayPoint(cv::Mat &img, cv::Point2i center, int id, int type); 337 | /** 338 | * @brief 变换中心点 339 | * @param _x 340 | * 中心点x轴坐标（真实坐标） 341 | * @param _y 342 | * 中心点y轴坐标（真实坐标） 343 | * @param width 344 | * 场地宽度 345 | * @param height 346 | * 场地高度 347 | * @param img_cols 348 | * 图像宽度 349 | * @param img_rows 350 | * 图像高度 351 | * @return 352 | * 真实坐标对应的图像坐标 353 | */ 354 | cv::Point2i transformPoint(float _x, float _y, float width, float height, int img_cols, int img_rows); 355 | /** 356 | * @brief 变换中心点 357 | * @param center 358 | * 中心点（真实坐标） 359 | * @param width 360 | * 场地宽度 361 | * @param height 362 | * 场地高度 363 | * @param img_cols 364 | * 图像宽度 365 | * @param img_rows 366 | * 图像高度 367 | * @return 368 | * 真实坐标对应的图像坐标 369 | */ 370 | cv::Point2i transformPoint(cv::Point2f center, float width, float height, int img_cols, int img_rows); 371 | /** 372 | * @brief 绘制车辆 373 | * @param img 374 | * 目标绘制图像 375 | * @param center 376 | * 车辆位置（图像坐标） 377 | * @param car_orientation 378 | * 车辆朝向 379 | * @param yaw 380 | * yaw轴朝向 381 | */ 382 | void drawCar(cv::Mat &img, cv::Point2i center, double &car_orientation, double &yaw, double &aim_yaw); 383 | /** 384 | * @brief 绘制敌方车辆 385 | * @param img 386 | * 目标绘制图像 387 | * @param center 388 | * 车辆位置（图像坐标） 389 | * @param type 390 | * 车辆所属(0：友方，1：敌方) 391 | */ 392 | void drawOthCar(cv::Mat &img, cv::Point2i center, int &id, int type); 393 | }; 394 | } 395 | #endif -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /src/robot_decision/include/Json/json-forwards.h: -------------------------------------------------------------------------------- 1 | /// Json-cpp amalgamated forward header (http://jsoncpp.sourceforge.net/). 2 | /// It is intended to be used with #include "json/json-forwards.h" 3 | /// This header provides forward declaration for all JsonCpp types. 4 | 5 | // ////////////////////////////////////////////////////////////////////// 6 | // Beginning of content of file: LICENSE 7 | // ////////////////////////////////////////////////////////////////////// 8 | 9 | /* 10 | The JsonCpp library's source code, including accompanying documentation, 11 | tests and demonstration applications, are licensed under the following 12 | conditions... 13 | 14 | Baptiste Lepilleur and The JsonCpp Authors explicitly disclaim copyright in all 15 | jurisdictions which recognize such a disclaimer. In such jurisdictions, 16 | this software is released into the Public Domain. 17 | 18 | In jurisdictions which do not recognize Public Domain property (e.g. Germany as of 19 | 2010), this software is Copyright (c) 2007-2010 by Baptiste Lepilleur and 20 | The JsonCpp Authors, and is released under the terms of the MIT License (see below). 21 | 22 | In jurisdictions which recognize Public Domain property, the user of this 23 | software may choose to accept it either as 1) Public Domain, 2) under the 24 | conditions of the MIT License (see below), or 3) under the terms of dual 25 | Public Domain/MIT License conditions described here, as they choose. 26 | 27 | The MIT License is about as close to Public Domain as a license can get, and is 28 | described in clear, concise terms at: 29 | 30 | http://en.wikipedia.org/wiki/MIT_License 31 | 32 | The full text of the MIT License follows: 33 | 34 | ======================================================================== 35 | Copyright (c) 2007-2010 Baptiste Lepilleur and The JsonCpp Authors 36 | 37 | Permission is hereby granted, free of charge, to any person 38 | obtaining a copy of this software and associated documentation 39 | files (the "Software"), to deal in the Software without 40 | restriction, including without limitation the rights to use, copy, 41 | modify, merge, publish, distribute, sublicense, and/or sell copies 42 | of the Software, and to permit persons to whom the Software is 43 | furnished to do so, subject to the following conditions: 44 | 45 | The above copyright notice and this permission notice shall be 46 | included in all copies or substantial portions of the Software. 47 | 48 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 49 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 50 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 51 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 52 | BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 53 | ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 54 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 55 | SOFTWARE. 56 | ======================================================================== 57 | (END LICENSE TEXT) 58 | 59 | The MIT license is compatible with both the GPL and commercial 60 | software, affording one all of the rights of Public Domain with the 61 | minor nuisance of being required to keep the above copyright notice 62 | and license text in the source code. Note also that by accepting the 63 | Public Domain "license" you can re-license your copy using whatever 64 | license you like. 65 | 66 | */ 67 | 68 | // ////////////////////////////////////////////////////////////////////// 69 | // End of content of file: LICENSE 70 | // ////////////////////////////////////////////////////////////////////// 71 | 72 | 73 | 74 | 75 | 76 | #ifndef JSON_FORWARD_AMALGAMATED_H_INCLUDED 77 | # define JSON_FORWARD_AMALGAMATED_H_INCLUDED 78 | /// If defined, indicates that the source file is amalgamated 79 | /// to prevent private header inclusion. 80 | #define JSON_IS_AMALGAMATION 81 | 82 | // ////////////////////////////////////////////////////////////////////// 83 | // Beginning of content of file: include/json/version.h 84 | // ////////////////////////////////////////////////////////////////////// 85 | 86 | #ifndef JSON_VERSION_H_INCLUDED 87 | #define JSON_VERSION_H_INCLUDED 88 | 89 | // Note: version must be updated in three places when doing a release. This 90 | // annoying process ensures that amalgamate, CMake, and meson all report the 91 | // correct version. 92 | // 1. /meson.build 93 | // 2. /include/json/version.h 94 | // 3. /CMakeLists.txt 95 | // IMPORTANT: also update the SOVERSION!! 96 | 97 | #define JSONCPP_VERSION_STRING "1.9.5" 98 | #define JSONCPP_VERSION_MAJOR 1 99 | #define JSONCPP_VERSION_MINOR 9 100 | #define JSONCPP_VERSION_PATCH 5 101 | #define JSONCPP_VERSION_QUALIFIER 102 | #define JSONCPP_VERSION_HEXA \ 103 | ((JSONCPP_VERSION_MAJOR << 24) | (JSONCPP_VERSION_MINOR << 16) | \ 104 | (JSONCPP_VERSION_PATCH << 8)) 105 | 106 | #ifdef JSONCPP_USING_SECURE_MEMORY 107 | #undef JSONCPP_USING_SECURE_MEMORY 108 | #endif 109 | #define JSONCPP_USING_SECURE_MEMORY 0 110 | // If non-zero, the library zeroes any memory that it has allocated before 111 | // it frees its memory. 112 | 113 | #endif // JSON_VERSION_H_INCLUDED 114 | 115 | // ////////////////////////////////////////////////////////////////////// 116 | // End of content of file: include/json/version.h 117 | // ////////////////////////////////////////////////////////////////////// 118 | 119 | 120 | 121 | 122 | 123 | 124 | // ////////////////////////////////////////////////////////////////////// 125 | // Beginning of content of file: include/json/allocator.h 126 | // ////////////////////////////////////////////////////////////////////// 127 | 128 | // Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors 129 | // Distributed under MIT license, or public domain if desired and 130 | // recognized in your jurisdiction. 131 | // See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE 132 | 133 | #ifndef JSON_ALLOCATOR_H_INCLUDED 134 | #define JSON_ALLOCATOR_H_INCLUDED 135 | 136 | #include 137 | #include 138 | 139 | #pragma pack(push, 8) 140 | 141 | namespace Json { 142 | template class SecureAllocator { 143 | public: 144 | // Type definitions 145 | using value_type = T; 146 | using pointer = T*; 147 | using const_pointer = const T*; 148 | using reference = T&; 149 | using const_reference = const T&; 150 | using size_type = std::size_t; 151 | using difference_type = std::ptrdiff_t; 152 | 153 | /** 154 | * Allocate memory for N items using the standard allocator. 155 | */ 156 | pointer allocate(size_type n) { 157 | // allocate using "global operator new" 158 | return static_cast(::operator new(n * sizeof(T))); 159 | } 160 | 161 | /** 162 | * Release memory which was allocated for N items at pointer P. 163 | * 164 | * The memory block is filled with zeroes before being released. 165 | */ 166 | void deallocate(pointer p, size_type n) { 167 | // memset_s is used because memset may be optimized away by the compiler 168 | memset_s(p, n * sizeof(T), 0, n * sizeof(T)); 169 | // free using "global operator delete" 170 | ::operator delete(p); 171 | } 172 | 173 | /** 174 | * Construct an item in-place at pointer P. 175 | */ 176 | template void construct(pointer p, Args&&... args) { 177 | // construct using "placement new" and "perfect forwarding" 178 | ::new (static_cast(p)) T(std::forward(args)...); 179 | } 180 | 181 | size_type max_size() const { return size_t(-1) / sizeof(T); } 182 | 183 | pointer address(reference x) const { return std::addressof(x); } 184 | 185 | const_pointer address(const_reference x) const { return std::addressof(x); } 186 | 187 | /** 188 | * Destroy an item in-place at pointer P. 189 | */ 190 | void destroy(pointer p) { 191 | // destroy using "explicit destructor" 192 | p->~T(); 193 | } 194 | 195 | // Boilerplate 196 | SecureAllocator() {} 197 | template SecureAllocator(const SecureAllocator&) {} 198 | template struct rebind { using other = SecureAllocator; }; 199 | }; 200 | 201 | template 202 | bool operator==(const SecureAllocator&, const SecureAllocator&) { 203 | return true; 204 | } 205 | 206 | template 207 | bool operator!=(const SecureAllocator&, const SecureAllocator&) { 208 | return false; 209 | } 210 | 211 | } // namespace Json 212 | 213 | #pragma pack(pop) 214 | 215 | #endif // JSON_ALLOCATOR_H_INCLUDED 216 | 217 | // ////////////////////////////////////////////////////////////////////// 218 | // End of content of file: include/json/allocator.h 219 | // ////////////////////////////////////////////////////////////////////// 220 | 221 | 222 | 223 | 224 | 225 | 226 | // ////////////////////////////////////////////////////////////////////// 227 | // Beginning of content of file: include/json/config.h 228 | // ////////////////////////////////////////////////////////////////////// 229 | 230 | // Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors 231 | // Distributed under MIT license, or public domain if desired and 232 | // recognized in your jurisdiction. 233 | // See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE 234 | 235 | #ifndef JSON_CONFIG_H_INCLUDED 236 | #define JSON_CONFIG_H_INCLUDED 237 | #include 238 | #include 239 | #include 240 | #include 241 | #include 242 | #include 243 | #include 244 | #include 245 | 246 | // If non-zero, the library uses exceptions to report bad input instead of C 247 | // assertion macros. The default is to use exceptions. 248 | #ifndef JSON_USE_EXCEPTION 249 | #define JSON_USE_EXCEPTION 1 250 | #endif 251 | 252 | // Temporary, tracked for removal with issue #982. 253 | #ifndef JSON_USE_NULLREF 254 | #define JSON_USE_NULLREF 1 255 | #endif 256 | 257 | /// If defined, indicates that the source file is amalgamated 258 | /// to prevent private header inclusion. 259 | /// Remarks: it is automatically defined in the generated amalgamated header. 260 | // #define JSON_IS_AMALGAMATION 261 | 262 | // Export macros for DLL visibility 263 | #if defined(JSON_DLL_BUILD) 264 | #if defined(_MSC_VER) || defined(__MINGW32__) 265 | #define JSON_API __declspec(dllexport) 266 | #define JSONCPP_DISABLE_DLL_INTERFACE_WARNING 267 | #elif defined(__GNUC__) || defined(__clang__) 268 | #define JSON_API __attribute__((visibility("default"))) 269 | #endif // if defined(_MSC_VER) 270 | 271 | #elif defined(JSON_DLL) 272 | #if defined(_MSC_VER) || defined(__MINGW32__) 273 | #define JSON_API __declspec(dllimport) 274 | #define JSONCPP_DISABLE_DLL_INTERFACE_WARNING 275 | #endif // if defined(_MSC_VER) 276 | #endif // ifdef JSON_DLL_BUILD 277 | 278 | #if !defined(JSON_API) 279 | #define JSON_API 280 | #endif 281 | 282 | #if defined(_MSC_VER) && _MSC_VER < 1800 283 | #error \ 284 | "ERROR: Visual Studio 12 (2013) with _MSC_VER=1800 is the oldest supported compiler with sufficient C++11 capabilities" 285 | #endif 286 | 287 | #if defined(_MSC_VER) && _MSC_VER < 1900 288 | // As recommended at 289 | // https://stackoverflow.com/questions/2915672/snprintf-and-visual-studio-2010 290 | extern JSON_API int msvc_pre1900_c99_snprintf(char* outBuf, size_t size, 291 | const char* format, ...); 292 | #define jsoncpp_snprintf msvc_pre1900_c99_snprintf 293 | #else 294 | #define jsoncpp_snprintf std::snprintf 295 | #endif 296 | 297 | // If JSON_NO_INT64 is defined, then Json only support C++ "int" type for 298 | // integer 299 | // Storages, and 64 bits integer support is disabled. 300 | // #define JSON_NO_INT64 1 301 | 302 | // JSONCPP_OVERRIDE is maintained for backwards compatibility of external tools. 303 | // C++11 should be used directly in JSONCPP. 304 | #define JSONCPP_OVERRIDE override 305 | 306 | #ifdef __clang__ 307 | #if __has_extension(attribute_deprecated_with_message) 308 | #define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) 309 | #endif 310 | #elif defined(__GNUC__) // not clang (gcc comes later since clang emulates gcc) 311 | #if (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)) 312 | #define JSONCPP_DEPRECATED(message) __attribute__((deprecated(message))) 313 | #elif (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 1)) 314 | #define JSONCPP_DEPRECATED(message) __attribute__((__deprecated__)) 315 | #endif // GNUC version 316 | #elif defined(_MSC_VER) // MSVC (after clang because clang on Windows emulates 317 | // MSVC) 318 | #define JSONCPP_DEPRECATED(message) __declspec(deprecated(message)) 319 | #endif // __clang__ || __GNUC__ || _MSC_VER 320 | 321 | #if !defined(JSONCPP_DEPRECATED) 322 | #define JSONCPP_DEPRECATED(message) 323 | #endif // if !defined(JSONCPP_DEPRECATED) 324 | 325 | #if defined(__clang__) || (defined(__GNUC__) && (__GNUC__ >= 6)) 326 | #define JSON_USE_INT64_DOUBLE_CONVERSION 1 327 | #endif 328 | 329 | #if !defined(JSON_IS_AMALGAMATION) 330 | 331 | #include "allocator.h" 332 | #include "version.h" 333 | 334 | #endif // if !defined(JSON_IS_AMALGAMATION) 335 | 336 | namespace Json { 337 | using Int = int; 338 | using UInt = unsigned int; 339 | #if defined(JSON_NO_INT64) 340 | using LargestInt = int; 341 | using LargestUInt = unsigned int; 342 | #undef JSON_HAS_INT64 343 | #else // if defined(JSON_NO_INT64) 344 | // For Microsoft Visual use specific types as long long is not supported 345 | #if defined(_MSC_VER) // Microsoft Visual Studio 346 | using Int64 = __int64; 347 | using UInt64 = unsigned __int64; 348 | #else // if defined(_MSC_VER) // Other platforms, use long long 349 | using Int64 = int64_t; 350 | using UInt64 = uint64_t; 351 | #endif // if defined(_MSC_VER) 352 | using LargestInt = Int64; 353 | using LargestUInt = UInt64; 354 | #define JSON_HAS_INT64 355 | #endif // if defined(JSON_NO_INT64) 356 | 357 | template 358 | using Allocator = 359 | typename std::conditional, 360 | std::allocator>::type; 361 | using String = std::basic_string, Allocator>; 362 | using IStringStream = 363 | std::basic_istringstream; 365 | using OStringStream = 366 | std::basic_ostringstream; 368 | using IStream = std::istream; 369 | using OStream = std::ostream; 370 | } // namespace Json 371 | 372 | // Legacy names (formerly macros). 373 | using JSONCPP_STRING = Json::String; 374 | using JSONCPP_ISTRINGSTREAM = Json::IStringStream; 375 | using JSONCPP_OSTRINGSTREAM = Json::OStringStream; 376 | using JSONCPP_ISTREAM = Json::IStream; 377 | using JSONCPP_OSTREAM = Json::OStream; 378 | 379 | #endif // JSON_CONFIG_H_INCLUDED 380 | 381 | // ////////////////////////////////////////////////////////////////////// 382 | // End of content of file: include/json/config.h 383 | // ////////////////////////////////////////////////////////////////////// 384 | 385 | 386 | 387 | 388 | 389 | 390 | // ////////////////////////////////////////////////////////////////////// 391 | // Beginning of content of file: include/json/forwards.h 392 | // ////////////////////////////////////////////////////////////////////// 393 | 394 | // Copyright 2007-2010 Baptiste Lepilleur and The JsonCpp Authors 395 | // Distributed under MIT license, or public domain if desired and 396 | // recognized in your jurisdiction. 397 | // See file LICENSE for detail or copy at http://jsoncpp.sourceforge.net/LICENSE 398 | 399 | #ifndef JSON_FORWARDS_H_INCLUDED 400 | #define JSON_FORWARDS_H_INCLUDED 401 | 402 | #if !defined(JSON_IS_AMALGAMATION) 403 | #include "config.h" 404 | #endif // if !defined(JSON_IS_AMALGAMATION) 405 | 406 | namespace Json { 407 | 408 | // writer.h 409 | class StreamWriter; 410 | class StreamWriterBuilder; 411 | class Writer; 412 | class FastWriter; 413 | class StyledWriter; 414 | class StyledStreamWriter; 415 | 416 | // reader.h 417 | class Reader; 418 | class CharReader; 419 | class CharReaderBuilder; 420 | 421 | // json_features.h 422 | class Features; 423 | 424 | // value.h 425 | using ArrayIndex = unsigned int; 426 | class StaticString; 427 | class Path; 428 | class PathArgument; 429 | class Value; 430 | class ValueIteratorBase; 431 | class ValueIterator; 432 | class ValueConstIterator; 433 | 434 | } // namespace Json 435 | 436 | #endif // JSON_FORWARDS_H_INCLUDED 437 | 438 | // ////////////////////////////////////////////////////////////////////// 439 | // End of content of file: include/json/forwards.h 440 | // ////////////////////////////////////////////////////////////////////// 441 | 442 | 443 | 444 | 445 | 446 | #endif //ifndef JSON_FORWARD_AMALGAMATED_H_INCLUDED 447 | -------------------------------------------------------------------------------- /src/robot_decision/src/robot_decision/RobotDecision.cpp: -------------------------------------------------------------------------------- 1 | #include "../../include/robot_decision/RobotDecision.h" 2 | 3 | namespace rdsys 4 | { 5 | GameHandler::GameHandler() 6 | { 7 | } 8 | 9 | GameHandler::~GameHandler() 10 | { 11 | } 12 | 13 | void GameHandler::update(int &gameTime) 14 | { 15 | this->lastUpdateTime = std::chrono::time_point_cast(std::chrono::system_clock::now()); 16 | this->gameTime = gameTime; 17 | } 18 | 19 | int RobotDecisionSys::calculatePosition(RobotPosition &pos) 20 | { 21 | double distance = FLT_MAX; 22 | int id = -1; 23 | for (auto it : this->wayPointMap) 24 | { 25 | double tempDistance = sqrtf(powf(double(it->x - pos.x), 2) + powf(double(it->y - pos.y), 2)); 26 | if (tempDistance - distance < 0.) 27 | { 28 | distance = tempDistance; 29 | id = it->id; 30 | } 31 | } 32 | return distance - this->_distance_THR > 0. ? -1 : id; 33 | } 34 | 35 | std::vector> RobotDecisionSys::calculatePath(int startWapPointID, int endWapPointID) 36 | { 37 | std::vector> result; 38 | std::vector container = {startWapPointID}; 39 | std::map waypoint_flag; 40 | waypoint_flag.insert(std::make_pair(startWapPointID, true)); 41 | bool flag = true; 42 | while (flag) 43 | { 44 | int current_waypoint = container.back(); 45 | auto iter = connection_map.find(current_waypoint); 46 | bool flag2 = false; 47 | for (auto &it : iter->second) 48 | { 49 | auto iter2 = waypoint_flag.find(it); 50 | if (iter2 == waypoint_flag.end()) 51 | { 52 | waypoint_flag.insert(std::make_pair(it, true)); 53 | container.emplace_back(it); 54 | flag2 = true; 55 | flag = it != endWapPointID; 56 | } 57 | if (flag2) 58 | break; 59 | } 60 | if (!flag2) 61 | { 62 | container.pop_back(); 63 | } 64 | if (waypoint_flag.size() == wayPointMap.size()) 65 | break; 66 | } 67 | for (auto &it : container) 68 | { 69 | if (it != container.front()) 70 | result.emplace_back(this->getWayPointByID(it)); 71 | } 72 | return result; 73 | } 74 | 75 | cv::Point2i RobotDecisionSys::createEndPointByTheta(double x1, double y1, double theta, int length) 76 | { 77 | theta = theta < 0. ? 2. * CV_PI - abs(theta) : theta; 78 | return cv::Point2i((int)round(x1 + length * cos(theta)), (int)round(y1 + length * sin(theta))); 79 | } 80 | 81 | cv::Point2i RobotDecisionSys::createEndPointByTheta(cv::Point start, double theta, int length) 82 | { 83 | theta = theta < 0. ? 2. * CV_PI - abs(theta) : theta; 84 | return cv::Point2i((int)round(start.x + length * cos(theta)), (int)round(start.y + length * sin(theta))); 85 | } 86 | 87 | bool RobotDecisionSys::checkBlock(cv::Point start, double theta, int distance) 88 | { 89 | int temp_step = int(this->_STEP_DISTANCE * (1080. / this->_REAL_HEIGHT)); 90 | for (float distance_step = temp_step; distance_step < distance; distance_step += temp_step) 91 | { 92 | cv::Point2i check_point = this->createEndPointByTheta(start, theta, distance_step); 93 | if ((int)this->decisionMap_Gray.at(check_point.y, check_point.x) < 10) 94 | { 95 | return true; 96 | } 97 | } 98 | return false; 99 | } 100 | 101 | RobotDecisionSys::RobotDecisionSys(float &_distance_THR, float &_seek_THR, float &_REAL_WIDTH, float &_REAL_HEIGHT, std::string &_MAP_PATH, float &_STEP_DISTANCE, float &_CAR_SEEK_FOV) 102 | { 103 | this->_distance_THR = _distance_THR; 104 | this->_seek_THR = _seek_THR; 105 | this->_REAL_WIDTH = _REAL_WIDTH; 106 | this->_REAL_HEIGHT = _REAL_HEIGHT; 107 | this->_MAP_PATH = _MAP_PATH; 108 | this->_STEP_DISTANCE = _STEP_DISTANCE; 109 | this->_CAR_SEEK_FOV = _CAR_SEEK_FOV; 110 | 111 | this->myGameHandler = std::make_shared(GameHandler()); 112 | this->decisionMap = cv::imread(this->_MAP_PATH); 113 | cv::resize(this->decisionMap, this->decisionMap, cv::Size(int(this->_REAL_WIDTH / this->_REAL_HEIGHT * 1080.), 1080)); 114 | cv::cvtColor(this->decisionMap, this->decisionMap_Gray, cv::COLOR_BGR2GRAY, 1); 115 | } 116 | 117 | RobotDecisionSys::~RobotDecisionSys() 118 | { 119 | } 120 | 121 | bool RobotDecisionSys::decodeWayPoints(std::string &filePath) 122 | { 123 | Json::Reader jsonReader; 124 | Json::Value jsonValue; 125 | std::ifstream jsonFile(filePath); 126 | if (!jsonReader.parse(jsonFile, jsonValue, true)) 127 | { 128 | std::cout << "read error" << std::endl; 129 | jsonFile.close(); 130 | return false; 131 | } 132 | Json::Value arrayValue = jsonValue["data"]; 133 | std::vector tempwayPointMap; 134 | for (int i = 0; i < int(arrayValue.size()); ++i) 135 | { 136 | std::shared_ptr wayPoint = std::make_shared(WayPoint()); 137 | wayPoint->id = arrayValue[i]["id"].asInt(); 138 | wayPoint->type = arrayValue[i]["type"].asInt(); 139 | wayPoint->x = arrayValue[i]["x"].asFloat(); 140 | wayPoint->y = arrayValue[i]["y"].asFloat(); 141 | wayPoint->theta = arrayValue[i]["angle"].asDouble(); 142 | Json::Value connectedPoints = arrayValue[i]["connect"]; 143 | for (int j = 0; j < int(connectedPoints.size()); ++j) 144 | { 145 | wayPoint->connection.emplace_back(connectedPoints[j].asInt()); 146 | } 147 | Json::Value enemyWeightsArray = arrayValue[i]["enemyWeights"]; 148 | for (int j = 0; j < int(enemyWeightsArray.size()); ++j) 149 | { 150 | wayPoint->enemyWeights[j] = enemyWeightsArray[j].asInt(); 151 | } 152 | this->wayPointMap.emplace_back(wayPoint); 153 | this->connection_map.insert(std::make_pair(wayPoint->id, wayPoint->connection)); 154 | } 155 | jsonFile.close(); 156 | return true; 157 | } 158 | 159 | bool RobotDecisionSys::decodeDecisions(std::string &filePath) 160 | { 161 | Json::Reader jsonReader; 162 | Json::Value jsonValue; 163 | std::ifstream jsonFile(filePath); 164 | if (!jsonReader.parse(jsonFile, jsonValue, true)) 165 | { 166 | std::cout << "read error" << std::endl; 167 | jsonFile.close(); 168 | return false; 169 | } 170 | Json::Value arrayValue = jsonValue["data"]; 171 | for (int i = 0; i < int(arrayValue.size()); ++i) 172 | { 173 | std::shared_ptr decision = std::make_shared(Decision()); 174 | decision->id = arrayValue[i]["id"].asInt(); 175 | decision->name = arrayValue[i]["name"].asCString(); 176 | Json::Value wayPointIDArray = arrayValue[i]["wayPointID"]; 177 | for (int j = 0; j < int(wayPointIDArray.size()); ++j) 178 | { 179 | decision->wayPointID.emplace_back(wayPointIDArray[j].asInt()); 180 | } 181 | decision->weight = arrayValue[i]["weight"].asInt(); 182 | decision->start_time = arrayValue[i]["start_time"].asInt(); 183 | decision->end_time = arrayValue[i]["end_time"].asInt(); 184 | decision->robot_mode = arrayValue[i]["robot_mode"].asInt(); 185 | decision->_minHP = arrayValue[i]["minHP"].asInt(); 186 | decision->_maxHP = arrayValue[i]["maxHP"].asInt(); 187 | decision->decide_mode = arrayValue[i]["decide_mode"].asInt(); 188 | decision->decide_wayPoint = arrayValue[i]["decide_wayPoint"].asInt(); 189 | decision->out_post_HP_min = arrayValue[i]["out_post_HP_min"].asInt(); 190 | decision->out_post_HP_max = arrayValue[i]["out_post_HP_max"].asInt(); 191 | decision->base_HP_min = arrayValue[i]["base_HP_min"].asInt(); 192 | decision->if_succession = arrayValue[i]["if_succession"].asBool(); 193 | decision->if_reverse = arrayValue[i]["if_reverse"].asBool(); 194 | Json::Value enemyPositionArray = arrayValue[i]["enemyPosition"]; 195 | for (int j = 0; j < int(enemyPositionArray.size()); ++j) 196 | { 197 | std::vector temp; 198 | for (int k = 0; k < int(enemyPositionArray[j].size()); ++k) 199 | { 200 | temp.emplace_back(enemyPositionArray[j][k].asInt()); 201 | } 202 | decision->enemy_position.emplace_back(temp); 203 | } 204 | Json::Value friendPositionArray = arrayValue[i]["friendPosition"]; 205 | for (int j = 0; j < int(friendPositionArray.size()); ++j) 206 | { 207 | std::vector temp; 208 | for (int k = 0; k < int(friendPositionArray[j].size()); ++k) 209 | { 210 | temp.emplace_back(friendPositionArray[j][k].asInt()); 211 | } 212 | decision->friend_position.emplace_back(temp); 213 | } 214 | this->decisions.emplace_back(decision); 215 | } 216 | jsonFile.close(); 217 | return true; 218 | } 219 | 220 | int RobotDecisionSys::checkNowWayPoint(float x, float y) 221 | { 222 | RobotPosition pos; 223 | pos.x = x; 224 | pos.y = y; 225 | return this->calculatePosition(pos); 226 | } 227 | 228 | int RobotDecisionSys::checkNowWayPoint(RobotPosition pos) 229 | { 230 | return this->calculatePosition(pos); 231 | } 232 | 233 | std::shared_ptr RobotDecisionSys::decide(int wayPointID, int robot_mode, int _HP, int nowtime, int now_out_post_HP, int now_base_HP, std::vector &friendPositions, std::vector &enemyPositions, std::vector &availableDecisionID, std::map &id_pos_f, std::map &id_pos_e) 234 | { 235 | this->myGameHandler->update(nowtime); 236 | std::vector> tempDecision; 237 | for (auto &it : friendPositions) 238 | { 239 | id_pos_f.insert(std::make_pair(it.robot_id, this->calculatePosition(it))); 240 | } 241 | for (auto &it : enemyPositions) 242 | { 243 | id_pos_e.insert(std::make_pair(it.robot_id, this->calculatePosition(it))); 244 | } 245 | for (auto it : this->decisions) 246 | { 247 | 248 | if (it->wayPointID[0] != -1) 249 | { 250 | bool check = false; 251 | for (auto &jt : it->wayPointID) 252 | { 253 | check = jt == wayPointID; 254 | if (check) 255 | break; 256 | } 257 | if (!check) 258 | continue; 259 | } 260 | if ((it->robot_mode != -1 && it->robot_mode != robot_mode) || 261 | (it->_maxHP != -1 && _HP > it->_maxHP) || 262 | (it->_minHP != -1 && _HP <= it->_minHP) || 263 | (it->end_time != -1 && nowtime > it->end_time) || 264 | (it->start_time != -1 && nowtime <= it->start_time) || 265 | (it->out_post_HP_min != -1 && now_out_post_HP <= it->out_post_HP_min) || 266 | (it->out_post_HP_max != -1 && now_out_post_HP > it->out_post_HP_max) || 267 | (it->base_HP_min != -1 && now_base_HP < it->base_HP_min)) 268 | { 269 | continue; 270 | } 271 | bool fpFLAG = true; 272 | for (int i = 0; i < int(it->friend_position.size()); ++i) 273 | { 274 | int temp_pos = id_pos_f.find(i)->second; 275 | if (it->friend_position[i][0] == -1) 276 | { 277 | continue; 278 | } 279 | if (std::find(it->friend_position[i].begin(), it->friend_position[i].end(), temp_pos) == it->friend_position[i].end()) 280 | { 281 | fpFLAG = false; 282 | break; 283 | } 284 | } 285 | bool epFLAG = true; 286 | for (int i = 0; i < int(it->enemy_position.size()); ++i) 287 | { 288 | int temp_pos = id_pos_e.find(i)->second; 289 | if (it->enemy_position[i][0] == -1) 290 | { 291 | continue; 292 | } 293 | if (std::find(it->enemy_position[i].begin(), it->enemy_position[i].end(), temp_pos) == it->enemy_position[i].end()) 294 | { 295 | fpFLAG = false; 296 | break; 297 | } 298 | } 299 | if (epFLAG && fpFLAG) 300 | tempDecision.emplace_back(it); 301 | } 302 | int max_weight = 0; 303 | std::shared_ptr decision = nullptr; 304 | for (auto it : tempDecision) 305 | { 306 | if (it->weight > max_weight) 307 | { 308 | max_weight = it->weight; 309 | decision = it; 310 | decision->if_auto = false; 311 | } 312 | availableDecisionID.emplace_back(it->id); 313 | } 314 | if (decision != nullptr && decision->decide_mode == -1) 315 | decision->decide_mode = decision->robot_mode; 316 | if (decision != nullptr && decision->decide_wayPoint == -1) 317 | decision->decide_wayPoint = wayPointID; 318 | return decision; 319 | } 320 | 321 | std::shared_ptr RobotDecisionSys::getWayPointByID(int id) 322 | { 323 | for (auto &it : this->wayPointMap) 324 | { 325 | if (it->id == id) 326 | { 327 | return it; 328 | } 329 | } 330 | return nullptr; 331 | } 332 | 333 | std::shared_ptr RobotDecisionSys::getDecisionByID(int id) 334 | { 335 | for (auto &it : this->decisions) 336 | { 337 | if (it->id == id) 338 | { 339 | return it; 340 | } 341 | } 342 | return nullptr; 343 | } 344 | 345 | std::map RobotDecisionSys::decideAimTarget(bool _IsBlue, RobotPosition &mypos, std::vector &enemyPositions, std::vector &enemyHP, float distance_weight_ratio, float hp_weight_ratio, bool enemyOutpostDown) 346 | { 347 | std::map result; 348 | float max_distance = 0.0000001; 349 | std::map distances; 350 | std::map distance_weight; 351 | for (auto &it : enemyPositions) 352 | { 353 | float tempDistance = sqrtf(powf(double(it.x - mypos.x), 2) + powf(double(it.y - mypos.y), 2)); 354 | distances[it.robot_id] = tempDistance; 355 | if (tempDistance > max_distance) 356 | max_distance = tempDistance; 357 | } 358 | std::map::iterator iter_distance_weight; 359 | for (iter_distance_weight = distances.begin(); iter_distance_weight != distances.end(); iter_distance_weight++) 360 | { 361 | distance_weight[iter_distance_weight->first] = (1. - (iter_distance_weight->second / max_distance)) * distance_weight_ratio; 362 | } 363 | float max_hp = 0.0000001; 364 | std::map hps; 365 | std::map hp_weight; 366 | for (int i = 0; i < int(enemyHP.size()); ++i) 367 | { 368 | hps[!_IsBlue * CARPOS_NUM + i] = float(enemyHP[i]); 369 | if (enemyHP[i] > max_hp) 370 | max_hp = float(enemyHP[i]); 371 | } 372 | std::map::iterator iter_hp_weight; 373 | for (iter_hp_weight = hps.begin(); iter_hp_weight != hps.end(); iter_hp_weight++) 374 | { 375 | hp_weight[iter_hp_weight->first] = (1. - (iter_hp_weight->second / max_hp)) * hp_weight_ratio; 376 | } 377 | for (int i = 0; i < CARPOS_NUM; ++i) 378 | { 379 | result[i] = (hp_weight.find(!_IsBlue * CARPOS_NUM + i) != hp_weight.end() ? hp_weight.find(!_IsBlue * CARPOS_NUM + i)->second : -0.5) + (distance_weight.find(!_IsBlue * CARPOS_NUM + i) != distance_weight.end() ? distance_weight.find(!_IsBlue * CARPOS_NUM + i)->second : -0.5) + 0.000001; 380 | } 381 | // if (!enemyOutpostDown) 382 | result[5] = 0; 383 | return result; 384 | } 385 | 386 | double RobotDecisionSys::decideAngleByEnemyPos(float _x, float _y, std::vector &enemyPositions) 387 | { 388 | int index = -1; 389 | float min_distance = MAXFLOAT; 390 | for (int i = 0; i < int(enemyPositions.size()); ++i) 391 | { 392 | // if(i == 5 || i == 10) 393 | // continue; 394 | if (_x == enemyPositions[i].x && _y == enemyPositions[i].y) 395 | continue; 396 | if (enemyPositions[i].x == 0 || enemyPositions[i].y == 0) 397 | continue; 398 | float temp_distance = sqrtf(powf(double(enemyPositions[i].x - _x), 2) + powf(double(enemyPositions[i].y - _y), 2)); 399 | if (temp_distance > this->_seek_THR) 400 | continue; 401 | double temp_angle = this->calculateAngle(_x, _y, enemyPositions[i].x, enemyPositions[i].y); 402 | // if (this->checkBlock(this->transformPoint(_x, _y, this->_REAL_WIDTH, this->_REAL_HEIGHT, int((this->_REAL_WIDTH / this->_REAL_HEIGHT) * 1080), 1080), temp_angle, int(temp_distance * (1080. / this->_REAL_HEIGHT)))) 403 | // continue; 404 | if (temp_distance < min_distance) 405 | { 406 | min_distance = temp_distance; 407 | index = i; 408 | } 409 | 410 | } 411 | return index == -1 ? -1 : this->calculateAngle(_x, _y, enemyPositions[index].x, enemyPositions[index].y); 412 | } 413 | 414 | double RobotDecisionSys::calculateAngle(double x1, double y1, double x2, double y2) 415 | { 416 | if (x1 == x2 && y1 == y2) 417 | return 0; 418 | double angle_temp; 419 | double xx, yy; 420 | xx = x2 - x1; 421 | yy = y2 - y1; 422 | if (xx == 0.0) 423 | angle_temp = CV_PI / 2.0; 424 | else 425 | angle_temp = atan(fabs(yy / xx)); 426 | if ((xx < 0.0) && (yy >= 0.0)) 427 | angle_temp = CV_PI - angle_temp; 428 | else if ((xx < 0.0) && (yy < 0.0)) 429 | angle_temp = CV_PI + angle_temp; 430 | else if ((xx >= 0.0) && (yy < 0.0)) 431 | angle_temp = CV_PI * 2.0 - angle_temp; 432 | return angle_temp; 433 | } 434 | 435 | double RobotDecisionSys::calculateAngle(cv::Point2d p1, cv::Point2d p2) 436 | { 437 | if (p1.x == p2.x && p1.y == p2.y) 438 | return 0; 439 | double angle_temp; 440 | double xx, yy; 441 | xx = p2.x - p1.x; 442 | yy = p2.y - p1.y; 443 | if (xx == 0.0) 444 | angle_temp = CV_PI / 2.0; 445 | else 446 | angle_temp = atan(fabs(yy / xx)); 447 | if ((xx < 0.0) && (yy >= 0.0)) 448 | angle_temp = CV_PI - angle_temp; 449 | else if ((xx < 0.0) && (yy < 0.0)) 450 | angle_temp = CV_PI + angle_temp; 451 | else if ((xx >= 0.0) && (yy < 0.0)) 452 | angle_temp = CV_PI * 2.0 - angle_temp; 453 | return angle_temp; 454 | } 455 | 456 | float RobotDecisionSys::getDistanceTHR() 457 | { 458 | return this->_distance_THR; 459 | } 460 | 461 | void RobotDecisionSys::setDistanceTHR(float thr) 462 | { 463 | this->_distance_THR = thr; 464 | } 465 | 466 | float RobotDecisionSys::getSeekTHR() 467 | { 468 | return this->_seek_THR; 469 | } 470 | 471 | void RobotDecisionSys::setSeekTHR(float thr) 472 | { 473 | this->_seek_THR = thr; 474 | } 475 | 476 | void RobotDecisionSys::UpdateDecisionMap(int &activateDecisionID, std::vector &availableDecisionID, int &nowWayPoint, double yaw, cv::Point2f car_center, double car_orientation, double aim_yaw, std::vector &friendPositions, std::vector &enemyPositions, std::map &id_pos_f, std::map &id_pos_e) 477 | { 478 | cv::Mat dstMap; 479 | if (IfShowUI) 480 | { 481 | if (!IfUIInited) 482 | { 483 | cv::namedWindow("DecisionMapUI", cv::WindowFlags::WINDOW_NORMAL); 484 | this->IfUIInited = true; 485 | } 486 | } 487 | else 488 | { 489 | return; 490 | } 491 | car_orientation = car_orientation == -1 ? yaw : car_orientation; 492 | cv::Point dst_car_center = this->transformPoint(car_center, this->_REAL_WIDTH, this->_REAL_HEIGHT, int((this->_REAL_WIDTH / this->_REAL_HEIGHT) * 1080), 1080); 493 | dstMap = this->decisionMap.clone(); 494 | int activateWayPointID = this->getDecisionByID(activateDecisionID)->decide_wayPoint; 495 | std::vector availableWayPointID; 496 | for (auto &it : availableDecisionID) 497 | { 498 | availableWayPointID.emplace_back(this->getDecisionByID(it)->decide_wayPoint); 499 | } 500 | for (int i = 0; i < int(this->wayPointMap.size()); ++i) 501 | { 502 | int temp_id = this->wayPointMap[i]->id; 503 | cv::Point dst_way_point_center = this->transformPoint(this->wayPointMap[i]->x, this->wayPointMap[i]->y, this->_REAL_WIDTH, this->_REAL_HEIGHT, int((this->_REAL_WIDTH / this->_REAL_HEIGHT) * 1080), 1080); 504 | if (std::find_if(id_pos_f.begin(), id_pos_f.end(), [temp_id](const auto &item) 505 | { return item.second == temp_id; }) != id_pos_f.end()) 506 | { 507 | cv::rectangle(dstMap, cv::Point(dst_way_point_center.x - 28, dst_way_point_center.y - 15), cv::Point(dst_way_point_center.x + 28, dst_way_point_center.y + 15), cv::Scalar(0, 255, 0), -1); 508 | } 509 | if (std::find_if(id_pos_e.begin(), id_pos_e.end(), [temp_id](const auto &item) 510 | { return item.second == temp_id; }) != id_pos_e.end()) 511 | { 512 | cv::rectangle(dstMap, cv::Point(dst_way_point_center.x - 15, dst_way_point_center.y - 28), cv::Point(dst_way_point_center.x + 15, dst_way_point_center.y + 28), cv::Scalar(0, 0, 255), -1); 513 | } 514 | if (temp_id == activateWayPointID) 515 | { 516 | this->drawWayPoint(dstMap, dst_way_point_center, temp_id, 2); 517 | } 518 | else if (temp_id == nowWayPoint) 519 | { 520 | this->drawWayPoint(dstMap, dst_way_point_center, temp_id, 3); 521 | } 522 | else 523 | { 524 | bool check_flag = false; 525 | for (auto &it : availableWayPointID) 526 | { 527 | if (it == temp_id) 528 | { 529 | check_flag = true; 530 | break; 531 | } 532 | } 533 | this->drawWayPoint(dstMap, dst_way_point_center, temp_id, check_flag ? 1 : 0); 534 | } 535 | } 536 | this->drawCar(dstMap, dst_car_center, car_orientation, yaw, aim_yaw); 537 | cv::circle(dstMap, dst_car_center, int(this->_seek_THR / float(this->_REAL_HEIGHT / 1080)), cv::Scalar(255, 0, 0), 1); 538 | for (auto &it : friendPositions) 539 | { 540 | this->drawOthCar(dstMap, this->transformPoint(it.x, it.y, this->_REAL_WIDTH, this->_REAL_HEIGHT, int((this->_REAL_WIDTH / this->_REAL_HEIGHT) * 1080), 1080), it.robot_id, 0); 541 | } 542 | for (auto &it : enemyPositions) 543 | { 544 | this->drawOthCar(dstMap, this->transformPoint(it.x, it.y, this->_REAL_WIDTH, this->_REAL_HEIGHT, int((this->_REAL_WIDTH / this->_REAL_HEIGHT) * 1080), 1080), it.robot_id, 1); 545 | } 546 | cv::imshow("DecisionMapUI", dstMap); 547 | cv::resizeWindow("DecisionMapUI", cv::Size(1280, 720)); 548 | cv::waitKey(1); 549 | } 550 | 551 | void RobotDecisionSys::drawWayPoint(cv::Mat &img, cv::Point2i center, int id, int type) 552 | { 553 | cv::Scalar color; 554 | cv::Scalar color_txt; 555 | switch (type) 556 | { 557 | case 0: 558 | color = cv::Scalar(135, 138, 128); 559 | color_txt = cv::Scalar(255, 248, 248); 560 | break; 561 | case 1: 562 | color = cv::Scalar(230, 224, 176); 563 | color_txt = cv::Scalar(255, 105, 65); 564 | break; 565 | case 2: 566 | color = cv::Scalar(0, 225, 0); 567 | color_txt = cv::Scalar(20, 128, 48); 568 | break; 569 | case 3: 570 | color = cv::Scalar(0, 255, 255); 571 | color_txt = cv::Scalar(18, 153, 255); 572 | break; 573 | 574 | default: 575 | break; 576 | } 577 | cv::circle(img, center, 24, color_txt, -1); 578 | cv::circle(img, center, 22, color, -1); 579 | cv::Size text_size = cv::getTextSize(std::to_string(id), cv::FONT_HERSHEY_SIMPLEX, 1, 2, 0); 580 | cv::putText(img, std::to_string(id), cv::Point2i(center.x - int(text_size.width / 2), center.y + int(text_size.height / 2)), cv::FONT_HERSHEY_SIMPLEX, 1, color_txt, 2); 581 | } 582 | 583 | cv::Point2i RobotDecisionSys::transformPoint(float _x, float _y, float width, float height, int img_cols, int img_rows) 584 | { 585 | return cv::Point2i(int(double(_x / width) * img_cols), int(double(_y / height) * img_rows)); 586 | } 587 | 588 | cv::Point2i RobotDecisionSys::transformPoint(cv::Point2f center, float width, float height, int img_cols, int img_rows) 589 | { 590 | return cv::Point2i(int(double(center.x / width) * img_cols), int(double(center.y / height) * img_rows)); 591 | } 592 | 593 | void RobotDecisionSys::drawCar(cv::Mat &img, cv::Point2i center, double &car_orientation, double &yaw, double &aim_yaw) 594 | { 595 | if (yaw == -1) 596 | { 597 | yaw = car_orientation; 598 | } 599 | cv::Size wh = cv::Size(50, 35); 600 | cv::Point point_L_U = cv::Point(center.x - wh.width / 2, center.y - wh.height / 2); 601 | cv::Point point_R_U = cv::Point(center.x + wh.width / 2, center.y - wh.height / 2); 602 | cv::Point point_R_L = cv::Point(center.x + wh.width / 2, center.y + wh.height / 2); 603 | cv::Point point_L_L = cv::Point(center.x - wh.width / 2, center.y + wh.height / 2); 604 | cv::Point point[4] = {point_L_U, point_R_U, point_R_L, point_L_L}; 605 | cv::Point after_point[4] = {cv::Point(0, 0)}; 606 | for (int i = 0; i < 4; ++i) 607 | { 608 | int x = point[i].x - center.x; 609 | int y = point[i].y - center.y; 610 | after_point[i].x = cvRound(x * cos(-car_orientation) + y * sin(-car_orientation) + center.x); 611 | after_point[i].y = cvRound(-x * sin(-car_orientation) + y * cos(-car_orientation) + center.y); 612 | } 613 | for (int i = 0; i < 3; ++i) 614 | { 615 | cv::line(img, after_point[i], after_point[i + 1], cv::Scalar(0, 255, 0), 3); 616 | if (i == 2) 617 | { 618 | cv::line(img, after_point[i + 1], after_point[0], cv::Scalar(0, 255, 0), 3); 619 | } 620 | } 621 | int seek_radius = int(this->_seek_THR / float(this->_REAL_HEIGHT / 1080)); 622 | cv::Point2i Car_End = this->createEndPointByTheta(center, car_orientation, 35); 623 | cv::Point2i PAN_End = this->createEndPointByTheta(center, yaw, int(seek_radius / 3)); 624 | if (aim_yaw < 0. && aim_yaw != -1.) 625 | { 626 | aim_yaw = 2. * CV_PI - abs(aim_yaw); 627 | } 628 | cv::Point2i AIM_YAW_End = this->createEndPointByTheta(center, aim_yaw, seek_radius); 629 | cv::Point2i Seek_End_s = this->createEndPointByTheta(center, yaw + (this->_CAR_SEEK_FOV / 2.) * CV_PI / 180., seek_radius); 630 | cv::Point2i Seek_End_e = this->createEndPointByTheta(center, yaw - (this->_CAR_SEEK_FOV / 2.) * CV_PI / 180., seek_radius); 631 | cv::line(img, center, Car_End, cv::Scalar(255, 255, 0), 3); 632 | cv::line(img, center, Seek_End_s, cv::Scalar(0, 255, 255), 2); 633 | cv::line(img, center, Seek_End_e, cv::Scalar(0, 255, 255), 2); 634 | if (aim_yaw != -1.) 635 | cv::line(img, center, AIM_YAW_End, cv::Scalar(255, 100, 0), 2); 636 | cv::line(img, center, PAN_End, cv::Scalar(0, 0, 255), 2); 637 | } 638 | 639 | void RobotDecisionSys::drawOthCar(cv::Mat &img, cv::Point2i center, int &id, int type) 640 | { 641 | float w = 35; 642 | cv::Scalar color; 643 | switch (type) 644 | { 645 | case 0: 646 | color = cv::Scalar(0, 255, 0); 647 | break; 648 | case 1: 649 | color = cv::Scalar(0, 0, 255); 650 | break; 651 | 652 | default: 653 | break; 654 | } 655 | cv::Point _t = cv::Point(center.x, center.y - int((w / 2.) / cos(30. * CV_PI / 180.))); 656 | cv::Point _l = cv::Point(center.x - w / 2, center.y + int((w / 2.) * tan(30. * CV_PI / 180.))); 657 | cv::Point _r = cv::Point(center.x + w / 2, center.y + int((w / 2.) * tan(30. * CV_PI / 180.))); 658 | cv::line(img, _t, _l, color, 2); 659 | cv::line(img, _r, _l, color, 2); 660 | cv::line(img, _t, _r, color, 2); 661 | cv::Size text_size = cv::getTextSize(std::to_string(id), cv::FONT_HERSHEY_SIMPLEX, 0.5, 2, 0); 662 | cv::putText(img, std::to_string(id), cv::Point2i(center.x - int(text_size.width / 2), center.y + int(text_size.height / 2)), cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 2); 663 | } 664 | 665 | bool RobotDecisionSys::getIfShowUI() 666 | { 667 | return this->IfShowUI; 668 | } 669 | 670 | void RobotDecisionSys::setIfShowUI(bool ifShowUI) 671 | { 672 | this->IfShowUI = ifShowUI; 673 | } 674 | } -------------------------------------------------------------------------------- /src/robot_decision/src/robot_decision_node.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/robot_decision_node.hpp" 2 | 3 | using namespace std::placeholders; 4 | 5 | namespace rdsys 6 | { 7 | RobotDecisionNode::RobotDecisionNode(const rclcpp::NodeOptions &options) 8 | : rclcpp::Node("robot_decision_node", options) 9 | { 10 | RCLCPP_INFO( 11 | this->get_logger(), 12 | "RobotDecision node..."); 13 | RCLCPP_INFO( 14 | this->get_logger(), 15 | "starting..."); 16 | if (!this->decodeConfig()) 17 | { 18 | RCLCPP_ERROR( 19 | this->get_logger(), 20 | "Failed to get Config!"); 21 | abort(); 22 | } 23 | this->init(); 24 | assert(CARPOS_NUM == this->type_id.size() / 2); 25 | } 26 | 27 | RobotDecisionNode::~RobotDecisionNode() 28 | { 29 | } 30 | 31 | bool RobotDecisionNode::decodeConfig() 32 | { 33 | std::string package_share_directory = ament_index_cpp::get_package_share_directory("robot_decision"); 34 | Json::Reader jsonReader; 35 | Json::Value jsonValue; 36 | std::ifstream jsonFile(package_share_directory + "/" + "JsonFile/config.json"); 37 | if (!jsonReader.parse(jsonFile, jsonValue, true)) 38 | { 39 | std::cout << "read error" << std::endl; 40 | jsonFile.close(); 41 | return false; 42 | } 43 | Json::Value arrayValue = jsonValue["config"]; 44 | this->_Debug = arrayValue["Debug"].asBool(); 45 | this->_WayPointsPath = package_share_directory + "/JsonFile/" + arrayValue["WayPointsPATH"].asCString(); 46 | this->_DecisionsPath = package_share_directory + "/JsonFile/" + arrayValue["DecisionsPATH"].asCString(); 47 | this->_INIT_DISTANCE_THR = arrayValue["INIT_DISTANCE_THR"].asFloat(); 48 | this->_INIT_SEEK_THR = arrayValue["INIT_SEEK_THR"].asFloat(); 49 | this->_INIT_IsBlue = arrayValue["INIT_ISBLUE"].asBool(); 50 | this->_INIT_IFSHOWUI = arrayValue["INIT_IFSHOWUI"].asBool(); 51 | this->_INIT_IFUSEMANUAL = arrayValue["INIT_IFUSEMANUAL"].asBool(); 52 | this->_INIT_SELFINDEX = arrayValue["INIT_SELFINDEX"].asInt(); 53 | this->_INIT_FRIENDOUTPOSTINDEX = arrayValue["INIT_FRIENDOUTPOSTINDEX"].asInt(); 54 | this->_INIT_FRIENDBASEINDEX = arrayValue["INIT_FRIENDBASEINDEX"].asInt(); 55 | this->_GAME_TIME = arrayValue["GAME_TIME"].asInt(); 56 | this->_TIME_THR = arrayValue["TIME_THR"].asInt(); 57 | 58 | this->_MAP_PATH = package_share_directory + "/resources/" + arrayValue["MAP_PATH"].asCString(); 59 | this->_REAL_WIDTH = arrayValue["REAL_WIDTH"].asFloat(); 60 | this->_REAL_HEIGHT = arrayValue["REAL_HEIGHT"].asFloat(); 61 | this->_STEP_DISTANCE = arrayValue["STEP_DISTANCE"].asFloat(); 62 | this->_CAR_SEEK_FOV = arrayValue["CAR_SEEK_FOV"].asFloat(); 63 | return true; 64 | } 65 | 66 | void RobotDecisionNode::init() 67 | { 68 | this->declare_parameter("distance_thr", this->_INIT_DISTANCE_THR); 69 | this->declare_parameter("seek_thr", this->_INIT_SEEK_THR); 70 | this->declare_parameter("IsBlue", this->_INIT_IsBlue); 71 | this->declare_parameter("IfShowUI", this->_INIT_IFSHOWUI); 72 | 73 | this->_selfIndex = this->_INIT_SELFINDEX; 74 | this->_friendOutPostIndex = this->_INIT_FRIENDOUTPOSTINDEX; 75 | this->_friendBaseIndex = this->_INIT_FRIENDBASEINDEX; 76 | 77 | this->myRDS = std::make_shared(RobotDecisionSys(this->_distance_THR_Temp, this->_seek_THR_Temp, this->_REAL_WIDTH, this->_REAL_HEIGHT, this->_MAP_PATH, this->_STEP_DISTANCE, this->_CAR_SEEK_FOV)); 78 | 79 | this->timer_ = this->create_wall_timer(200ms, std::bind(&RobotDecisionNode::respond, this)); 80 | 81 | if (!this->myRDS->decodeWayPoints(this->_WayPointsPath)) 82 | RCLCPP_ERROR( 83 | this->get_logger(), 84 | "Decode waypoints failed"); 85 | if (!this->myRDS->decodeDecisions(this->_DecisionsPath)) 86 | RCLCPP_ERROR( 87 | this->get_logger(), 88 | "Decode decisions failed"); 89 | 90 | rclcpp::QoS qos(0); 91 | qos.keep_last(1); 92 | qos.best_effort(); 93 | qos.durability(); 94 | qos.durability_volatile(); 95 | 96 | this->objHP_sub_.subscribe(this, "/obj_hp", qos.get_rmw_qos_profile()); 97 | this->carPos_sub_.subscribe(this, "/car_pos", qos.get_rmw_qos_profile()); 98 | this->gameInfo_sub_.subscribe(this, "/game_info", qos.get_rmw_qos_profile()); 99 | this->serial_sub_.subscribe(this, "/serial_msg", qos.get_rmw_qos_profile()); 100 | 101 | this->joint_state_sub_ = this->create_subscription("/joint_states", qos, std::bind(&RobotDecisionNode::jointStateCallBack, this, _1)); 102 | this->autoaim_sub_ = this->create_subscription("/armor_detector/armor_msg", qos, std::bind(&RobotDecisionNode::autoaimCallBack, this, _1)); 103 | this->detectionArray_sub_ = this->create_subscription("perception_detector/perception_array", qos, std::bind(&RobotDecisionNode::detectionArrayCallBack, this, _1)); 104 | this->modeSet_sub_ = this->create_subscription("/mode_set", qos, std::bind(&RobotDecisionNode::modeSetCallBack, this, _1)); 105 | 106 | this->TS_sync_.reset(new message_filters::Synchronizer(ApproximateSyncPolicy(10), this->objHP_sub_, this->carPos_sub_, this->gameInfo_sub_, this->serial_sub_)); 107 | this->TS_sync_->registerCallback(std::bind(&RobotDecisionNode::messageCallBack, this, _1, _2, _3, _4)); 108 | 109 | this->nav_through_poses_feedback_sub_ = this->create_subscription( 110 | "navigate_through_poses/_action/feedback", 111 | qos, 112 | std::bind(&RobotDecisionNode::nav2FeedBackCallBack, this, _1)); 113 | this->nav_through_poses_goal_status_sub_ = this->create_subscription( 114 | "navigate_through_poses/_action/status", 115 | qos, 116 | std::bind(&RobotDecisionNode::nav2GoalStatusCallBack, this, _1)); 117 | 118 | this->decision_pub_ = this->create_publisher("robot_decision/decision", qos); 119 | this->strikeLicensing_pub_ = this->create_publisher("robot_decision/strikeLicensing", qos); 120 | RCLCPP_INFO( 121 | this->get_logger(), 122 | "Starting action_client"); 123 | this->nav_through_poses_action_client_ = rclcpp_action::create_client(this, "navigate_through_poses"); 124 | if (!this->nav_through_poses_action_client_->wait_for_action_server(std::chrono::seconds(5))) 125 | { 126 | RCLCPP_ERROR( 127 | this->get_logger(), 128 | "Action server not available after waiting"); 129 | return; 130 | } 131 | else 132 | { 133 | this->nav_through_poses_action_client_->async_cancel_all_goals(); 134 | } 135 | } 136 | 137 | bool RobotDecisionNode::process_once(int &_HP, int &mode, float &_x, float &_y, int &time, int &now_out_post_HP, int &now_base_HP, std::vector &friendPositions, std::vector &enemyPositions, geometry_msgs::msg::TransformStamped::SharedPtr transformStamped) 138 | { 139 | RCLCPP_INFO( 140 | this->get_logger(), 141 | "Heartbeat Processing"); 142 | if (_x == 0.0 || _y == 0.0 || std::isnan(_x) || std::isnan(_y)) 143 | { 144 | if (transformStamped != nullptr) 145 | { 146 | _x = transformStamped->transform.translation.x; 147 | _y = transformStamped->transform.translation.y; 148 | } 149 | else if (this->_transformStamped != nullptr && abs(this->get_clock()->now().seconds() - this->_transformStamped->header.stamp.sec) < this->_TIME_THR) 150 | { 151 | _x = this->_transformStamped->transform.translation.x; 152 | _y = this->_transformStamped->transform.translation.y; 153 | } 154 | else 155 | { 156 | RCLCPP_ERROR( 157 | this->get_logger(), 158 | "Cannot get current Position!"); 159 | return false; 160 | } 161 | } 162 | double aim_yaw = this->myRDS->decideAngleByEnemyPos(_x, _y, enemyPositions); 163 | double roll, pitch, yaw; 164 | tf2::Quaternion nv2_quat; 165 | if (transformStamped != nullptr) 166 | { 167 | tf2::fromMsg(transformStamped->transform.rotation, nv2_quat); 168 | } 169 | else 170 | { 171 | nv2_quat.setRPY(0, 0, 0); 172 | } 173 | tf2::Matrix3x3 m(nv2_quat); 174 | m.getRPY(roll, pitch, yaw); 175 | RCLCPP_INFO( 176 | this->get_logger(), 177 | "Current status: x = %lf , y = %lf , yaw = %lf, HP: %d , MODE: %d", 178 | _x, _y, yaw, _HP, mode); 179 | 180 | if (aim_yaw != -1) 181 | { 182 | this->delta_yaw = double(aim_yaw - yaw); 183 | if (this->delta_yaw > CV_PI) 184 | { 185 | this->delta_yaw = -(2 * CV_PI - this->delta_yaw); 186 | } 187 | 188 | } 189 | else 190 | { 191 | this->delta_yaw = 0.; 192 | RCLCPP_WARN( 193 | this->get_logger(), 194 | "None Aim Yaw"); 195 | } 196 | 197 | int myWayPointID = this->myRDS->checkNowWayPoint(_x, _y); 198 | std::vector availableDecisionID; 199 | std::map id_pos_f, id_pos_e; 200 | std::shared_ptr myDecision = this->myRDS->decide(myWayPointID, mode, _HP, time, now_out_post_HP, now_base_HP, friendPositions, enemyPositions, availableDecisionID, id_pos_f, id_pos_e); 201 | if (myDecision == nullptr) 202 | { 203 | return false; 204 | } 205 | // RCLCPP_WARN( 206 | // this->get_logger(), 207 | // "Aim Yaw: %lf",delta_yaw); 208 | // this->delta_yaw = abs(this->delta_yaw) > 1.2217 && myDecision->decide_mode == 6 ? 0. : this->delta_yaw; 209 | // RCLCPP_WARN( 210 | // this->get_logger(), 211 | // "Aim Yaw AFTER: %lf",delta_yaw); 212 | bool auto_flag = true; 213 | std::unique_lock slk_modeSet(this->myMutex_modeSet); 214 | if (!this->_auto_mode) 215 | { 216 | RCLCPP_DEBUG( 217 | this->get_logger(), 218 | "Not in Auto mode, wait."); 219 | auto_flag = false; 220 | } 221 | slk_modeSet.unlock(); 222 | std::shared_lock slk_2(this->myMutex_status); 223 | if (!auto_flag) 224 | { 225 | this->excuting_decision = myDecision; 226 | this->myRDS->UpdateDecisionMap(myDecision->id, availableDecisionID, myWayPointID, yaw, cv::Point2f(_x, _y), (this->joint_states_msg != nullptr && !isnan(this->joint_states_msg->position[0])) ? yaw + this->joint_states_msg->position[0] : -1, aim_yaw, friendPositions, enemyPositions, id_pos_f, id_pos_e); 227 | return true; 228 | } 229 | else if (this->goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || this->goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) 230 | { 231 | if (this->excuting_decision != nullptr && myDecision->weight > this->excuting_decision->weight) 232 | { 233 | nav_through_poses_action_client_->async_cancel_all_goals(); 234 | RCLCPP_INFO( 235 | this->get_logger(), 236 | "Cancel Previous Goals"); 237 | } 238 | else 239 | { 240 | RCLCPP_INFO( 241 | this->get_logger(), 242 | "Function Normal Continue"); 243 | if (this->excuting_decision == nullptr) 244 | { 245 | RCLCPP_ERROR( 246 | this->get_logger(), 247 | "Failed to get Previous Decision. Try to clean up!"); 248 | nav_through_poses_action_client_->async_cancel_all_goals(); 249 | return false; 250 | } 251 | auto myDecision_msg = this->makeDecisionMsg(this->excuting_decision, this->delta_yaw); 252 | this->decision_pub_->publish(myDecision_msg); 253 | if (this->_IfShowUI) 254 | { 255 | std::shared_lock slk_3(this->myMutex_joint_states); 256 | this->myRDS->UpdateDecisionMap(this->excuting_decision->id, availableDecisionID, myWayPointID, yaw, cv::Point2f(_x, _y), (this->joint_states_msg != nullptr && !isnan(this->joint_states_msg->position[0])) ? yaw + this->joint_states_msg->position[0] : -1, aim_yaw, friendPositions, enemyPositions, id_pos_f, id_pos_e); 257 | slk_3.unlock(); 258 | } 259 | return true; 260 | } 261 | } 262 | this->excuting_decision = myDecision; 263 | slk_2.unlock(); 264 | std::vector> aimWayPoints; 265 | if (!myDecision->if_succession) 266 | { 267 | aimWayPoints.emplace_back(this->myRDS->getWayPointByID(myDecision->decide_wayPoint)); 268 | } 269 | else 270 | { 271 | aimWayPoints = this->myRDS->calculatePath(myWayPointID, myDecision->decide_wayPoint); 272 | } 273 | if (aimWayPoints.empty()) 274 | { 275 | RCLCPP_ERROR( 276 | this->get_logger(), 277 | "Failed to calculatePath"); 278 | return false; 279 | } 280 | this->clearGoals(); 281 | for (auto &it : aimWayPoints) 282 | { 283 | double temp_waypoint_yaw = myDecision->if_reverse ? it->theta + CV_PI : it->theta; 284 | this->makeNewGoal(it->x, it->y, aim_yaw == -1 ? temp_waypoint_yaw : aim_yaw); 285 | } 286 | this->nav_through_poses_goal_.poses = this->acummulated_poses_; 287 | RCLCPP_INFO( 288 | this->get_logger(), 289 | "Sending a path of %zu waypoints:", 290 | this->nav_through_poses_goal_.poses.size()); 291 | for (auto waypoint : this->nav_through_poses_goal_.poses) 292 | { 293 | RCLCPP_INFO( 294 | this->get_logger(), 295 | "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); 296 | } 297 | auto send_goal_options = 298 | rclcpp_action::Client::SendGoalOptions(); 299 | 300 | if (this->nav_through_poses_action_client_->wait_for_action_server(std::chrono::microseconds(100))) 301 | { 302 | auto future_goal_handle = nav_through_poses_action_client_->async_send_goal(nav_through_poses_goal_, send_goal_options); 303 | } 304 | else 305 | { 306 | RCLCPP_WARN( 307 | this->get_logger(), 308 | "Action server still not available !"); 309 | } 310 | global_interface::msg::Decision myDecision_msg = this->makeDecisionMsg(myDecision, this->delta_yaw); 311 | this->decision_pub_->publish(myDecision_msg); 312 | if (this->_IfShowUI) 313 | { 314 | std::shared_lock slk_3(this->myMutex_joint_states); 315 | this->myRDS->UpdateDecisionMap(myDecision->id, availableDecisionID, myWayPointID, yaw, cv::Point2f(_x, _y), (this->joint_states_msg != nullptr && !isnan(this->joint_states_msg->position[0])) ? yaw + this->joint_states_msg->position[0] : -1, aim_yaw, friendPositions, enemyPositions, id_pos_f, id_pos_e); 316 | slk_3.unlock(); 317 | } 318 | return true; 319 | } 320 | 321 | void RobotDecisionNode::makeNewGoal(double x, double y, double theta) 322 | { 323 | auto pose = geometry_msgs::msg::PoseStamped(); 324 | 325 | pose.header.stamp = this->get_clock()->now(); 326 | pose.header.frame_id = "map_decision"; 327 | pose.pose.position.x = x; 328 | pose.pose.position.y = y; 329 | pose.pose.position.z = 0.0; 330 | pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta); 331 | 332 | this->acummulated_poses_.emplace_back(pose); 333 | } 334 | 335 | std::vector RobotDecisionNode::point2f2Position(std::array pos) 336 | { 337 | if (pos.size() != CARPOS_NUM * 2) 338 | { 339 | RCLCPP_ERROR( 340 | this->get_logger(), 341 | "Position msg not valid !"); 342 | return {}; 343 | } 344 | std::vector result; 345 | for (int i = 0; i < int(pos.size()); ++i) 346 | { 347 | result.emplace_back(RobotPosition(i, pos[i].x, pos[i].y)); 348 | } 349 | return result; 350 | } 351 | 352 | void RobotDecisionNode::messageCallBack(const std::shared_ptr &objHP_msg_, 353 | const std::shared_ptr &objPos_msg_, 354 | const std::shared_ptr &gameInfo_msg_, 355 | const std::shared_ptr &serial_msg_) 356 | { 357 | RCLCPP_INFO( 358 | this->get_logger(), 359 | "----------------------------------------------------------------------------------------[ONCE PROCESS]"); 360 | auto start_t = std::chrono::system_clock::now().time_since_epoch(); 361 | geometry_msgs::msg::TransformStamped::SharedPtr transformStamped = nullptr; 362 | for (int i = 0; i < int(objHP_msg_->hp.size()); ++i) 363 | { 364 | if (std::isnan(objHP_msg_->hp[i])) 365 | { 366 | RCLCPP_ERROR( 367 | this->get_logger(), 368 | "Receive ObjHP Msg Error"); 369 | return; 370 | } 371 | RCLCPP_DEBUG( 372 | this->get_logger(), 373 | "Receive ObjHP Msg %d: %d", 374 | i, objHP_msg_->hp[i]); 375 | } 376 | for (int i = 0; i < int(objPos_msg_->pos.size()); ++i) 377 | { 378 | if (std::isnan(objPos_msg_->pos[i].x) || std::isnan(objPos_msg_->pos[i].y)) 379 | { 380 | RCLCPP_ERROR( 381 | this->get_logger(), 382 | "Receive ObjPos Msg Error"); 383 | return; 384 | } 385 | RCLCPP_DEBUG( 386 | this->get_logger(), 387 | "Receive ObjPos Msg %d: x=%lf, y=%lf", 388 | i, objPos_msg_->pos[i].x, objPos_msg_->pos[i].y); 389 | } 390 | if (std::isnan(gameInfo_msg_->timestamp) || std::isnan(gameInfo_msg_->game_stage)) 391 | { 392 | RCLCPP_ERROR( 393 | this->get_logger(), 394 | "Receive GameInfo Msg Error"); 395 | return; 396 | } 397 | RCLCPP_DEBUG( 398 | this->get_logger(), 399 | "Receive GameInfo Msg : timestamp=%d", 400 | gameInfo_msg_->timestamp); 401 | if (std::isnan(serial_msg_->mode) || std::isnan(serial_msg_->theta)) 402 | { 403 | RCLCPP_ERROR( 404 | this->get_logger(), 405 | "Receive Serial Msg Error"); 406 | return; 407 | } 408 | RCLCPP_DEBUG( 409 | this->get_logger(), 410 | "Receive Serial Msg : mode=%d, theta=%lf", 411 | serial_msg_->mode, serial_msg_->theta); 412 | int currentSelfIndex = this->_selfIndex; 413 | int currentFriendOutpostIndex_hp = this->_friendOutPostIndex; 414 | int currentEnemyOutpostIndex_hp = this->_friendOutPostIndex + OBJHP_NUM; 415 | int currentBaseIndex_hp = this->_friendBaseIndex; 416 | int currentSelfIndex_hp = this->_selfIndex; 417 | if (this->_IsBlue) 418 | { 419 | currentSelfIndex_hp = this->_selfIndex + OBJHP_NUM; 420 | currentFriendOutpostIndex_hp = this->_friendOutPostIndex + OBJHP_NUM; 421 | currentEnemyOutpostIndex_hp = this->_friendOutPostIndex; 422 | currentBaseIndex_hp = this->_friendBaseIndex + OBJHP_NUM; 423 | currentSelfIndex = this->_selfIndex + CARPOS_NUM; 424 | } 425 | if (gameInfo_msg_->game_stage != GameStage::IN_BATTLE && !this->_Debug) 426 | { 427 | RCLCPP_WARN_ONCE( 428 | this->get_logger(), 429 | "Wait for game start ..."); 430 | return; 431 | } 432 | this->nav_through_poses_goal_ = nav2_msgs::action::NavigateThroughPoses::Goal(); 433 | int myHP = objHP_msg_->hp[currentSelfIndex_hp]; 434 | float myPos_x_ = objPos_msg_->pos[currentSelfIndex].x; 435 | float myPos_y_ = objPos_msg_->pos[currentSelfIndex].y; 436 | int nowTime = this->_GAME_TIME - gameInfo_msg_->timestamp; 437 | int mode = serial_msg_->mode; 438 | int now_out_post_HP = objHP_msg_->hp[currentFriendOutpostIndex_hp]; 439 | int now_out_post_HP_enemy = objHP_msg_->hp[currentEnemyOutpostIndex_hp]; 440 | bool _if_enemy_outpost_down = now_out_post_HP_enemy == 0; 441 | int now_base_hp = objHP_msg_->hp[currentBaseIndex_hp]; 442 | std::vector _car_hps; 443 | for (auto &it : this->_car_ids) 444 | { 445 | _car_hps.emplace_back(objHP_msg_->hp[it + !this->_IsBlue * OBJHP_NUM]); 446 | } 447 | std::vector allPositions = this->point2f2Position(objPos_msg_->pos); 448 | try 449 | { 450 | transformStamped = std::make_shared(this->tf_buffer_->lookupTransform("map_decision", "base_link", tf2::TimePointZero)); 451 | this->_transformStamped = transformStamped; 452 | std::shared_lock slk(this->myMutex_detectionArray); 453 | if (this->detectionArray_msg != nullptr && this->get_clock()->now().seconds() - this->detectionArray_msg->header.stamp.sec <= this->_TIME_THR) 454 | { 455 | for (auto it : this->detectionArray_msg->detections) 456 | { 457 | tf2::Transform tf2_transform; 458 | tf2::Vector3 p(it.center.position.x, it.center.position.y, it.center.position.z); 459 | tf2::convert(transformStamped->transform, tf2_transform); 460 | p = tf2_transform * p; 461 | allPositions[this->type_id.find(it.type)->second].x = p.getX(); 462 | allPositions[this->type_id.find(it.type)->second].y = p.getY(); 463 | } 464 | } 465 | slk.unlock(); 466 | } 467 | catch (tf2::TransformException &ex) 468 | { 469 | RCLCPP_ERROR( 470 | this->get_logger(), 471 | "Cannot get transform ! TransformException: %s", 472 | ex.what()); 473 | } 474 | std::vector friendPositions; 475 | std::vector enemyPositions; 476 | for (int i = 0; i < int(allPositions.size()); ++i) 477 | { 478 | if (i == currentSelfIndex) 479 | { 480 | continue; 481 | } 482 | if (i < CARPOS_NUM) 483 | { 484 | if (this->_IsBlue) 485 | enemyPositions.emplace_back(allPositions[i]); 486 | else 487 | friendPositions.emplace_back(allPositions[i]); 488 | } 489 | else 490 | { 491 | if (this->_IsBlue) 492 | friendPositions.emplace_back(allPositions[i]); 493 | else 494 | enemyPositions.emplace_back(allPositions[i]); 495 | } 496 | } 497 | RobotPosition myPos; 498 | myPos.robot_id = this->_IsBlue ? this->type_id.find("B7")->second : this->type_id.find("R7")->second; 499 | myPos.x = myPos_x_; 500 | myPos.y = myPos_y_; 501 | std::map target_weights = this->myRDS->decideAimTarget(_IsBlue, myPos, enemyPositions, _car_hps, 0.5, 0.5, _if_enemy_outpost_down); 502 | RCLCPP_INFO( 503 | this->get_logger(), 504 | "Publish StrikeLicensing: "); 505 | for (auto iter_target_weights = target_weights.begin(); iter_target_weights != target_weights.end(); iter_target_weights++) 506 | { 507 | RCLCPP_INFO( 508 | this->get_logger(), 509 | " (id=%d, %lf)", 510 | iter_target_weights->first, iter_target_weights->second); 511 | } 512 | global_interface::msg::StrikeLicensing strikeLicensing_msg; 513 | strikeLicensing_msg.header.stamp = this->get_clock()->now(); 514 | strikeLicensing_msg.header.frame_id = "decision"; 515 | int ids = 0; 516 | for (auto iter_target_weights = target_weights.begin(); iter_target_weights != target_weights.end(); iter_target_weights++) 517 | { 518 | strikeLicensing_msg.weights[ids] = iter_target_weights->second; 519 | ++ids; 520 | } 521 | this->strikeLicensing_pub_->publish(strikeLicensing_msg); 522 | if (!this->process_once(myHP, mode, myPos_x_, myPos_y_, nowTime, now_out_post_HP, now_base_hp, friendPositions, enemyPositions, transformStamped)) 523 | { 524 | RCLCPP_WARN( 525 | this->get_logger(), 526 | "Decision failed!"); 527 | } 528 | auto end_t = std::chrono::system_clock::now().time_since_epoch(); 529 | RCLCPP_DEBUG( 530 | this->get_logger(), 531 | "Take Time: %ld nsec FPS: %d", 532 | end_t.count() - start_t.count(), int(std::chrono::nanoseconds(1000000000).count() / (end_t - start_t).count())); 533 | } 534 | 535 | void RobotDecisionNode::autoaimCallBack(const global_interface::msg::Autoaim::SharedPtr msg) 536 | { 537 | std::unique_lock ulk(this->myMutex_autoaim); 538 | this->autoaim_msg = msg; 539 | ulk.unlock(); 540 | RCLCPP_DEBUG( 541 | this->get_logger(), 542 | "autoaim Recived"); 543 | } 544 | 545 | void RobotDecisionNode::jointStateCallBack(const sensor_msgs::msg::JointState::SharedPtr msg) 546 | { 547 | std::unique_lock ulk(this->myMutex_joint_states); 548 | if (msg->name[0] == "gimbal_yaw_joint" && !std::isnan(msg->position[0])) 549 | { 550 | this->joint_states_msg = msg; 551 | } 552 | ulk.unlock(); 553 | RCLCPP_DEBUG( 554 | this->get_logger(), 555 | "jointState Recived: yaw = %lf , pitch = %lf", 556 | msg->position[0], msg->position[1]); 557 | } 558 | 559 | void RobotDecisionNode::detectionArrayCallBack(const global_interface::msg::DetectionArray::SharedPtr msg) 560 | { 561 | std::unique_lock ulk(this->myMutex_detectionArray); 562 | this->detectionArray_msg = msg; 563 | ulk.unlock(); 564 | RCLCPP_DEBUG( 565 | this->get_logger(), 566 | "Detection Array Recived: %d || %d", 567 | msg->header.stamp.sec, msg->header.stamp.nanosec); 568 | } 569 | 570 | void RobotDecisionNode::modeSetCallBack(const global_interface::msg::ModeSet::SharedPtr msg) 571 | { 572 | if(!_INIT_IFUSEMANUAL) 573 | return; 574 | int mode = msg->mode; 575 | float _x = msg->x; 576 | float _y = msg->y; 577 | RCLCPP_INFO( 578 | this->get_logger(), 579 | "Manual mode: %d x:%lf y:%lf", 580 | mode, _x, _y); 581 | if (mode == 0) 582 | return; 583 | bool check = true; 584 | std::unique_lock ulk(this->myMutex_modeSet); 585 | if (this->modeSet_msg == nullptr) 586 | { 587 | this->modeSet_msg = msg; 588 | check = true; 589 | } 590 | else if (this->modeSet_msg != nullptr && this->modeSet_msg->mode == mode && this->modeSet_msg->x == _x && this->modeSet_msg->y == _y) 591 | check = false; 592 | // return; 593 | else 594 | { 595 | this->modeSet_msg = msg; 596 | check = true; 597 | } 598 | std::shared_lock slk(this->myMutex_autoaim); 599 | slk.unlock(); 600 | global_interface::msg::Decision newDecision_msg; 601 | rclcpp_action::Client::SendGoalOptions send_goal_options; 602 | switch (mode) 603 | { 604 | case 1: 605 | this->nav_through_poses_action_client_->async_cancel_all_goals(); 606 | this->_auto_mode = true; 607 | break; 608 | case 2: 609 | this->nav_through_poses_action_client_->async_cancel_all_goals(); 610 | this->_auto_mode = false; 611 | newDecision_msg = this->makeDecisionMsg(Mode::MANUAL_ATTACK, this->delta_yaw, msg->x, msg->y); 612 | slk.lock(); 613 | if (this->autoaim_msg != nullptr && abs(rclcpp::Clock().now().seconds() - this->autoaim_msg->header.stamp.sec) < this->_TIME_THR && !this->autoaim_msg->is_target_lost) 614 | { 615 | newDecision_msg.set__mode(Mode::AUTOAIM); 616 | } 617 | else 618 | { 619 | newDecision_msg.set__mode(Mode::MANUAL_ATTACK); 620 | } 621 | slk.unlock(); 622 | this->decision_pub_->publish(newDecision_msg); 623 | if (check) 624 | { 625 | this->clearGoals(); 626 | this->makeNewGoal(_x, _y, 0); 627 | this->nav_through_poses_goal_.poses = this->acummulated_poses_; 628 | RCLCPP_INFO( 629 | this->get_logger(), 630 | "Sending a path of %zu waypoints:", 631 | this->nav_through_poses_goal_.poses.size()); 632 | for (auto waypoint : this->nav_through_poses_goal_.poses) 633 | { 634 | RCLCPP_INFO( 635 | this->get_logger(), 636 | "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); 637 | } 638 | send_goal_options = 639 | rclcpp_action::Client::SendGoalOptions(); 640 | // send_goal_options.result_callback = [this, msg](auto) 641 | // { 642 | // global_interface::msg::Decision newDecision_msg = this->makeDecisionMsg(Mode::AUTOAIM, 0, msg->x, msg->y); 643 | // this->decision_pub_->publish(newDecision_msg); 644 | // }; 645 | if (this->nav_through_poses_action_client_->wait_for_action_server(std::chrono::microseconds(100))) 646 | { 647 | auto future_goal_handle = nav_through_poses_action_client_->async_send_goal(nav_through_poses_goal_, send_goal_options); 648 | } 649 | else 650 | { 651 | RCLCPP_WARN( 652 | this->get_logger(), 653 | "Action server still not available !"); 654 | } 655 | } 656 | 657 | break; 658 | case 3: 659 | this->nav_through_poses_action_client_->async_cancel_all_goals(); 660 | this->_auto_mode = false; 661 | newDecision_msg = this->makeDecisionMsg(Mode::MANUAL_BACKDEFENSE, 0., msg->x, msg->y); 662 | this->decision_pub_->publish(newDecision_msg); 663 | if (check) 664 | { 665 | this->clearGoals(); 666 | this->makeNewGoal(_x, _y, 0); 667 | this->nav_through_poses_goal_.poses = this->acummulated_poses_; 668 | RCLCPP_INFO( 669 | this->get_logger(), 670 | "Sending a path of %zu waypoints:", 671 | this->nav_through_poses_goal_.poses.size()); 672 | for (auto waypoint : this->nav_through_poses_goal_.poses) 673 | { 674 | RCLCPP_INFO( 675 | this->get_logger(), 676 | "\t(%lf, %lf)", waypoint.pose.position.x, waypoint.pose.position.y); 677 | } 678 | send_goal_options = 679 | rclcpp_action::Client::SendGoalOptions(); 680 | send_goal_options.result_callback = [this, msg](auto) 681 | { 682 | global_interface::msg::Decision newDecision_msg = this->makeDecisionMsg(Mode::AUTOAIM, this->delta_yaw, msg->x, msg->y); 683 | this->decision_pub_->publish(newDecision_msg); 684 | }; 685 | if (this->nav_through_poses_action_client_->wait_for_action_server(std::chrono::microseconds(100))) 686 | { 687 | auto future_goal_handle = nav_through_poses_action_client_->async_send_goal(nav_through_poses_goal_, send_goal_options); 688 | } 689 | else 690 | { 691 | RCLCPP_WARN( 692 | this->get_logger(), 693 | "Action server still not available !"); 694 | } 695 | } 696 | break; 697 | case 4: 698 | this->nav_through_poses_action_client_->async_cancel_all_goals(); 699 | this->_auto_mode = false; 700 | /* code */ 701 | break; 702 | 703 | default: 704 | break; 705 | } 706 | ulk.unlock(); 707 | } 708 | 709 | void RobotDecisionNode::nav2FeedBackCallBack(const nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage::SharedPtr msg) 710 | { 711 | std::unique_lock ulk(this->myMutex_NTP_FeedBack); 712 | this->current_NTP_FeedBack_msg = msg; 713 | ulk.unlock(); 714 | RCLCPP_DEBUG( 715 | this->get_logger(), 716 | "Receive Nav2FeedBack: Distance Remainimg: %f Current Pose: x=%lf , y=%lf , z=%lf Time Remaining: %d in frame %s", 717 | msg->feedback.distance_remaining, msg->feedback.current_pose.pose.position.x, msg->feedback.current_pose.pose.position.y, msg->feedback.current_pose.pose.position.z, msg->feedback.estimated_time_remaining.sec, msg->feedback.current_pose.header.frame_id.c_str()); 718 | } 719 | 720 | void RobotDecisionNode::nav2GoalStatusCallBack(const action_msgs::msg::GoalStatusArray::SharedPtr msg) 721 | { 722 | std::shared_lock slk(this->myMutex_status); 723 | bool check = (this->goal_status != msg->status_list.back().status); 724 | slk.unlock(); 725 | if (check) 726 | { 727 | std::unique_lock ulk(this->myMutex_status); 728 | this->goal_status = msg->status_list.back().status; 729 | ulk.unlock(); 730 | } 731 | if (msg->status_list.back().status != action_msgs::msg::GoalStatus::STATUS_EXECUTING) 732 | { 733 | RCLCPP_INFO( 734 | this->get_logger(), 735 | "Nav2StatusCallBack Status: %d", 736 | msg->status_list.back().status); 737 | } 738 | } 739 | 740 | void RobotDecisionNode::respond() 741 | { 742 | this->get_parameter("distance_thr", this->_distance_THR_Temp); 743 | this->get_parameter("seek_thr", this->_seek_THR_Temp); 744 | this->get_parameter("IfShowUI", this->_IfShowUI_Temp); 745 | this->get_parameter("IsBlue", this->_IsBlue_Temp); 746 | 747 | if (this->myRDS->getDistanceTHR() != this->_distance_THR_Temp) 748 | { 749 | RCLCPP_INFO( 750 | this->get_logger(), 751 | "set _distance_THR to %f", 752 | this->_distance_THR_Temp); 753 | this->myRDS->setDistanceTHR(this->_distance_THR_Temp); 754 | } 755 | if (this->myRDS->getSeekTHR() != this->_seek_THR_Temp) 756 | { 757 | RCLCPP_INFO( 758 | this->get_logger(), 759 | "set _seek_THR to %f", 760 | this->_seek_THR_Temp); 761 | this->myRDS->setSeekTHR(this->_seek_THR_Temp); 762 | } 763 | if (this->myRDS->getIfShowUI() != this->_IfShowUI_Temp) 764 | { 765 | RCLCPP_INFO( 766 | this->get_logger(), 767 | "set _IfShowUI to %d", 768 | this->_IfShowUI_Temp); 769 | this->myRDS->setIfShowUI(this->_IfShowUI_Temp); 770 | this->_IfShowUI = this->_IfShowUI_Temp; 771 | } 772 | if (this->_IsBlue != this->_IsBlue_Temp) 773 | { 774 | RCLCPP_INFO( 775 | this->get_logger(), 776 | "set _IsBlue to %d", 777 | this->_IsBlue_Temp); 778 | this->_IsBlue = this->_IsBlue_Temp; 779 | } 780 | } 781 | 782 | global_interface::msg::Decision RobotDecisionNode::makeDecisionMsg(std::shared_ptr decision, double &theta) 783 | { 784 | global_interface::msg::Decision myDecision_msg; 785 | myDecision_msg.header.frame_id = "base_link"; 786 | myDecision_msg.header.stamp = this->get_clock()->now(); 787 | myDecision_msg.set__decision_id(decision->id); 788 | std::shared_lock slk(this->myMutex_autoaim); 789 | if (this->autoaim_msg != nullptr && abs(rclcpp::Clock().now().seconds() - this->autoaim_msg->header.stamp.sec) < this->_TIME_THR && decision->decide_mode != Mode::AUTOAIM && !this->autoaim_msg->is_target_lost) 790 | { 791 | myDecision_msg.set__mode(Mode::AUTOAIM); 792 | } 793 | else 794 | { 795 | myDecision_msg.set__mode(decision->decide_mode); 796 | } 797 | slk.unlock(); 798 | std::shared_ptr aimWayPoint = this->myRDS->getWayPointByID(decision->decide_wayPoint); 799 | myDecision_msg.set__x(aimWayPoint->x); 800 | myDecision_msg.set__y(aimWayPoint->y); 801 | myDecision_msg.set__theta(theta); 802 | RCLCPP_INFO( 803 | this->get_logger(), 804 | "Publish Decision : [id] %d [mode] %d [x,y] %lf %lf", 805 | myDecision_msg.decision_id, myDecision_msg.mode, myDecision_msg.x, myDecision_msg.y); 806 | RCLCPP_INFO( 807 | this->get_logger(), 808 | "Publish Aim Yaw : %lf | angle: %lf", 809 | theta, theta * 180. / CV_PI); 810 | return myDecision_msg; 811 | } 812 | 813 | global_interface::msg::Decision RobotDecisionNode::makeDecisionMsg(int mode, double theta, float _x, float _y) 814 | { 815 | global_interface::msg::Decision myDecision_msg; 816 | myDecision_msg.header.frame_id = "base_link"; 817 | myDecision_msg.header.stamp = this->get_clock()->now(); 818 | myDecision_msg.set__decision_id(-1); 819 | myDecision_msg.set__mode(mode); 820 | myDecision_msg.set__x(_x); 821 | myDecision_msg.set__y(_y); 822 | myDecision_msg.set__theta(theta); 823 | RCLCPP_INFO( 824 | this->get_logger(), 825 | "Publish Decision : [id] %d [mode] %d [x,y] %lf %lf", 826 | myDecision_msg.decision_id, myDecision_msg.mode, myDecision_msg.x, myDecision_msg.y); 827 | RCLCPP_INFO( 828 | this->get_logger(), 829 | "Publish Aim Yaw : %lf | angle: %lf", 830 | theta, theta * 180. / CV_PI); 831 | return myDecision_msg; 832 | } 833 | 834 | void RobotDecisionNode::clearGoals() 835 | { 836 | this->acummulated_poses_.clear(); 837 | } 838 | } 839 | 840 | int main(int argc, char *argv[]) 841 | { 842 | rclcpp::init(argc, argv); 843 | auto my_node = std::make_shared(); 844 | rclcpp::spin(my_node); 845 | rclcpp::shutdown(); 846 | return 0; 847 | } 848 | 849 | #include "rclcpp_components/register_node_macro.hpp" 850 | RCLCPP_COMPONENTS_REGISTER_NODE(rdsys::RobotDecisionNode) --------------------------------------------------------------------------------