diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml index a792739f6..19b846b6a 100644 --- a/.github/workflows/build_test.yaml +++ b/.github/workflows/build_test.yaml @@ -15,7 +15,7 @@ jobs: || fromJSON('[ "ubuntu-latest" ]') }} timeout-minutes: 30 - container: ghcr.io/ibis-ssl/crane:prebuilt + container: ghcr.io/ibis-ssl/crane:base env: DEBIAN_FRONTEND: noninteractive strategy: @@ -27,75 +27,73 @@ jobs: run: | git config --global --add safe.directory '*' + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + - uses: actions/checkout@v4 with: - fetch-depth: 0 + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} token: ${{ secrets.GITHUB_TOKEN }} - - name: Copy repository - run: | - mkdir -p ~/ibis_ws/src/crane - cp -rf . ~/ibis_ws/src/crane - shell: bash - - - name: Install dependencies - run: | - cd ~/ibis_ws - vcs import src < src/crane/dependency_${{ matrix.rosdistro }}.repos - - - name: Resolve rosdep - run: | - cd ~/ibis_ws - apt-get update - rosdep update - rosdep install -iy --from-paths src --rosdistro ${{ matrix.rosdistro }} --dependency-types=build --dependency-types=buildtool --dependency-types=test - shell: bash - - name: Setup Problem Matchers for GCC - run: echo "::add-matcher::${{ github.workspace }}/.github/matchers/gcc.json" + run: echo "::add-matcher::.github/matchers/gcc.json" - - name: Restore build artifacts - uses: actions/cache/restore@v4 - with: - path: | - /root/ibis_ws/build - /root/ibis_ws/install - key: build-humble-${{ github.event.pull_request.number }} + - name: Setup Problem Matchers for clang-format + if: steps.get-modified-packages.outputs.modified-packages != '' + run: echo "::add-matcher::.github/matchers/clang-format.json" - name: Get modified packages id: get-modified-packages uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Set git config + uses: autowarefoundation/autoware-github-actions/set-git-config@v1 with: - base-branch: develop + token: ${{ secrets.GITHUB_TOKEN }} - - name: Build packages + - name: Clone dependency packages run: | - source /opt/ros/${{ matrix.rosdistro }}/setup.bash - cd ~/ibis_ws - colcon build --symlink-install --packages-above-and-dependencies ${{ steps.get-modified-packages.outputs.modified-packages }} --cmake-args '-DCMAKE_BUILD_TYPE=Release' + mkdir -p dependency_ws + vcs import dependency_ws < dependency_${{ matrix.rosdistro }}.repos shell: bash - - name: Save build artifacts - uses: actions/cache/save@v4 - with: - path: | - /root/ibis_ws/build - /root/ibis_ws/install - key: build-humble-${{ github.event.pull_request.number }}-${{ github.sha }} - - - name: Colcon test + - name: Run rosdep install run: | - source /opt/ros/${{ matrix.rosdistro }}/setup.bash - cd ~/ibis_ws - colcon test + package_paths=$(colcon list -p --packages-above-and-dependencies ${{ steps.get-modified-packages.outputs.modified-packages }} --base-paths . dependency_ws) + sudo apt-get -yqq update + rosdep update + DEBIAN_FRONTEND=noninteractive rosdep install -yqq --from-paths ${package_paths} --ignore-src --rosdistro ${{ matrix.rosdistro }} shell: bash - - name: Setup Problem Matchers for clang-format - run: echo "::add-matcher::${{ github.workspace }}/.github/matchers/clang-format.json" + - name: Set up colcon-mixin + run: | + colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml || true + colcon mixin update default + shell: bash - - name: Show test result + - name: Build run: | - source /opt/ros/${{ matrix.rosdistro }}/setup.bash - cd ~/ibis_ws - colcon test-result --verbose + . /opt/ros/${{ matrix.rosdistro }}/setup.sh + colcon build --event-handlers console_cohesion+ \ + --packages-above-and-dependencies ${{ steps.get-modified-packages.outputs.modified-packages }} \ + --cmake-args -DCMAKE_BUILD_TYPE=Release \ + --mixin coverage-gcc coverage-pytest compile-commands shell: bash + + - name: Cache build artifacts + uses: actions/cache@v4 + with: + path: | + ./build + ./install + key: build-${{ matrix.rosdistro }}-${{ runner.os }}-${{ runner.arch }}-${{ github.sha }} + + - name: Test + id: test + if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 + with: + rosdistro: ${{ matrix.rosdistro }} + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + build-depends-repos: dependency_${{ matrix.rosdistro }}.repos diff --git a/.github/workflows/custom_dict.json b/.github/workflows/custom_dict.json index 7eec110d6..2df6ef022 100644 --- a/.github/workflows/custom_dict.json +++ b/.github/workflows/custom_dict.json @@ -79,6 +79,9 @@ "ZJUNlict", "luhbots", "nAMeC", + "TRAPS", + "Shinobi", + "reRo", "roboticserlangen", "tigersmannheim", "simulatorcli", @@ -105,6 +108,7 @@ "MPPI", "Kwargs", "voicevox", - "FREEKICK" + "FREEKICK", + "rcst" ] } diff --git a/.github/workflows/docker_build.yaml b/.github/workflows/docker_build.yaml index 4bc009e54..be25795d7 100644 --- a/.github/workflows/docker_build.yaml +++ b/.github/workflows/docker_build.yaml @@ -5,7 +5,6 @@ on: branches: - develop workflow_dispatch: - jobs: build_base: name: build base image @@ -16,6 +15,9 @@ jobs: fetch-depth: 0 token: ${{ secrets.GITHUB_TOKEN }} + - name: Set up buildx + uses: docker/setup-buildx-action@v3 + - name: Login to GitHub Container Registry uses: docker/login-action@v3 with: @@ -25,12 +27,14 @@ jobs: - name: Build image if: steps.check_image.outputs.GITHUB_OUTPUT == ${{ steps.extract_dependencies.outputs.rosdep_checksum }} - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v5 with: context: . file: docker/base/Dockerfile push: true tags: ghcr.io/${{ github.repository }}:base + cache-from: type=gha + cache-to: type=gha,mode=max build_ccache: name: build ccache image @@ -41,6 +45,9 @@ jobs: fetch-depth: 0 token: ${{ secrets.GITHUB_TOKEN }} + - name: Set up buildx + uses: docker/setup-buildx-action@v3 + - name: Login to GitHub Container Registry uses: docker/login-action@v3 with: @@ -50,12 +57,14 @@ jobs: - name: Build image if: steps.check_image.outputs.GITHUB_OUTPUT == ${{ steps.extract_dependencies.outputs.rosdep_checksum }} - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v5 with: context: . file: docker/ccache/Dockerfile push: true tags: ghcr.io/${{ github.repository }}:ccache + cache-from: type=gha + cache-to: type=gha,mode=max build_prebuilt: name: build prebuilt image @@ -66,6 +75,9 @@ jobs: fetch-depth: 0 token: ${{ secrets.GITHUB_TOKEN }} + - name: Set up buildx + uses: docker/setup-buildx-action@v3 + - name: Login to GitHub Container Registry uses: docker/login-action@v3 with: @@ -75,9 +87,11 @@ jobs: - name: Build image if: steps.check_image.outputs.GITHUB_OUTPUT == ${{ steps.extract_dependencies.outputs.rosdep_checksum }} - uses: docker/build-push-action@v2 + uses: docker/build-push-action@v5 with: context: . file: docker/prebuilt/Dockerfile push: true tags: ghcr.io/${{ github.repository }}:prebuilt + cache-from: type=gha + cache-to: type=gha,mode=max diff --git a/.github/workflows/scenario_test.yaml b/.github/workflows/scenario_test.yaml new file mode 100644 index 000000000..0271361b8 --- /dev/null +++ b/.github/workflows/scenario_test.yaml @@ -0,0 +1,185 @@ +name: scenario test + +on: + workflow_dispatch: + pull_request: + merge_group: + +env: + PYTHON_VERSION: '3.10' + +jobs: + create_scenario_test_image: + name: build image for scenario test + runs-on: >- + ${{ + (contains(github.event.pull_request.labels.*.name, 'SelfHostedRunner')) + && fromJSON('[ "self-hosted"]') + || fromJSON('[ "ubuntu-latest" ]') + }} + env: + DEBIAN_FRONTEND: noninteractive + strategy: + fail-fast: false + matrix: + rosdistro: [humble] + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + token: ${{ secrets.GITHUB_TOKEN }} + + - name: Set up buildx + uses: docker/setup-buildx-action@v3 + + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Build image + uses: docker/build-push-action@v5 + with: + context: . + file: docker/scenario/Dockerfile + push: true + tags: ghcr.io/${{ github.repository }}:scenario-${{ github.sha }} + cache-from: type=gha + + setup_scenario_test_library: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + token: ${{ secrets.GITHUB_TOKEN }} + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: ${{ env.PYTHON_VERSION }} + + - name: Setup virtual environment + run: | + python -m venv env + source env/bin/activate + + - name: Cache virtual environment + uses: actions/cache@v4 + with: + path: env + key: ${{ runner.os }}-env-${{ github.run_id }} + restore-keys: | + ${{ runner.os }}-env- + + - name: Install robocup_scenario_test library + run: | + source env/bin/activate + python -m pip install --upgrade pip + sudo apt update + sudo apt install -y protobuf-compiler + pip install -v git+https://github.com/SSL-Roots/robocup_scenario_test + pip install pytest + + scenario_test: + needs: [create_scenario_test_image, setup_scenario_test_library] + strategy: + fail-fast: false + matrix: + env: + - TEST: STOP_AVOID_BALL + - TEST: STOP_ROBOT_SPEED + - TEST: emit_from_penalty_01 + + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + token: ${{ secrets.GITHUB_TOKEN }} + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: ${{ env.PYTHON_VERSION }} + + - name: Cache virtual environment + uses: actions/cache@v4 + with: + path: env + key: ${{ runner.os }}-env-${{ github.run_id }} + restore-keys: | + ${{ runner.os }}-env- + + - name: Setup virtual environment + run: | + python -m venv env + source env/bin/activate + python -m pip list + + - name: Start Docker Compose services + run: | + docker compose -f docker/scenario/docker-compose.yaml up -d + env: + CRANE_TAG: scenario-${{ github.sha }} + + - name: Wait for crane to start + timeout-minutes: 5 + run: | + sleep 5 + + - name: Download logger + run: | + curl -L https://github.com/RoboCup-SSL/ssl-go-tools/releases/download/v1.5.2/ssl-log-recorder_v1.5.2_linux_amd64 -o ssl-log-recorder + chmod +x ssl-log-recorder + + - name: Run pytest + id: pytest + run: | + source env/bin/activate + pytest scenario_test/${{ matrix.env.TEST }}.py --vision_port=10020 --logging --log_recorder=./ssl-log-recorder + echo "LOG_EXISTS=$(ls *.log.gz 2> /dev/null)" >> $GITHUB_ENV + + - name: Get and print logs + if: always() + run: docker compose -f docker/scenario/docker-compose.yaml logs + + - name: Clean up Docker Compose services + if: always() + run: docker compose -f docker/scenario/docker-compose.yaml down + + - name: setup ssl-log-video + if: ${{ failure() }} + run: | + export PATH=$PATH:/usr/local/go/bin && \ + git clone https://github.com/ibis-ssl/ssl-go-tools.git -b video && \ + cd ssl-go-tools/cmd/ssl-log-video && \ + go mod tidy && \ + go build -o ssl-log-video ssl-log-video.go && \ + chmod +x ssl-log-video + + - name: make log video + if: ${{ failure() }} + run: | + export PATH=$PATH:/usr/local/go/bin + sudo apt update + sudo apt install -y ffmpeg + find . -name "*.log.gz" -exec sh -c 'gunzip -c "$1" > "${1%.gz}" && ./ssl-go-tools/cmd/ssl-log-video/ssl-log-video -file "${1%.gz}" -output "${1%.gz}.avi" && ffmpeg -i "${1%.gz}.avi" -vcodec libx264 -acodec aac ${1%.gz}.mp4' _ {} \; + + - name: Upload artifacts + id: upload-artifact + if: ${{ failure() }} + uses: actions/upload-artifact@v4 + with: + name: ${{ matrix.env.TEST}}-failure-log-video + path: '*.mp4' + + - name: Comment PR with Artifact URL + if: ${{ failure() }} + uses: peter-evans/create-or-update-comment@v4 + with: + issue-number: ${{ github.event.pull_request.number }} + body: | + Failure logs: ${{ steps.upload-artifact.outputs.artifact-url }} diff --git a/3rdparty/closest_point_vendor/CMakeLists.txt b/3rdparty/closest_point_vendor/CMakeLists.txt index ccf93aecd..d817d9a8f 100755 --- a/3rdparty/closest_point_vendor/CMakeLists.txt +++ b/3rdparty/closest_point_vendor/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/3rdparty/matplotlib_cpp_17_vendor/CMakeLists.txt b/3rdparty/matplotlib_cpp_17_vendor/CMakeLists.txt index e36b08c48..69e23374b 100755 --- a/3rdparty/matplotlib_cpp_17_vendor/CMakeLists.txt +++ b/3rdparty/matplotlib_cpp_17_vendor/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/3rdparty/rvo2_vendor/CMakeLists.txt b/3rdparty/rvo2_vendor/CMakeLists.txt index 503b40620..4f3db559a 100755 --- a/3rdparty/rvo2_vendor/CMakeLists.txt +++ b/3rdparty/rvo2_vendor/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/consai_ros2/robocup_ssl_comm/CMakeLists.txt b/consai_ros2/robocup_ssl_comm/CMakeLists.txt index 70deeeba7..ad31d5d88 100644 --- a/consai_ros2/robocup_ssl_comm/CMakeLists.txt +++ b/consai_ros2/robocup_ssl_comm/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/consai_ros2/robocup_ssl_msgs/CMakeLists.txt b/consai_ros2/robocup_ssl_msgs/CMakeLists.txt index 7d28f3a27..873ae14ad 100644 --- a/consai_ros2/robocup_ssl_msgs/CMakeLists.txt +++ b/consai_ros2/robocup_ssl_msgs/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_bringup/CMakeLists.txt b/crane_bringup/CMakeLists.txt index 826abecdd..c078662d4 100755 --- a/crane_bringup/CMakeLists.txt +++ b/crane_bringup/CMakeLists.txt @@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_bringup/launch/crane.launch.xml b/crane_bringup/launch/crane.launch.xml index d5be34e0c..a44fc6255 100644 --- a/crane_bringup/launch/crane.launch.xml +++ b/crane_bringup/launch/crane.launch.xml @@ -7,8 +7,11 @@ + + + @@ -32,12 +35,6 @@ - - - - - - @@ -53,13 +50,25 @@ + + + + + + + + + + + + + - - + + - - - + + @@ -93,14 +102,18 @@ - + + + - + - + + + diff --git a/crane_bringup/launch/joystick_with_vision.launch.py b/crane_bringup/launch/joystick_with_vision.launch.py deleted file mode 100755 index 07ee4faf2..000000000 --- a/crane_bringup/launch/joystick_with_vision.launch.py +++ /dev/null @@ -1,167 +0,0 @@ -# Copyright (c) 2019 SSL-Roots -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. - -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - - -def generate_launch_description(): - # parameter - dev = LaunchConfiguration("dev") - # sim = LaunchConfiguration('sim') # TODO : 現在未使用 シミュレータの切り替え用 - # TODO : crane_descriptionからのパラメータ読み込み - - declare_dev = DeclareLaunchArgument( - "dev", default_value="/dev/input/js0", description="joystick device file" - ) - - # joy_node = Node( - # package="joy", executable="joy_node", output="screen", parameters=[{"dev": dev}] - # ) - # - # teleop_node = Node( - # package="crane_teleop", executable="teleop_node", output="screen" - # ) - - # sim_sender = Node( - # package="crane_sender", - # executable="sim_sender_node", - # output="screen", - # parameters=[ - # os.path.join(get_package_share_directory("crane_sender"), "config", "grsim.yaml") - # ], - # ) - - # grsim = Node(package="robocup_ssl_comm", executable="grsim_node") - - ibis_sender = Node( - package="crane_sender", - executable="ibis_sender_node", - output="screen", - parameters=[{"sim": True}], - ) - - declare_arg_vision_addr = DeclareLaunchArgument( - "vision_addr", - default_value="224.5.23.2", - description="Set multicast address to connect SSL-Vision.", - ) - - declare_arg_vision_port = DeclareLaunchArgument( - "vision_port", - default_value="10006", - description="Set multicast port to connect SSL-Vision.", - ) - - vision = Node( - package="robocup_ssl_comm", - executable="vision_node", - parameters=[ - { - "multicast_address": LaunchConfiguration("vision_addr"), - "multicast_port": LaunchConfiguration("vision_port"), - } - ], - ) - - vision_tracker = Node(package="consai_vision_tracker", executable="vision_tracker_node") - - world_model_publisher = Node( - package="crane_world_model_publisher", executable="crane_world_model_publisher_node" - ) - - visualizer = Node(package="consai_visualizer", executable="consai_visualizer", output="screen") - - declare_arg_referee_addr = DeclareLaunchArgument( - "referee_addr", - default_value="224.5.23.1", - description="Set multicast address to connect Game Controller.", - ) - - declare_arg_referee_port = DeclareLaunchArgument( - "referee_port", - default_value="11003", - description="Set multicast port to connect Game Controller.", - ) - - game_controller = Node( - package="robocup_ssl_comm", - executable="game_controller_node", - parameters=[ - { - "multicast_address": LaunchConfiguration("referee_addr"), - "multicast_port": LaunchConfiguration("referee_port"), - } - ], - ) - - play_switcher = Node( - package="crane_play_switcher", executable="play_switcher_node", output="screen" - ) - - waiter = Node(package="crane_planner_plugins", executable="waiter_node") - - defender = Node(package="crane_planner_plugins", executable="defender_node") - - goalie = Node(package="crane_planner_plugins", executable="goalie_node") - - session_controller = Node( - package="crane_session_controller", - executable="crane_session_controller_node", - output="screen", - ) - - # local_planner = Node( - # package="crane_local_planner", - # executable="crane_local_planner_node", - # output="screen", - # ) - - ld = LaunchDescription() - - ld.add_action(declare_dev) - # ld.add_action(joy_node) - # ld.add_action(teleop_node) - ld.add_action(real_sender) - # ld.add_action(grsim) - ld.add_action(declare_arg_vision_addr) - ld.add_action(declare_arg_vision_port) - ld.add_action(vision) - ld.add_action(vision_tracker) - ld.add_action(world_model_publisher) - ld.add_action(visualizer) - - ld.add_action(declare_arg_referee_addr) - ld.add_action(declare_arg_referee_port) - ld.add_action(game_controller) - - ld.add_action(play_switcher) - ld.add_action(session_controller) - # ld.add_action(local_planner) - ld.add_action(waiter) - ld.add_action(defender) - ld.add_action(goalie) - - return ld diff --git a/crane_bringup/launch/simple_ai.launch.xml b/crane_bringup/launch/simple_ai.launch.xml deleted file mode 100644 index e7115798d..000000000 --- a/crane_bringup/launch/simple_ai.launch.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/crane_description/CMakeLists.txt b/crane_description/CMakeLists.txt index 0faf72c0b..0175647b0 100755 --- a/crane_description/CMakeLists.txt +++ b/crane_description/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_description) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_game_analyzer/CMakeLists.txt b/crane_game_analyzer/CMakeLists.txt index 2312740e2..3c6bf6cfd 100755 --- a/crane_game_analyzer/CMakeLists.txt +++ b/crane_game_analyzer/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_game_analyzer) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_game_analyzer/src/crane_game_analyzer.cpp b/crane_game_analyzer/src/crane_game_analyzer.cpp index cfe8fe0e2..c540e2bcb 100644 --- a/crane_game_analyzer/src/crane_game_analyzer.cpp +++ b/crane_game_analyzer/src/crane_game_analyzer.cpp @@ -22,7 +22,6 @@ GameAnalyzerComponent::GameAnalyzerComponent(const rclcpp::NodeOptions & options world_model->addCallback([&]() { crane_msgs::msg::GameAnalysis game_analysis_msg; - bool is_ball_idle = getBallIdle(); updateBallPossession(game_analysis_msg.ball); auto robot_collision_info = getRobotCollisionInfo(); diff --git a/crane_game_analyzer/src/evaluations.cpp b/crane_game_analyzer/src/evaluations.cpp index dc8121cc1..1cdb7164c 100644 --- a/crane_game_analyzer/src/evaluations.cpp +++ b/crane_game_analyzer/src/evaluations.cpp @@ -37,7 +37,6 @@ double getAngleScore( RobotIdentifier id, Point p, Point next_target, WorldModelWrapper::SharedPtr world_model) { // 入射角＋反射角のcosを計算(内積を使用) - auto & pos = world_model->getRobot(id)->pose.pos; auto current_pass_line = (world_model->ball.pos - p).normalized(); auto next_pass_line = (next_target - p).normalized(); float dot = current_pass_line.dot(next_pass_line); diff --git a/crane_local_planner/CMakeLists.txt b/crane_local_planner/CMakeLists.txt index fe590cc7b..380a07c5a 100755 --- a/crane_local_planner/CMakeLists.txt +++ b/crane_local_planner/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_local_planner) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_local_planner/src/gridmap_planner.cpp b/crane_local_planner/src/gridmap_planner.cpp index cf7a50dab..aec30c8b5 100644 --- a/crane_local_planner/src/gridmap_planner.cpp +++ b/crane_local_planner/src/gridmap_planner.cpp @@ -477,6 +477,12 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( path.push_back(target); } + bool is_escape_mode = [&]() { + grid_map::Index start_index; + map.getIndex(robot->pose.pos, start_index); + return map.at(map_name, start_index) > 0.5; + }(); + // ゴール地点の量子化誤差を除去 if ((path.back() - target).norm() < 0.05) { path.back() = target; @@ -484,20 +490,30 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( // 始点と終点以外の経由点を近い順に削除できるものは取り除く int max_safe_index = [&]() { - for (int i = 1; i < static_cast(path.size()); ++i) { - if (not map.isInside(path[i])) { - return i - 1; - } - auto diff = path[i] - path[0]; - for (int j = 0; j < i; ++j) { - auto intermediate_point = path[0] + diff * ((j + 1.) / (i + 1.)); + if (is_escape_mode) { + // 脱出モードの場合は最初の障害物までを短絡 + for (int i = 1; i < static_cast(path.size()); ++i) { grid_map::Index index; - if (not map.getIndex(intermediate_point, index) or map.at(map_name, index) >= 1.0) { - // 次は障害物なので当然目標速度は0 - command.local_planner_config.terminal_velocity = 0.; - // i番目の経由点は障害物にぶつかるのでi-1番目まではOK + if (map.getIndex(path[i], index) && map.at(map_name, index) < 0.5) { + return i; + } + } + } else { + for (int i = 1; i < static_cast(path.size()); ++i) { + if (not map.isInside(path[i])) { return i - 1; } + auto diff = path[i] - path[0]; + for (int j = 0; j < i; ++j) { + auto intermediate_point = path[0] + diff * ((j + 1.) / (i + 1.)); + grid_map::Index index; + if (not map.getIndex(intermediate_point, index) or map.at(map_name, index) >= 1.0) { + // 次は障害物なので当然目標速度は0 + command.local_planner_config.terminal_velocity = 0.; + // i番目の経由点は障害物にぶつかるのでi-1番目まではOK + return i - 1; + } + } } } return static_cast(path.size()) - 1; @@ -567,7 +583,8 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand( if ( world_model->play_situation.getSituationCommandID() == crane_msgs::msg::PlaySituation::STOP) { - max_vel = std::min(max_vel, 1.5); + // 1.5m/sだとたまに超えるので1.0m/sにしておく + max_vel = std::min(max_vel, 1.0); } // ロボットに衝突しそうなときに速度を抑える diff --git a/crane_msgs/CMakeLists.txt b/crane_msgs/CMakeLists.txt index 8f48e6c47..e506b965d 100755 --- a/crane_msgs/CMakeLists.txt +++ b/crane_msgs/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_msgs) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_play_switcher/CMakeLists.txt b/crane_play_switcher/CMakeLists.txt index 895c6feea..cfd0cc150 100755 --- a/crane_play_switcher/CMakeLists.txt +++ b/crane_play_switcher/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_play_switcher) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_robot_receiver/CMakeLists.txt b/crane_robot_receiver/CMakeLists.txt index 143e98ae2..d08a9c4db 100755 --- a/crane_robot_receiver/CMakeLists.txt +++ b/crane_robot_receiver/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_robot_receiver) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_robot_skills/CMakeLists.txt b/crane_robot_skills/CMakeLists.txt index 309069485..7aef55536 100755 --- a/crane_robot_skills/CMakeLists.txt +++ b/crane_robot_skills/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_robot_skills) -# Default to C++17 +# Default to C++20 if (NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -17,15 +17,7 @@ find_package(rclcpp_components REQUIRED) ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME}_component SHARED - src/robot_skills.cpp - src/robot_command_as_skill.cpp - src/get_ball_contact.cpp - src/go_over_ball.cpp - src/goalie.cpp - src/move_with_ball.cpp - src/receiver.cpp - src/simple_attacker.cpp - src/single_ball_placement.cpp + DIRECTORY src include ) #rclcpp_components_register_nodes(${PROJECT_NAME}_component "crane::WorldModelPublisherComponent") diff --git a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp index 83964ebac..a40f0875c 100644 --- a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp +++ b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp @@ -18,44 +18,10 @@ class GetBallContact : public SkillBase<> public: explicit GetBallContact(uint8_t id, const std::shared_ptr & wm); - void print(std::ostream & out) const override - { - out << "[GetBallContact] "; - auto contact_duration = std::chrono::duration_cast( - robot->ball_contact.getContactDuration()) - .count(); - if (contact_duration > 0) { - out << "contacted: " << contact_duration << "ms"; - } else { - out << "ball distance: " << (robot->pose.pos - world_model->ball.pos).norm(); - } - } + void print(std::ostream & out) const override; private: - Vector2 getApproachNormVec() - { - // if robot is far away from ball, the approach angle is the angle to the ball from robot - // if robot is close to ball, the approach angle is robot angle - // and, the approach angle is interpolated between these two cases - constexpr double FAR_THRESHOLD = 3.5; - constexpr double NEAR_THRESHOLD = 0.5; - - Vector2 far_vec{(robot->pose.pos - world_model->ball.pos).normalized()}; - Vector2 near_vec{cos(robot->pose.theta), sin(robot->pose.theta)}; - - double distance = (robot->pose.pos - world_model->ball.pos).norm(); - - return [&]() { - if (distance > FAR_THRESHOLD) { - return far_vec; - } else if (distance < NEAR_THRESHOLD) { - return near_vec; - } else { - double ratio = (distance - NEAR_THRESHOLD) / (FAR_THRESHOLD - NEAR_THRESHOLD); - return (far_vec * ratio + near_vec * (1.0 - ratio)).normalized(); - } - }(); - } + Vector2 getApproachNormVec(); std::optional last_contact_start_time; diff --git a/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp b/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp index 90f28cfc7..c76779079 100644 --- a/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp +++ b/crane_robot_skills/include/crane_robot_skills/go_over_ball.hpp @@ -20,17 +20,7 @@ class GoOverBall : public SkillBase<> public: explicit GoOverBall(uint8_t id, const std::shared_ptr & wm); - void print(std::ostream & out) const override - { - out << "[GoOverBall] "; - if ( - has_passed_intermediate_target && - (robot->pose.pos - final_target_pos).norm() > getParameter("reach_threshold")) { - out << "中間地点へ向かっています"; - } else { - out << "最終地点へ向かっています, 距離　" << (robot->pose.pos - final_target_pos).norm(); - } - } + void print(std::ostream & out) const override; private: bool has_started = false; diff --git a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp index 2945bce84..651414b61 100644 --- a/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp @@ -21,48 +21,7 @@ enum class KickoffAttackState { class KickoffAttack : public SkillBase { public: - explicit KickoffAttack(uint8_t id, const std::shared_ptr & wm) - : SkillBase("KickoffAttack", id, wm, KickoffAttackState::PREPARE_KICKOFF) - { - setParameter("target_x", 0.0f); - setParameter("target_y", 1.0f); - setParameter("kick_power", 0.3); - addStateFunction( - KickoffAttackState::PREPARE_KICKOFF, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - if (not go_over_ball) { - go_over_ball = std::make_shared(robot->id, world_model); - go_over_ball->setCommander(command); - go_over_ball->setParameter("next_target_x", getParameter("target_x")); - go_over_ball->setParameter("next_target_y", getParameter("target_y")); - go_over_ball->setParameter("margin", 0.3); - command->setMaxVelocity(0.5); - command->disableRuleAreaAvoidance(); - } - go_over_ball_status = go_over_ball->run(visualizer); - return Status::RUNNING; - }); - addTransition( - KickoffAttackState::PREPARE_KICKOFF, KickoffAttackState::KICKOFF, - [this]() -> bool { return go_over_ball_status == Status::SUCCESS; }); - - addStateFunction( - KickoffAttackState::KICKOFF, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - command->setMaxVelocity(0.5); - command->liftUpDribbler(); - command->kickStraight(getParameter("kick_power")); - command->setTargetPosition(world_model->ball.pos); - command->setTerminalVelocity(0.5); - command->disableBallAvoidance(); - command->disableRuleAreaAvoidance(); - if (world_model->ball.vel.norm() > 0.3) { - return Status::SUCCESS; - } else { - return Status::RUNNING; - } - }); - } + explicit KickoffAttack(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { os << "[KickoffAttack]"; } diff --git a/crane_robot_skills/include/crane_robot_skills/marker.hpp b/crane_robot_skills/include/crane_robot_skills/marker.hpp index eda8c9650..cd482587a 100644 --- a/crane_robot_skills/include/crane_robot_skills/marker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/marker.hpp @@ -21,37 +21,7 @@ namespace crane::skills class Marker : public SkillBase<> { public: - explicit Marker(uint8_t id, const std::shared_ptr & wm) - : SkillBase<>("Marker", id, wm, DefaultStates::DEFAULT) - { - setParameter("marking_robot_id", 0); - setParameter("mark_distance", 0.5); - setParameter("mark_mode", std::string("save_goal")); - addStateFunction( - DefaultStates::DEFAULT, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - auto marked_robot = world_model->getTheirRobot(getParameter("marking_robot_id")); - auto enemy_pos = marked_robot->pose.pos; - - std::string mode = getParameter("mark_mode"); - Point marking_point; - double target_theta; - - if (mode == "save_goal") { - marking_point = enemy_pos + (world_model->getOurGoalCenter() - enemy_pos).normalized() * - getParameter("mark_distance"); - target_theta = getAngle(enemy_pos - world_model->getOurGoalCenter()); - } else if (mode == "intercept_pass") { - marking_point = enemy_pos + (world_model->ball.pos - enemy_pos).normalized() * - getParameter("mark_distance"); - target_theta = getAngle(enemy_pos - world_model->ball.pos); - } else { - throw std::runtime_error("unknown mark mode"); - } - command->setTargetPosition(marking_point, target_theta); - return Status::RUNNING; - }); - } + explicit Marker(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { os << "[Marker]"; } }; diff --git a/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp index 99aa53ed7..e7020499d 100644 --- a/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp +++ b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp @@ -38,9 +38,7 @@ class MoveToGeometry : public SkillBase<> Point getTargetPoint() { - ClosestPoint result; - bg::closest_point(geometry, robot->pose.pos, result); - return result.closest_point; + return getClosestPointAndDistance(geometry, robot->pose.pos).closest_point; } void updateGeometry(Geometry geometry) { this->geometry = geometry; } diff --git a/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp b/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp index a987a8b9a..0f2de50a5 100644 --- a/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp +++ b/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp @@ -28,34 +28,7 @@ class MoveWithBall : public SkillBase<> public: explicit MoveWithBall(uint8_t id, const std::shared_ptr & wm); - Point getTargetPoint(const Point & target_pos) - { - // 正しい方向でドリブルできている場合だけ前進 - if ( - getAngleDiff(robot->pose, target_theta) < - getParameter("moving_direction_tolerance")) { - if (robot->ball_contact.findPastContact(getParameter("max_contact_lost_time"))) { - return robot->pose.pos + (target_pos - robot->pose.pos).normalized() * - getParameter("dribble_target_horizon"); - } - } - return robot->pose.pos; - } - - // double getTargetAngle(const Pose2D & target_pose) - // { - // auto distance = world_model->getDistanceFromRobot(robot->getID(), target_pose.pos); - // auto to_target = getAngle(target_pose.pos - robot->pose.pos); - // - // if (distance > 0.2) { - // return to_target; - // } else if (distance > 0.1) { - // double ratio = (distance - 0.1) / (0.2 - 0.1); - // return ratio * to_target + (1.0 - ratio) * target_pose.theta; - // } else { - // return target_pose.theta; - // } - // } + Point getTargetPoint(const Point & target_pos); void setTargetPoint(const Point & target_point) { diff --git a/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp b/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp index 1dc9bee0a..f6a3a6f58 100644 --- a/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp @@ -27,80 +27,7 @@ class PenaltyKick : public SkillBase std::optional start_ball_point = std::nullopt; public: - explicit PenaltyKick(uint8_t id, const std::shared_ptr & wm) - : SkillBase("PenaltyKick", id, wm, PenaltyKickState::PREPARE) - { - setParameter("start_from_kick", false); - setParameter("prepare_margin", 0.6); - addStateFunction( - PenaltyKickState::PREPARE, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - Point target = world_model->ball.pos; - auto margin = getParameter("prepare_margin"); - target.x() += world_model->getOurGoalCenter().x() > 0 ? margin : -margin; - command->setTargetPosition(target); - command->lookAtBall(); - command->disableRuleAreaAvoidance(); - return Status::RUNNING; - }); - - addTransition(PenaltyKickState::PREPARE, PenaltyKickState::KICK, [this]() { - if (getParameter("start_from_kick")) { - return true; - } else { - return world_model->play_situation.getSituationCommandID() == - crane_msgs::msg::PlaySituation::OUR_PENALTY_START; - } - }); - addStateFunction( - PenaltyKickState::KICK, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - if (not start_ball_point) { - start_ball_point = world_model->ball.pos; - } - - auto [best_angle, goal_angle_width] = - world_model->getLargestGoalAngleRangeFromPoint(world_model->ball.pos); - Point best_target = world_model->ball.pos + getNormVec(best_angle) * 0.5; - visualizer->addPoint(best_target.x(), best_target.y(), 1, "red", 1.0, "best_target"); - - // 経由ポイント - Point intermediate_point = - world_model->ball.pos + (world_model->ball.pos - best_target).normalized() * 0.2; - visualizer->addPoint( - intermediate_point.x(), intermediate_point.y(), 1, "red", 1.0, "intermediate_point"); - - double dot = (robot->pose.pos - world_model->ball.pos) - .normalized() - .dot((world_model->ball.pos - best_target).normalized()); - double target_theta = getAngle(best_target - world_model->ball.pos); - // ボールと敵ゴールの延長線上にいない && 角度があってないときは，中間ポイントを経由 - if (dot < 0.9 || std::abs(getAngleDiff(target_theta, robot->pose.theta)) > 0.1) { - command->setTargetPosition(intermediate_point); - command->enableCollisionAvoidance(); - } else { - command->setTargetPosition(world_model->ball.pos); - command->kickStraight(0.3).disableCollisionAvoidance(); - command->enableCollisionAvoidance(); - command->disableBallAvoidance(); - } - - command->setTargetTheta(target_theta); - command->disableRuleAreaAvoidance(); - return Status::RUNNING; - }); - - addTransition(PenaltyKickState::KICK, PenaltyKickState::DONE, [this]() { - return world_model->isDefenseArea(world_model->ball.pos); - }); - - addStateFunction( - PenaltyKickState::DONE, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - command->stopHere(); - return Status::RUNNING; - }); - } + explicit PenaltyKick(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { diff --git a/crane_robot_skills/include/crane_robot_skills/receiver.hpp b/crane_robot_skills/include/crane_robot_skills/receiver.hpp index dd081a331..044230d57 100644 --- a/crane_robot_skills/include/crane_robot_skills/receiver.hpp +++ b/crane_robot_skills/include/crane_robot_skills/receiver.hpp @@ -44,70 +44,15 @@ class Receiver : public SkillBase<> void print(std::ostream & os) const override { os << "[Receiver]"; } - std::vector> getPositionsWithScore(Segment ball_line, Point next_target) - { - auto points = getPoints(ball_line, 0.05); - std::vector> position_with_score; - for (auto point : points) { - double score = getPointScore(point, next_target); - position_with_score.push_back(std::make_pair(score, point)); - } - return position_with_score; - } - - std::vector getPoints(Segment ball_line, double interval) - { - std::vector points; - float ball_line_len = (ball_line.first - ball_line.second).norm(); - auto norm_vec = (ball_line.second - ball_line.first).normalized(); - for (double d = 0.0; d <= ball_line_len; d += interval) { - points.emplace_back(ball_line.first + d * norm_vec); - } - return points; - } + std::vector> getPositionsWithScore(Segment ball_line, Point next_target); - std::vector getPoints(Point center, float unit, int unit_num) - { - std::vector points; - for (float x = center.x() - unit * (unit_num / 2.f); x <= center.x() + unit * (unit_num / 2.f); - x += unit) { - for (float y = center.y() - unit * (unit_num / 2.f); - y <= center.y() + unit * (unit_num / 2.f); y += unit) { - points.emplace_back(Point(x, y)); - } - } - return points; - } + std::vector getPoints(Segment ball_line, double interval); - std::vector getDPPSPoints(Point center, double r_resolution, int theta_div_num) - { - std::vector points; - for (int theta_index = 0; theta_index < theta_div_num; theta_index++) { - double theta = 2.0 * M_PI * theta_index / theta_div_num; - for (double r = r_resolution; r <= 10.0; r += r_resolution) { - points.emplace_back(Point(center.x() + r * cos(theta), center.y() + r * sin(theta))); - } - } - points.erase( - std::remove_if( - points.begin(), points.end(), - [&](const auto & point) { - return (not world_model->isFieldInside(point)) or world_model->isDefenseArea(point); - }), - points.end()); + std::vector getPoints(Point center, float unit, int unit_num); - return points; - } + std::vector getDPPSPoints(Point center, double r_resolution, int theta_div_num); - double getPointScore(Point p, Point next_target) - { - double nearest_dist; - RobotIdentifier receiver{true, static_cast(session_info.receiver_id)}; - return evaluation::getNextTargetVisibleScore(p, next_target, world_model) * - evaluation::getReachScore(receiver, p, nearest_dist, world_model) * - evaluation::getAngleScore(receiver, p, next_target, world_model) * - evaluation::getEnemyDistanceScore(p, world_model); - } + double getPointScore(Point p, Point next_target); }; } // namespace crane::skills #endif // CRANE_ROBOT_SKILLS__RECEIVER_HPP_ diff --git a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp index b9324be23..64db2ae42 100644 --- a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp +++ b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp @@ -21,40 +21,7 @@ namespace crane::skills class SimpleKickOff : public SkillBase<> { public: - explicit SimpleKickOff(uint8_t id, const std::shared_ptr & wm) - : SkillBase<>("SimpleKickOff", id, wm, DefaultStates::DEFAULT) - { - addStateFunction( - DefaultStates::DEFAULT, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - Point intermediate_point = - world_model->ball.pos + - (world_model->ball.pos - world_model->getTheirGoalCenter()).normalized() * 0.3; - - double dot = - (robot->pose.pos - world_model->ball.pos) - .normalized() - .dot((world_model->ball.pos - world_model->getTheirGoalCenter()).normalized()); - double target_theta = getAngle(world_model->getTheirGoalCenter() - world_model->ball.pos); - // ボールと敵ゴールの延長線上にいない && 角度があってないときは，中間ポイントを経由 - if ( - (dot < 0.95 && (robot->pose.pos - world_model->ball.pos).norm() > 0.1) || - std::abs(getAngleDiff(target_theta, robot->pose.theta)) > 0.2) { - command->setTargetPosition( - world_model->ball.pos + - (world_model->ball.pos - world_model->getTheirGoalCenter()).normalized() * 0.3); - } else { - command->setTargetPosition( - world_model->ball.pos + - (world_model->getTheirGoalCenter() - world_model->ball.pos).normalized() * 0.3); - command->dribble(0.3); - command->kickWithChip(1.0); - command->disableBallAvoidance(); - } - command->setTargetTheta(target_theta); - return Status::RUNNING; - }); - } + explicit SimpleKickOff(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { os << "[SimpleKickOff]"; } }; diff --git a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp index f80aa232a..4c7a9c01b 100644 --- a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp +++ b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp @@ -51,34 +51,7 @@ class SingleBallPlacement : public SkillBase public: explicit SingleBallPlacement(uint8_t id, const std::shared_ptr & wm); - void print(std::ostream & os) const override - { - os << "[SingleBallPlacement]"; - - switch (getCurrentState()) { - case SingleBallPlacementStates::GO_OVER_BALL: - go_over_ball->print(os); - break; - case SingleBallPlacementStates::CONTACT_BALL: - get_ball_contact->print(os); - break; - case SingleBallPlacementStates::MOVE_TO_TARGET: - move_with_ball->print(os); - break; - case SingleBallPlacementStates::PLACE_BALL: - os << " PLACE_BALL"; - break; - case SingleBallPlacementStates::SLEEP: - sleep->print(os); - break; - case SingleBallPlacementStates::LEAVE_BALL: - set_target_position->print(os); - break; - default: - os << " UNKNOWN"; - break; - } - } + void print(std::ostream & os) const override; }; } // namespace crane::skills #endif // CRANE_ROBOT_SKILLS__SINGLE_BALL_PLACEMENT_HPP_ diff --git a/crane_robot_skills/include/crane_robot_skills/sleep.hpp b/crane_robot_skills/include/crane_robot_skills/sleep.hpp index 62034ad16..5c268c3dd 100644 --- a/crane_robot_skills/include/crane_robot_skills/sleep.hpp +++ b/crane_robot_skills/include/crane_robot_skills/sleep.hpp @@ -16,40 +16,14 @@ namespace crane::skills class Sleep : public SkillBase<> { public: - explicit Sleep(uint8_t id, const std::shared_ptr & wm) - : SkillBase<>("Sleep", id, wm, DefaultStates::DEFAULT) - { - setParameter("duration", 0.0); - addStateFunction( - DefaultStates::DEFAULT, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - if (not is_started) { - start_time = std::chrono::steady_clock::now(); - is_started = true; - } + explicit Sleep(uint8_t id, const std::shared_ptr & wm); - auto elapsed_time = - std::chrono::duration(std::chrono::steady_clock::now() - start_time); - if (elapsed_time.count() > getParameter("duration")) { - return Status::SUCCESS; - } else { - return Status::RUNNING; - } - }); - } + void print(std::ostream & os) const override; - void print(std::ostream & os) const override - { - os << "[Sleep] 残り時間: " << getRestTime() << "秒"; - } + double getRestTime() const; - double getRestTime() const - { - auto elapsed_time = - std::chrono::duration(std::chrono::steady_clock::now() - start_time); - return getParameter("duration") - elapsed_time.count(); - } bool is_started = false; + std::chrono::time_point start_time; }; } // namespace crane::skills diff --git a/crane_robot_skills/include/crane_robot_skills/steal_ball.hpp b/crane_robot_skills/include/crane_robot_skills/steal_ball.hpp index 34b7e8e4b..0fee65c19 100644 --- a/crane_robot_skills/include/crane_robot_skills/steal_ball.hpp +++ b/crane_robot_skills/include/crane_robot_skills/steal_ball.hpp @@ -25,187 +25,7 @@ enum class StealBallState { class StealBall : public SkillBase { public: - explicit StealBall(uint8_t id, const std::shared_ptr & wm) - : SkillBase("StealBall", id, wm, StealBallState::MOVE_TO_FRONT) - { - // ボールを奪う方法 - // front: 正面からドリブラーでボールを奪う - // side: 横から押し出すようにボールを奪う - setParameter("steal_method", std::string("side")); - setParameter("kicker_power", 0.4); - addStateFunction( - StealBallState::MOVE_TO_FRONT, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - // ボールの正面に移動 - // 到着判定すると遅くなるので、敵ロボットにボールが隠されていなかったら次に行ってもいいかも - auto theirs = world_model->theirs.getAvailableRobots(); - if (not theirs.empty()) { - auto [ball_holder, distance] = - world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, theirs); - Point target_pos = world_model->ball.pos + getNormVec(ball_holder->pose.theta) * 0.3; - command->setTargetPosition(target_pos); - command->lookAtBallFrom(target_pos); - if ((robot->pose.pos - target_pos).norm() < 0.2) { - skill_state = Status::SUCCESS; - } else { - skill_state = Status::RUNNING; - } - return skill_state; - } else { - return Status::RUNNING; - } - }); - - // 正面に移動したら突っ込んでボールを奪う - addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::STEAL, [this]() { - return skill_state == Status::SUCCESS; - }); - - // 敵よりもボールに近い場合はパス - addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::PASS, [this]() { - auto theirs = world_model->theirs.getAvailableRobots(); - if (not theirs.empty()) { - auto [their_attacker, their_distance] = - world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, theirs); - double our_distance = robot->getDistance(world_model->ball.pos); - return our_distance < their_distance - 0.2; - } else { - return true; - } - }); - - addStateFunction( - StealBallState::STEAL, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - command->disableBallAvoidance(); - command->disableCollisionAvoidance(); - const auto method = getParameter("steal_method"); - if (method == "front") { - command->setDribblerTargetPosition(world_model->ball.pos); - command->dribble(0.5); - } else if (method == "side") { - command->setDribblerTargetPosition(world_model->ball.pos); - if (robot->getDistance(world_model->ball.pos) < (0.085 + 0.000)) { - // ロボット半径より近くに来れば急回転して刈り取れる - command->setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos) + M_PI / 2); - } else { - command->setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos)); - } - } - return Status::RUNNING; - }); - - // 敵よりもボールに近い場合はパス - addTransition(StealBallState::STEAL, StealBallState::PASS, [this]() { - auto theirs = world_model->theirs.getAvailableRobots(); - if (not theirs.empty()) { - auto [their_attacker, their_distance] = - world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, theirs); - double our_distance = robot->getDistance(world_model->ball.pos); - return our_distance < their_distance - 0.2; - } else { - return true; - } - }); - - addStateFunction( - StealBallState::PASS, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - if (attacker_skill == nullptr) { - attacker_skill = std::make_shared(robot->id, world_model); - attacker_skill->setCommander(command); - } - auto ours = world_model->ours.getAvailableRobots(robot->id); - ours.erase( - std::remove_if( - ours.begin(), ours.end(), - [this](auto e) { - return e->getDistance(world_model->getTheirGoalCenter()) > - robot->getDistance(world_model->getTheirGoalCenter()); - }), - ours.end()); - if (not ours.empty()) { - auto [target_bot, distance] = world_model->getNearestRobotsWithDistanceFromPoint( - world_model->getTheirGoalCenter(), ours); - attacker_skill->setParameter("receiver_id", target_bot->id); - } - return attacker_skill->run(visualizer); - }); - - addTransition(StealBallState::PASS, StealBallState::MOVE_TO_FRONT, [this]() { - auto theirs = world_model->theirs.getAvailableRobots(); - if (theirs.empty()) { - return false; - } else { - auto [their_attacker, their_distance] = - world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, theirs); - double our_distance = robot->getDistance(world_model->ball.pos); - return our_distance > their_distance; - } - }); - - addTransition(StealBallState::PASS, StealBallState::INTERCEPT, [this]() { - return world_model->ball.vel.norm() > 0.5; - }); - - addTransition(StealBallState::STEAL, StealBallState::INTERCEPT, [this]() { - return world_model->ball.vel.norm() > 0.5; - }); - - addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::INTERCEPT, [this]() { - return world_model->ball.vel.norm() > 0.5; - }); - - addStateFunction(StealBallState::INTERCEPT, [this](const ConsaiVisualizerWrapper::SharedPtr &) { - Point vel_seg = world_model->ball.vel * 5.0; - if (vel_seg.norm() < 1.0) { - vel_seg = vel_seg.normalized() * 1.0; - } - Segment ball_line{world_model->ball.pos, world_model->ball.pos + vel_seg}; - - Point across_point = [&]() { - const double OFFSET = 0.3; - const double X = world_model->field_size.x() / 2.0 - OFFSET; - const double Y = world_model->field_size.y() / 2.0 - OFFSET; - - Segment seg1{Point(X, Y), Point(X, -Y)}; - Segment seg2{Point(-X, Y), Point(-X, -Y)}; - Segment seg3{Point(Y, X), Point(-Y, X)}; - Segment seg4{Point(Y, -X), Point(-Y, -X)}; - std::vector intersections; - if (bg::intersection(ball_line, seg1, intersections); not intersections.empty()) { - return intersections.front(); - } else if (bg::intersection(ball_line, seg2, intersections); not intersections.empty()) { - return intersections.front(); - } else if (bg::intersection(ball_line, seg3, intersections); not intersections.empty()) { - return intersections.front(); - } else if (bg::intersection(ball_line, seg4, intersections); not intersections.empty()) { - return intersections.front(); - } else { - return ball_line.second; - } - }(); - - // ClosestPoint result; - // bg::closest_point(robot->pose.pos, ball_line, result); - - // ゴールとボールの中間方向を向く - auto [goal_angle, width] = world_model->getLargestGoalAngleRangeFromPoint(across_point); - auto to_goal = getNormVec(goal_angle); - auto to_ball = (world_model->ball.pos - across_point).normalized(); - double intermediate_angle = getAngle(2 * to_goal + to_ball); - command->setTargetTheta(intermediate_angle); - command->liftUpDribbler(); - command->kickStraight(getParameter("kicker_power")); - command->setDribblerTargetPosition(across_point); - - return Status::RUNNING; - }); - - addTransition(StealBallState::INTERCEPT, StealBallState::MOVE_TO_FRONT, [this]() { - return world_model->ball.vel.norm() < 0.3; - }); - } + explicit StealBall(uint8_t id, const std::shared_ptr & wm); void print(std::ostream & os) const override { os << "[StealBall]"; } diff --git a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp index 57e006304..6973dce54 100644 --- a/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp +++ b/crane_robot_skills/include/crane_robot_skills/turn_around_point.hpp @@ -21,65 +21,7 @@ namespace crane::skills class TurnAroundPoint : public SkillBase<> { public: - explicit TurnAroundPoint(uint8_t id, const std::shared_ptr & wm) - : SkillBase<>("TurnAroundPoint", id, wm, DefaultStates::DEFAULT) - { - setParameter("target_x", 0.0); - setParameter("target_y", 0.0); - setParameter("target_angle", 0.0); - // 半径方向の制御ゲイン - setParameter("dr_p_gain", 30.0); - setParameter("max_velocity", 0.5); - setParameter("max_turn_omega", M_PI_4); - addStateFunction( - DefaultStates::DEFAULT, - [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - Point target_point(getParameter("target_x"), getParameter("target_y")); - double target_angle = getParameter("target_angle"); - if (target_distance < 0.0) { - // 初回のみ実行時のロボットとの距離を計算して、円弧の半径とする - target_distance = (robot->pose.pos - target_point).norm(); - current_target_angle = getAngle(robot->pose.pos - target_point); - auto max_velocity = getParameter("max_velocity"); - auto max_turn_omega = getParameter("max_turn_omega"); - if (target_distance * max_turn_omega > max_velocity) { - max_velocity = target_distance * max_turn_omega; - } else { - max_turn_omega = max_velocity / target_distance; - } - } - - if (std::abs(getAngleDiff(getAngle(robot->pose.pos - target_point), target_angle)) < 0.1) { - command->stopHere(); - return Status::SUCCESS; - } else { - // 円弧を描いて移動する - - double current_angle = getAngle(robot->pose.pos - target_point); - double angle_diff = getAngleDiff(target_angle, current_angle); - - // 半径方向の距離のズレ - double dr = (robot->pose.pos - target_point).norm() - target_distance; - std::cout << "dr: " << dr << std::endl; - std::cout << "target_distance: " << target_distance << std::endl; - auto max_velocity = getParameter("max_velocity"); - // 円弧を描くような速度 - Velocity velocity = ((target_point - robot->pose.pos).normalized() * - (dr * getParameter("dr_p_gain"))) + - getNormVec(current_angle + std::copysign(M_PI_2, angle_diff)) * - std::min(max_velocity, std::abs(angle_diff * 0.6)); - command->setVelocity(velocity); - - // current_target_angle += std::copysign(max_turn_omega / 30.0f, angle_diff); - // command->setTargetPosition( - // target_point + getNormVec(current_target_angle) * target_distance); - - // 中心点の方を向く - command->setTargetTheta(normalizeAngle(current_angle + M_PI)); - return Status::RUNNING; - } - }); - } + explicit TurnAroundPoint(uint8_t id, const std::shared_ptr & wm); void setTargetPoint(const Point & target_point) { diff --git a/crane_robot_skills/src/get_ball_contact.cpp b/crane_robot_skills/src/get_ball_contact.cpp index d8f6c45f6..4622170a6 100644 --- a/crane_robot_skills/src/get_ball_contact.cpp +++ b/crane_robot_skills/src/get_ball_contact.cpp @@ -34,4 +34,42 @@ GetBallContact::GetBallContact(uint8_t id, const std::shared_ptr(robot->ball_contact.getContactDuration()) + .count(); + if (contact_duration > 0) { + out << "contacted: " << contact_duration << "ms"; + } else { + out << "ball distance: " << (robot->pose.pos - world_model->ball.pos).norm(); + } +} + +Vector2 GetBallContact::getApproachNormVec() +{ + // if robot is far away from ball, the approach angle is the angle to the ball from robot + // if robot is close to ball, the approach angle is robot angle + // and, the approach angle is interpolated between these two cases + constexpr double FAR_THRESHOLD = 3.5; + constexpr double NEAR_THRESHOLD = 0.5; + + Vector2 far_vec{(robot->pose.pos - world_model->ball.pos).normalized()}; + Vector2 near_vec{cos(robot->pose.theta), sin(robot->pose.theta)}; + + double distance = (robot->pose.pos - world_model->ball.pos).norm(); + + return [&]() { + if (distance > FAR_THRESHOLD) { + return far_vec; + } else if (distance < NEAR_THRESHOLD) { + return near_vec; + } else { + double ratio = (distance - NEAR_THRESHOLD) / (FAR_THRESHOLD - NEAR_THRESHOLD); + return (far_vec * ratio + near_vec * (1.0 - ratio)).normalized(); + } + }(); +} } // namespace crane::skills diff --git a/crane_robot_skills/src/go_over_ball.cpp b/crane_robot_skills/src/go_over_ball.cpp index 5d500bd7c..b07de60e7 100644 --- a/crane_robot_skills/src/go_over_ball.cpp +++ b/crane_robot_skills/src/go_over_ball.cpp @@ -59,4 +59,16 @@ GoOverBall::GoOverBall(uint8_t id, const std::shared_ptr & wm } }); } + +void GoOverBall::print(std::ostream & out) const +{ + out << "[GoOverBall] "; + if ( + has_passed_intermediate_target && + (robot->pose.pos - final_target_pos).norm() > getParameter("reach_threshold")) { + out << "中間地点へ向かっています"; + } else { + out << "最終地点へ向かっています, 距離　" << (robot->pose.pos - final_target_pos).norm(); + } +} } // namespace crane::skills diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 7c94377f5..4258ae3d8 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -50,15 +50,17 @@ void Goalie::emitBallFromPenaltyArea( std::remove_if( passable_robot_list.begin(), passable_robot_list.end(), [&](const RobotInfo::SharedPtr & r) { - // 敵に塞がれていたら除外 - Segment ball_to_robot_line(ball, r->pose.pos); - for (const auto & enemy : world_model->theirs.getAvailableRobots()) { - auto dist = bg::distance(ball_to_robot_line, enemy->pose.pos); - if (dist < 0.2) { - return true; - } + if ( + std::abs(r->pose.pos.x() - world_model->getOurGoalCenter().x()) < + world_model->getDefenseHeight()) { + // ゴールラインに近いロボットは除外 + return true; + } else if (world_model->getDistanceFromRobotToBall(r->getID()) < 0.5) { + // ボールに近いロボットは除外 + return true; + } else { + return false; } - return false; }), passable_robot_list.end()); @@ -80,19 +82,17 @@ void Goalie::emitBallFromPenaltyArea( .dot((pass_target - world_model->ball.pos).normalized()); // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 if (dot < 0.9 || std::abs(getAngleDiff(angle_ball_to_target, command->robot->pose.theta)) > 0.1) { - command->setTargetPosition(intermediate_point); - command->enableCollisionAvoidance(); + command->setTargetPosition(intermediate_point).enableCollisionAvoidance().enableBallAvoidance(); } else { - command->setTargetPosition(world_model->ball.pos); - command->kickWithChip(1.0).disableCollisionAvoidance(); - // command->liftUpDribbler(); - // command->kickStraight(0.2).disableCollisionAvoidance(); - command->enableCollisionAvoidance(); - command->disableBallAvoidance(); + command->setTargetPosition(world_model->ball.pos) + .kickWithChip(1.0) + .disableCollisionAvoidance() + .enableCollisionAvoidance() + .disableBallAvoidance(); } - command->setTargetTheta(getAngle(pass_target - command->robot->pose.pos)); - command->disableGoalAreaAvoidance(); - command->disableRuleAreaAvoidance(); + command->setTargetTheta(getAngle(pass_target - command->robot->pose.pos)) + .disableGoalAreaAvoidance() + .disableRuleAreaAvoidance(); } void Goalie::inplay( @@ -104,25 +104,29 @@ void Goalie::inplay( // シュートチェック Segment goal_line(goals.first, goals.second); Segment ball_line(ball.pos, ball.pos + ball.vel.normalized() * 20.f); - std::vector intersections; - bg::intersection(ball_line, Segment{goals.first, goals.second}, intersections); - command->setTerminalVelocity(0.0); - command->disableGoalAreaAvoidance(); - command->disableBallAvoidance(); - command->disableRuleAreaAvoidance(); + auto intersections = getIntersections(ball_line, Segment{goals.first, goals.second}); + command->setTerminalVelocity(0.0) + .disableGoalAreaAvoidance() + .disableBallAvoidance() + .disableRuleAreaAvoidance(); if (not intersections.empty() && world_model->ball.vel.norm() > 0.3f) { // シュートブロック phase = "シュートブロック"; - ClosestPoint result; - bg::closest_point(ball_line, command->robot->pose.pos, result); - command->setTargetPosition(result.closest_point); - command->lookAtBallFrom(result.closest_point); - if (command->robot->getDistance(result.closest_point) > 0.05) { + auto result = getClosestPointAndDistance(ball_line, command->robot->pose.pos); + auto target = [&]() { + if (not world_model->isFieldInside(result.closest_point)) { + // フィールド外（=ゴール内）でのセーブは避ける + return intersections.front(); + } else { + return result.closest_point; + } + }(); + + command->setTargetPosition(target).lookAtBallFrom(target); + if (command->robot->getDistance(target) > 0.05) { // なりふり構わず爆加速 - command->setTerminalVelocity(2.0); - command->setMaxAcceleration(5.0); - command->setMaxVelocity(5.0); + command->setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } } else { if ( @@ -160,14 +164,13 @@ void Goalie::inplay( if (not world_model->isFieldInside(ball.pos)) { phase += "(範囲外なので正面に構える)"; - command->setTargetPosition(goal_center); - command->lookAt(Point(0, 0)); + command->setTargetPosition(goal_center).lookAt(Point(0, 0)); } else { Point threat_point; if (distance < 2.0) { phase += "(敵のパス先警戒モード)"; - ClosestPoint result; - bg::closest_point(ball_prediction_2s, next_their_attacker->pose.pos, result); + auto result = + getClosestPointAndDistance(ball_prediction_2s, next_their_attacker->pose.pos); threat_point = result.closest_point; } else { phase += "(とりあえず0.5s先を警戒モード)"; @@ -178,8 +181,7 @@ void Goalie::inplay( threat_point, world_model->ours.getAvailableRobots(world_model->getOurGoalieId())); Segment expected_ball_line(threat_point, threat_point + getNormVec(angle) * 10); Segment goal_line(goals.first, goals.second); - std::vector intersections; - bg::intersection(expected_ball_line, goal_line, intersections); + auto intersections = getIntersections(expected_ball_line, goal_line); if (intersections.empty()) { return goal_center; } else { @@ -189,13 +191,10 @@ void Goalie::inplay( Point wait_point = weak_point + (threat_point - weak_point).normalized() * BLOCK_DIST; - command->setTargetPosition(wait_point); - command->lookAtBallFrom(wait_point); + command->setTargetPosition(wait_point).lookAtBallFrom(wait_point); if (command->robot->getDistance(wait_point) > 0.05) { // なりふり構わず爆加速 - command->setTerminalVelocity(2.0); - command->setMaxAcceleration(5.0); - command->setMaxVelocity(5.0); + command->setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } } } diff --git a/crane_robot_skills/src/kickoff_attack.cpp b/crane_robot_skills/src/kickoff_attack.cpp new file mode 100644 index 000000000..7e959d6e3 --- /dev/null +++ b/crane_robot_skills/src/kickoff_attack.cpp @@ -0,0 +1,53 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane::skills +{ +KickoffAttack::KickoffAttack(uint8_t id, const std::shared_ptr & wm) +: SkillBase("KickoffAttack", id, wm, KickoffAttackState::PREPARE_KICKOFF) +{ + setParameter("target_x", 0.0f); + setParameter("target_y", 1.0f); + setParameter("kick_power", 0.3); + addStateFunction( + KickoffAttackState::PREPARE_KICKOFF, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + if (not go_over_ball) { + go_over_ball = std::make_shared(robot->id, world_model); + go_over_ball->setCommander(command); + go_over_ball->setParameter("next_target_x", getParameter("target_x")); + go_over_ball->setParameter("next_target_y", getParameter("target_y")); + go_over_ball->setParameter("margin", 0.3); + command->setMaxVelocity(0.5); + command->disableRuleAreaAvoidance(); + } + go_over_ball_status = go_over_ball->run(visualizer); + return Status::RUNNING; + }); + addTransition(KickoffAttackState::PREPARE_KICKOFF, KickoffAttackState::KICKOFF, [this]() -> bool { + return go_over_ball_status == Status::SUCCESS; + }); + + addStateFunction( + KickoffAttackState::KICKOFF, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + command->setMaxVelocity(0.5); + command->liftUpDribbler(); + command->kickStraight(getParameter("kick_power")); + command->setTargetPosition(world_model->ball.pos); + command->setTerminalVelocity(0.5); + command->disableBallAvoidance(); + command->disableRuleAreaAvoidance(); + if (world_model->ball.vel.norm() > 0.3) { + return Status::SUCCESS; + } else { + return Status::RUNNING; + } + }); +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/marker.cpp b/crane_robot_skills/src/marker.cpp new file mode 100644 index 000000000..aef16bee1 --- /dev/null +++ b/crane_robot_skills/src/marker.cpp @@ -0,0 +1,42 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane::skills +{ +Marker::Marker(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("Marker", id, wm, DefaultStates::DEFAULT) +{ + setParameter("marking_robot_id", 0); + setParameter("mark_distance", 0.5); + setParameter("mark_mode", std::string("save_goal")); + addStateFunction( + DefaultStates::DEFAULT, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + auto marked_robot = world_model->getTheirRobot(getParameter("marking_robot_id")); + auto enemy_pos = marked_robot->pose.pos; + + std::string mode = getParameter("mark_mode"); + Point marking_point; + double target_theta; + + if (mode == "save_goal") { + marking_point = enemy_pos + (world_model->getOurGoalCenter() - enemy_pos).normalized() * + getParameter("mark_distance"); + target_theta = getAngle(enemy_pos - world_model->getOurGoalCenter()); + } else if (mode == "intercept_pass") { + marking_point = enemy_pos + (world_model->ball.pos - enemy_pos).normalized() * + getParameter("mark_distance"); + target_theta = getAngle(enemy_pos - world_model->ball.pos); + } else { + throw std::runtime_error("unknown mark mode"); + } + command->setTargetPosition(marking_point, target_theta); + return Status::RUNNING; + }); +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/move_with_ball.cpp b/crane_robot_skills/src/move_with_ball.cpp index dd37eedd3..9acae48e1 100644 --- a/crane_robot_skills/src/move_with_ball.cpp +++ b/crane_robot_skills/src/move_with_ball.cpp @@ -61,4 +61,17 @@ MoveWithBall::MoveWithBall(uint8_t id, const std::shared_ptr } }); } + +Point MoveWithBall::getTargetPoint(const Point & target_pos) +{ + // 正しい方向でドリブルできている場合だけ前進 + if ( + getAngleDiff(robot->pose, target_theta) < getParameter("moving_direction_tolerance")) { + if (robot->ball_contact.findPastContact(getParameter("max_contact_lost_time"))) { + return robot->pose.pos + (target_pos - robot->pose.pos).normalized() * + getParameter("dribble_target_horizon"); + } + } + return robot->pose.pos; +} } // namespace crane::skills diff --git a/crane_robot_skills/src/penalty_kick.cpp b/crane_robot_skills/src/penalty_kick.cpp new file mode 100644 index 000000000..0ea716bce --- /dev/null +++ b/crane_robot_skills/src/penalty_kick.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane::skills +{ +PenaltyKick::PenaltyKick(uint8_t id, const std::shared_ptr & wm) +: SkillBase("PenaltyKick", id, wm, PenaltyKickState::PREPARE) +{ + setParameter("start_from_kick", false); + setParameter("prepare_margin", 0.6); + addStateFunction( + PenaltyKickState::PREPARE, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + Point target = world_model->ball.pos; + auto margin = getParameter("prepare_margin"); + target.x() += world_model->getOurGoalCenter().x() > 0 ? margin : -margin; + command->setTargetPosition(target); + command->lookAtBall(); + command->disableRuleAreaAvoidance(); + return Status::RUNNING; + }); + + addTransition(PenaltyKickState::PREPARE, PenaltyKickState::KICK, [this]() { + if (getParameter("start_from_kick")) { + return true; + } else { + return world_model->play_situation.getSituationCommandID() == + crane_msgs::msg::PlaySituation::OUR_PENALTY_START; + } + }); + addStateFunction( + PenaltyKickState::KICK, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + if (not start_ball_point) { + start_ball_point = world_model->ball.pos; + } + + auto [best_angle, goal_angle_width] = + world_model->getLargestGoalAngleRangeFromPoint(world_model->ball.pos); + Point best_target = world_model->ball.pos + getNormVec(best_angle) * 0.5; + visualizer->addPoint(best_target.x(), best_target.y(), 1, "red", 1.0, "best_target"); + + // 経由ポイント + Point intermediate_point = + world_model->ball.pos + (world_model->ball.pos - best_target).normalized() * 0.2; + visualizer->addPoint( + intermediate_point.x(), intermediate_point.y(), 1, "red", 1.0, "intermediate_point"); + + double dot = (robot->pose.pos - world_model->ball.pos) + .normalized() + .dot((world_model->ball.pos - best_target).normalized()); + double target_theta = getAngle(best_target - world_model->ball.pos); + // ボールと敵ゴールの延長線上にいない && 角度があってないときは，中間ポイントを経由 + if (dot < 0.9 || std::abs(getAngleDiff(target_theta, robot->pose.theta)) > 0.1) { + command->setTargetPosition(intermediate_point); + command->enableCollisionAvoidance(); + } else { + command->setTargetPosition(world_model->ball.pos); + command->kickStraight(0.3).disableCollisionAvoidance(); + command->enableCollisionAvoidance(); + command->disableBallAvoidance(); + } + + command->setTargetTheta(target_theta); + command->disableRuleAreaAvoidance(); + return Status::RUNNING; + }); + + addTransition(PenaltyKickState::KICK, PenaltyKickState::DONE, [this]() { + return world_model->isDefenseArea(world_model->ball.pos); + }); + + addStateFunction( + PenaltyKickState::DONE, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + command->stopHere(); + return Status::RUNNING; + }); +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/receiver.cpp b/crane_robot_skills/src/receiver.cpp index 4ef7f842f..bb2fe49cc 100644 --- a/crane_robot_skills/src/receiver.cpp +++ b/crane_robot_skills/src/receiver.cpp @@ -35,8 +35,7 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) // 後ろからきたボールは一旦避ける Segment short_ball_line{ world_model->ball.pos, world_model->ball.pos + world_model->ball.vel * 3.0}; - ClosestPoint result; - bg::closest_point(robot->pose.pos, short_ball_line, result); + auto result = getClosestPointAndDistance(robot->pose.pos, short_ball_line); // ボールが敵ゴールに向かっているか double dot_dir = (world_model->getTheirGoalCenter() - world_model->ball.pos).dot(world_model->ball.vel); @@ -55,8 +54,7 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) // ボールの進路上に移動 visualizer->addPoint( robot->pose.pos.x(), robot->pose.pos.y(), 0, "red", 1., "ボールの進路上に移動"); - ClosestPoint result; - bg::closest_point(robot->pose.pos, ball_line, result); + auto result = getClosestPointAndDistance(robot->pose.pos, ball_line); // ゴールとボールの中間方向を向く // TODO(Hansobo): ボールの速さ・キッカーの強さでボールの反射する角度が変わるため、要考慮 @@ -84,8 +82,7 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) ClosestPoint closest_result; closest_result.distance = std::numeric_limits::max(); for (const auto & robot : world_model->theirs.getAvailableRobots()) { - ClosestPoint result; - bg::closest_point(robot->pose.pos, line, result); + auto result = getClosestPointAndDistance(robot->pose.pos, line); if (result.distance < closest_result.distance) { closest_result = result; } @@ -153,4 +150,70 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr & wm) return Status::RUNNING; }); } + +std::vector> Receiver::getPositionsWithScore( + Segment ball_line, Point next_target) +{ + auto points = getPoints(ball_line, 0.05); + std::vector> position_with_score; + for (auto point : points) { + double score = getPointScore(point, next_target); + position_with_score.push_back(std::make_pair(score, point)); + } + return position_with_score; +} + +std::vector Receiver::getPoints(Segment ball_line, double interval) +{ + std::vector points; + float ball_line_len = (ball_line.first - ball_line.second).norm(); + auto norm_vec = (ball_line.second - ball_line.first).normalized(); + for (double d = 0.0; d <= ball_line_len; d += interval) { + points.emplace_back(ball_line.first + d * norm_vec); + } + return points; +} + +std::vector Receiver::getPoints(Point center, float unit, int unit_num) +{ + std::vector points; + for (float x = center.x() - unit * (unit_num / 2.f); x <= center.x() + unit * (unit_num / 2.f); + x += unit) { + for (float y = center.y() - unit * (unit_num / 2.f); y <= center.y() + unit * (unit_num / 2.f); + y += unit) { + points.emplace_back(Point(x, y)); + } + } + return points; +} + +std::vector Receiver::getDPPSPoints(Point center, double r_resolution, int theta_div_num) +{ + std::vector points; + for (int theta_index = 0; theta_index < theta_div_num; theta_index++) { + double theta = 2.0 * M_PI * theta_index / theta_div_num; + for (double r = r_resolution; r <= 10.0; r += r_resolution) { + points.emplace_back(Point(center.x() + r * cos(theta), center.y() + r * sin(theta))); + } + } + points.erase( + std::remove_if( + points.begin(), points.end(), + [&](const auto & point) { + return (not world_model->isFieldInside(point)) or world_model->isDefenseArea(point); + }), + points.end()); + + return points; +} + +double Receiver::getPointScore(Point p, Point next_target) +{ + double nearest_dist; + RobotIdentifier receiver{true, static_cast(session_info.receiver_id)}; + return evaluation::getNextTargetVisibleScore(p, next_target, world_model) * + evaluation::getReachScore(receiver, p, nearest_dist, world_model) * + evaluation::getAngleScore(receiver, p, next_target, world_model) * + evaluation::getEnemyDistanceScore(p, world_model); +} } // namespace crane::skills diff --git a/crane_robot_skills/src/robot_skills_node.cpp b/crane_robot_skills/src/robot_skills_node.cpp deleted file mode 100644 index b51d509f6..000000000 --- a/crane_robot_skills/src/robot_skills_node.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// Copyright (c) 2023 ibis-ssl -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#include -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(rclcpp::NodeOptions()); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/crane_robot_skills/src/simple_attacker.cpp b/crane_robot_skills/src/simple_attacker.cpp index 6b7071929..9dea544f1 100644 --- a/crane_robot_skills/src/simple_attacker.cpp +++ b/crane_robot_skills/src/simple_attacker.cpp @@ -70,8 +70,7 @@ SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptrkickStraight(0.8); // 後ろからきたボールは一旦避ける Segment ball_line{ball_pos, ball_pos + world_model->ball.vel * 3.0}; - ClosestPoint result; - bg::closest_point(robot->pose.pos, ball_line, result); + auto result = getClosestPointAndDistance(robot->pose.pos, ball_line); // ボールが敵ゴールに向かっているか double dot_dir = (world_model->getTheirGoalCenter() - ball_pos).dot(world_model->ball.vel); // ボールがロボットを追い越そうとしているか diff --git a/crane_robot_skills/src/simple_kickoff.cpp b/crane_robot_skills/src/simple_kickoff.cpp new file mode 100644 index 000000000..bd5a3ca17 --- /dev/null +++ b/crane_robot_skills/src/simple_kickoff.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane::skills +{ +SimpleKickOff::SimpleKickOff(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("SimpleKickOff", id, wm, DefaultStates::DEFAULT) +{ + addStateFunction( + DefaultStates::DEFAULT, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + Point intermediate_point = + world_model->ball.pos + + (world_model->ball.pos - world_model->getTheirGoalCenter()).normalized() * 0.3; + + double dot = (robot->pose.pos - world_model->ball.pos) + .normalized() + .dot((world_model->ball.pos - world_model->getTheirGoalCenter()).normalized()); + double target_theta = getAngle(world_model->getTheirGoalCenter() - world_model->ball.pos); + // ボールと敵ゴールの延長線上にいない && 角度があってないときは，中間ポイントを経由 + if ( + (dot < 0.95 && (robot->pose.pos - world_model->ball.pos).norm() > 0.1) || + std::abs(getAngleDiff(target_theta, robot->pose.theta)) > 0.2) { + command->setTargetPosition( + world_model->ball.pos + + (world_model->ball.pos - world_model->getTheirGoalCenter()).normalized() * 0.3); + } else { + command->setTargetPosition( + world_model->ball.pos + + (world_model->getTheirGoalCenter() - world_model->ball.pos).normalized() * 0.3); + command->dribble(0.3); + command->kickWithChip(1.0); + command->disableBallAvoidance(); + } + command->setTargetTheta(target_theta); + return Status::RUNNING; + }); +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index fe74d22f4..af2a33f0c 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -290,4 +290,33 @@ SingleBallPlacement::SingleBallPlacement(uint8_t id, const std::shared_ptrrun(visualizer); }); } + +void SingleBallPlacement::print(std::ostream & os) const +{ + os << "[SingleBallPlacement]"; + + switch (getCurrentState()) { + case SingleBallPlacementStates::GO_OVER_BALL: + go_over_ball->print(os); + break; + case SingleBallPlacementStates::CONTACT_BALL: + get_ball_contact->print(os); + break; + case SingleBallPlacementStates::MOVE_TO_TARGET: + move_with_ball->print(os); + break; + case SingleBallPlacementStates::PLACE_BALL: + os << " PLACE_BALL"; + break; + case SingleBallPlacementStates::SLEEP: + sleep->print(os); + break; + case SingleBallPlacementStates::LEAVE_BALL: + set_target_position->print(os); + break; + default: + os << " UNKNOWN"; + break; + } +} } // namespace crane::skills diff --git a/crane_robot_skills/src/sleep.cpp b/crane_robot_skills/src/sleep.cpp new file mode 100644 index 000000000..8e83b5eea --- /dev/null +++ b/crane_robot_skills/src/sleep.cpp @@ -0,0 +1,40 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane::skills +{ +Sleep::Sleep(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("Sleep", id, wm, DefaultStates::DEFAULT) +{ + setParameter("duration", 0.0); + addStateFunction( + DefaultStates::DEFAULT, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + if (not is_started) { + start_time = std::chrono::steady_clock::now(); + is_started = true; + } + + auto elapsed_time = + std::chrono::duration(std::chrono::steady_clock::now() - start_time); + if (elapsed_time.count() > getParameter("duration")) { + return Status::SUCCESS; + } else { + return Status::RUNNING; + } + }); +} + +void Sleep::print(std::ostream & os) const { os << "[Sleep] 残り時間: " << getRestTime() << "秒"; } + +double Sleep::getRestTime() const +{ + auto elapsed_time = std::chrono::duration(std::chrono::steady_clock::now() - start_time); + return getParameter("duration") - elapsed_time.count(); +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/steal_ball.cpp b/crane_robot_skills/src/steal_ball.cpp new file mode 100644 index 000000000..ff5b2b0af --- /dev/null +++ b/crane_robot_skills/src/steal_ball.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane::skills +{ +StealBall::StealBall(uint8_t id, const std::shared_ptr & wm) +: SkillBase("StealBall", id, wm, StealBallState::MOVE_TO_FRONT) +{ + // ボールを奪う方法 + // front: 正面からドリブラーでボールを奪う + // side: 横から押し出すようにボールを奪う + setParameter("steal_method", std::string("side")); + setParameter("kicker_power", 0.4); + addStateFunction( + StealBallState::MOVE_TO_FRONT, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + // ボールの正面に移動 + // 到着判定すると遅くなるので、敵ロボットにボールが隠されていなかったら次に行ってもいいかも + auto theirs = world_model->theirs.getAvailableRobots(); + if (not theirs.empty()) { + auto [ball_holder, distance] = + world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, theirs); + Point target_pos = world_model->ball.pos + getNormVec(ball_holder->pose.theta) * 0.3; + command->setTargetPosition(target_pos); + command->lookAtBallFrom(target_pos); + if ((robot->pose.pos - target_pos).norm() < 0.2) { + skill_state = Status::SUCCESS; + } else { + skill_state = Status::RUNNING; + } + return skill_state; + } else { + return Status::RUNNING; + } + }); + + // 正面に移動したら突っ込んでボールを奪う + addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::STEAL, [this]() { + return skill_state == Status::SUCCESS; + }); + + // 敵よりもボールに近い場合はパス + addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::PASS, [this]() { + auto theirs = world_model->theirs.getAvailableRobots(); + if (not theirs.empty()) { + auto [their_attacker, their_distance] = + world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, theirs); + double our_distance = robot->getDistance(world_model->ball.pos); + return our_distance < their_distance - 0.2; + } else { + return true; + } + }); + + addStateFunction( + StealBallState::STEAL, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + command->disableBallAvoidance(); + command->disableCollisionAvoidance(); + const auto method = getParameter("steal_method"); + if (method == "front") { + command->setDribblerTargetPosition(world_model->ball.pos); + command->dribble(0.5); + } else if (method == "side") { + command->setDribblerTargetPosition(world_model->ball.pos); + if (robot->getDistance(world_model->ball.pos) < (0.085 + 0.000)) { + // ロボット半径より近くに来れば急回転して刈り取れる + command->setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos) + M_PI / 2); + } else { + command->setTargetTheta(getAngle(world_model->ball.pos - robot->pose.pos)); + } + } + return Status::RUNNING; + }); + + // 敵よりもボールに近い場合はパス + addTransition(StealBallState::STEAL, StealBallState::PASS, [this]() { + auto theirs = world_model->theirs.getAvailableRobots(); + if (not theirs.empty()) { + auto [their_attacker, their_distance] = + world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, theirs); + double our_distance = robot->getDistance(world_model->ball.pos); + return our_distance < their_distance - 0.2; + } else { + return true; + } + }); + + addStateFunction( + StealBallState::PASS, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + if (attacker_skill == nullptr) { + attacker_skill = std::make_shared(robot->id, world_model); + attacker_skill->setCommander(command); + } + auto ours = world_model->ours.getAvailableRobots(robot->id); + ours.erase( + std::remove_if( + ours.begin(), ours.end(), + [this](auto e) { + return e->getDistance(world_model->getTheirGoalCenter()) > + robot->getDistance(world_model->getTheirGoalCenter()); + }), + ours.end()); + if (not ours.empty()) { + auto [target_bot, distance] = world_model->getNearestRobotsWithDistanceFromPoint( + world_model->getTheirGoalCenter(), ours); + attacker_skill->setParameter("receiver_id", target_bot->id); + } + return attacker_skill->run(visualizer); + }); + + addTransition(StealBallState::PASS, StealBallState::MOVE_TO_FRONT, [this]() { + auto theirs = world_model->theirs.getAvailableRobots(); + if (theirs.empty()) { + return false; + } else { + auto [their_attacker, their_distance] = + world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, theirs); + double our_distance = robot->getDistance(world_model->ball.pos); + return our_distance > their_distance; + } + }); + + addTransition(StealBallState::PASS, StealBallState::INTERCEPT, [this]() { + return world_model->ball.vel.norm() > 0.5; + }); + + addTransition(StealBallState::STEAL, StealBallState::INTERCEPT, [this]() { + return world_model->ball.vel.norm() > 0.5; + }); + + addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::INTERCEPT, [this]() { + return world_model->ball.vel.norm() > 0.5; + }); + + addStateFunction(StealBallState::INTERCEPT, [this](const ConsaiVisualizerWrapper::SharedPtr &) { + Point vel_seg = world_model->ball.vel * 5.0; + if (vel_seg.norm() < 1.0) { + vel_seg = vel_seg.normalized() * 1.0; + } + Segment ball_line{world_model->ball.pos, world_model->ball.pos + vel_seg}; + + Point across_point = [&]() { + const double OFFSET = 0.3; + const double X = world_model->field_size.x() / 2.0 - OFFSET; + const double Y = world_model->field_size.y() / 2.0 - OFFSET; + + Segment seg1{Point(X, Y), Point(X, -Y)}; + Segment seg2{Point(-X, Y), Point(-X, -Y)}; + Segment seg3{Point(Y, X), Point(-Y, X)}; + Segment seg4{Point(Y, -X), Point(-Y, -X)}; + std::vector intersections; + if (bg::intersection(ball_line, seg1, intersections); not intersections.empty()) { + return intersections.front(); + } else if (bg::intersection(ball_line, seg2, intersections); not intersections.empty()) { + return intersections.front(); + } else if (bg::intersection(ball_line, seg3, intersections); not intersections.empty()) { + return intersections.front(); + } else if (bg::intersection(ball_line, seg4, intersections); not intersections.empty()) { + return intersections.front(); + } else { + return ball_line.second; + } + }(); + + // ゴールとボールの中間方向を向く + auto [goal_angle, width] = world_model->getLargestGoalAngleRangeFromPoint(across_point); + auto to_goal = getNormVec(goal_angle); + auto to_ball = (world_model->ball.pos - across_point).normalized(); + double intermediate_angle = getAngle(2 * to_goal + to_ball); + command->setTargetTheta(intermediate_angle); + command->liftUpDribbler(); + command->kickStraight(getParameter("kicker_power")); + command->setDribblerTargetPosition(across_point); + + return Status::RUNNING; + }); + + addTransition(StealBallState::INTERCEPT, StealBallState::MOVE_TO_FRONT, [this]() { + return world_model->ball.vel.norm() < 0.3; + }); +} +} // namespace crane::skills diff --git a/crane_robot_skills/src/tuen_around_point.cpp b/crane_robot_skills/src/tuen_around_point.cpp new file mode 100644 index 000000000..9fad487bb --- /dev/null +++ b/crane_robot_skills/src/tuen_around_point.cpp @@ -0,0 +1,70 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane::skills +{ +TurnAroundPoint::TurnAroundPoint(uint8_t id, const std::shared_ptr & wm) +: SkillBase<>("TurnAroundPoint", id, wm, DefaultStates::DEFAULT) +{ + setParameter("target_x", 0.0); + setParameter("target_y", 0.0); + setParameter("target_angle", 0.0); + // 半径方向の制御ゲイン + setParameter("dr_p_gain", 30.0); + setParameter("max_velocity", 0.5); + setParameter("max_turn_omega", M_PI_4); + addStateFunction( + DefaultStates::DEFAULT, + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + Point target_point(getParameter("target_x"), getParameter("target_y")); + double target_angle = getParameter("target_angle"); + if (target_distance < 0.0) { + // 初回のみ実行時のロボットとの距離を計算して、円弧の半径とする + target_distance = (robot->pose.pos - target_point).norm(); + current_target_angle = getAngle(robot->pose.pos - target_point); + auto max_velocity = getParameter("max_velocity"); + auto max_turn_omega = getParameter("max_turn_omega"); + if (target_distance * max_turn_omega > max_velocity) { + max_velocity = target_distance * max_turn_omega; + } else { + max_turn_omega = max_velocity / target_distance; + } + } + + if (std::abs(getAngleDiff(getAngle(robot->pose.pos - target_point), target_angle)) < 0.1) { + command->stopHere(); + return Status::SUCCESS; + } else { + // 円弧を描いて移動する + + double current_angle = getAngle(robot->pose.pos - target_point); + double angle_diff = getAngleDiff(target_angle, current_angle); + + // 半径方向の距離のズレ + double dr = (robot->pose.pos - target_point).norm() - target_distance; + std::cout << "dr: " << dr << std::endl; + std::cout << "target_distance: " << target_distance << std::endl; + auto max_velocity = getParameter("max_velocity"); + // 円弧を描くような速度 + Velocity velocity = ((target_point - robot->pose.pos).normalized() * + (dr * getParameter("dr_p_gain"))) + + getNormVec(current_angle + std::copysign(M_PI_2, angle_diff)) * + std::min(max_velocity, std::abs(angle_diff * 0.6)); + command->setVelocity(velocity); + + // current_target_angle += std::copysign(max_turn_omega / 30.0f, angle_diff); + // command->setTargetPosition( + // target_point + getNormVec(current_target_angle) * target_distance); + + // 中心点の方を向く + command->setTargetTheta(normalizeAngle(current_angle + M_PI)); + return Status::RUNNING; + } + }); +} +} // namespace crane::skills diff --git a/crane_sender/CMakeLists.txt b/crane_sender/CMakeLists.txt index e085dfbf3..329b429b1 100755 --- a/crane_sender/CMakeLists.txt +++ b/crane_sender/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_sender) -# Default to C++17 +# Default to C++20 if (NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp index b4c5b62b6..350ae7cdf 100644 --- a/crane_sender/src/ibis_sender_node.cpp +++ b/crane_sender/src/ibis_sender_node.cpp @@ -34,13 +34,21 @@ namespace crane class RobotCommandSender { public: - explicit RobotCommandSender(uint8_t robot_id) + explicit RobotCommandSender(uint8_t robot_id, bool sim_mode) : io_service(), socket(io_service, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0)) { boost::asio::ip::udp::resolver resolver(io_service); - std::string host = "192.168.20." + std::to_string(100 + robot_id); - boost::asio::ip::udp::resolver::query query(host, std::to_string(12345)); - endpoint = *resolver.resolve(query); + endpoint = [&]() -> boost::asio::ip::udp::endpoint { + if (sim_mode) { + std::string host = "localhost"; + boost::asio::ip::udp::resolver::query query(host, std::to_string(50100 + robot_id)); + return *resolver.resolve(query); + } else { + std::string host = "192.168.20." + std::to_string(100 + robot_id); + boost::asio::ip::udp::resolver::query query(host, "12345"); + return *resolver.resolve(query); + } + }(); } RobotCommandSerialized send(RobotCommand packet) @@ -89,10 +97,13 @@ class IbisSenderNode : public SenderBase public: CLASS_LOADER_PUBLIC - explicit IbisSenderNode(const rclcpp::NodeOptions & options) : SenderBase("real_sender", options) + explicit IbisSenderNode(const rclcpp::NodeOptions & options) : SenderBase("ibis_sender", options) { declare_parameter("debug_id", -1); get_parameter("debug_id", debug_id); + + declare_parameter("sim_mode", true); + get_parameter("sim_mode", sim_mode); parameter_subscriber = std::make_shared(this); parameter_callback_handle = parameter_subscriber->add_parameter_callback("debug_id", [&](const rclcpp::Parameter & p) { @@ -112,8 +123,8 @@ class IbisSenderNode : public SenderBase // interface = "lo"; // } - for (int i = 0; i < senders.size(); i++) { - senders[i] = std::make_shared(i); + for (std::size_t i = 0; i < senders.size(); i++) { + senders[i] = std::make_shared(i, sim_mode); } world_model = std::make_shared(*this); @@ -126,15 +137,6 @@ class IbisSenderNode : public SenderBase if (not world_model->hasUpdated()) { return; } - // uint8_t send_packet[32] = {}; - - auto to_two_byte = [](float val, float range) -> std::pair { - uint16_t two_byte = static_cast(32767 * static_cast(val / range) + 32767); - uint8_t byte_low, byte_high; - byte_low = two_byte & 0x00FF; - byte_high = (two_byte & 0xFF00) >> 8; - return std::make_pair(byte_low, byte_high); - }; auto normalize_angle = [](float angle_rad) -> float { if (fabs(angle_rad) > M_PI) { diff --git a/crane_simple_ai/CMakeLists.txt b/crane_simple_ai/CMakeLists.txt index b40ccacc9..25987bba3 100755 --- a/crane_simple_ai/CMakeLists.txt +++ b/crane_simple_ai/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.14) project(crane_simple_ai) -# Default to C++17 +# Default to C++20 if (NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_speaker/CMakeLists.txt b/crane_speaker/CMakeLists.txt index 286063119..ed8429ee2 100755 --- a/crane_speaker/CMakeLists.txt +++ b/crane_speaker/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_speaker) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_world_model_publisher/CMakeLists.txt b/crane_world_model_publisher/CMakeLists.txt index 6d10d7d55..5fd95fe20 100755 --- a/crane_world_model_publisher/CMakeLists.txt +++ b/crane_world_model_publisher/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_world_model_publisher) -# Default to C++17 +# Default to C++20 if (NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index d0f36bb44..b5735e79e 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -244,8 +244,8 @@ void WorldModelPublisherComponent::publishWorldModel() wm.on_positive_half = on_positive_half; wm.ball_info = ball_info; - bool pre_is_our_ball = is_our_ball; - bool pre_is_their_ball = is_their_ball; + // bool pre_is_our_ball = is_our_ball; + // bool pre_is_their_ball = is_their_ball; updateBallContact(); @@ -410,7 +410,7 @@ void WorldModelPublisherComponent::updateBallContact() } // ローカルセンサーの情報でボール情報を更新 - for (int i = 0; i < robot_info[static_cast(our_color)].size(); i++) { + for (std::size_t i = 0; i < robot_info[static_cast(our_color)].size(); i++) { if (ball_detected[i]) { robot_info[static_cast(our_color)][i].ball_contact.is_vision_source = false; robot_info[static_cast(our_color)][i].ball_contact.current_time = now; diff --git a/docker/base/Dockerfile b/docker/base/Dockerfile index 427009299..0c84bc79d 100644 --- a/docker/base/Dockerfile +++ b/docker/base/Dockerfile @@ -1,18 +1,24 @@ ARG ROS_DISTRO="humble" FROM ros:$ROS_DISTRO +ENV PATH $PATH:/usr/local/go/bin + ENV ROS_OVERLAY /root/ibis_ws WORKDIR $ROS_OVERLAY/src COPY . crane RUN apt-get update && \ - apt-get install -y wget ccache python3-pip && \ + apt-get install -y wget ccache python3-pip ffmpeg&& \ wget https://launchpad.net/ubuntu/+archive/primary/+files/liborocos-bfl0.8_0.8.0-6_amd64.deb && \ wget https://launchpad.net/ubuntu/+archive/primary/+files/liborocos-bfl-dev_0.8.0-6_amd64.deb && \ apt-get install -y ./liborocos-bfl0.8_0.8.0-6_amd64.deb && \ apt-get install -y ./liborocos-bfl-dev_0.8.0-6_amd64.deb +RUN wget https://go.dev/dl/go1.22.4.linux-amd64.tar.gz && \ + tar -C /usr/local -xzf go1.22.4.linux-amd64.tar.gz && \ + rm go1.22.4.linux-amd64.tar.gz + RUN vcs import . < crane/dependency_${ROS_DISTRO}.repos RUN rosdep update && \ diff --git a/docker/config/state-store.json.stream b/docker/config/state-store.json.stream index f2901062d..2aef66eef 100644 --- a/docker/config/state-store.json.stream +++ b/docker/config/state-store.json.stream @@ -10,3 +10,16 @@ {"id":9, "statePre":{"stage":"NORMAL_FIRST_HALF_PRE", "command":{"type":"NORMAL_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"KICKOFF", "forTeam":"BLUE"}, "stageTimeElapsed":"0s", "stageTimeLeft":"0s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":6, "challengeFlags":3}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":6, "challengeFlags":3}}, "currentActionTimeRemaining":"10s", "division":"DIV_B", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH"}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"NORMAL_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"KICKOFF", "forTeam":"BLUE"}, "stageTimeElapsed":"0s", "stageTimeLeft":"300s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":6, "challengeFlags":3}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":6, "challengeFlags":3}}, "currentActionTimeRemaining":"10s", "division":"DIV_B", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH"}, "change":{"origin":"StateMachine", "revertible":false, "changeStageChange":{"newStage":"NORMAL_FIRST_HALF"}}, "timestamp":"2024-03-30T07:31:37.214660494Z"} {"id":10,"statePre":{"stage":"NORMAL_FIRST_HALF","command":{"type":"NORMAL_START","forTeam":"UNKNOWN"},"gameState":{"type":"KICKOFF","forTeam":"BLUE"},"stageTimeElapsed":"4.324243910s","stageTimeLeft":"295.675756090s","matchTimeStart":"1970-01-01T00:00:00Z","teamState":{"BLUE":{"name":"ibis","goals":0,"goalkeeper":0,"timeoutsLeft":4,"timeoutTimeLeft":"300s","onPositiveHalf":true,"ballPlacementFailures":0,"ballPlacementFailuresReached":false,"canPlaceBall":true,"maxAllowedBots":6,"challengeFlags":3,"botSubstitutionAllowed":false,"botSubstitutionsLeft":0,"botSubstitutionTimeLeft":"0s"},"YELLOW":{"name":"KIKS","goals":0,"goalkeeper":0,"timeoutsLeft":4,"timeoutTimeLeft":"300s","onPositiveHalf":false,"ballPlacementFailures":0,"ballPlacementFailuresReached":false,"canPlaceBall":true,"maxAllowedBots":6,"challengeFlags":3,"botSubstitutionAllowed":false,"botSubstitutionsLeft":0,"botSubstitutionTimeLeft":"0s"}},"currentActionTimeRemaining":"5.675756090s","division":"DIV_B","firstKickoffTeam":"BLUE","matchType":"UNKNOWN_MATCH","maxBotsPerTeam":0},"state":{"stage":"NORMAL_FIRST_HALF","command":{"type":"NORMAL_START","forTeam":"UNKNOWN"},"gameState":{"type":"KICKOFF","forTeam":"BLUE"},"stageTimeElapsed":"4.324243910s","stageTimeLeft":"295.675756090s","matchTimeStart":"1970-01-01T00:00:00Z","teamState":{"BLUE":{"name":"ibis","goals":0,"goalkeeper":0,"timeoutsLeft":4,"timeoutTimeLeft":"300s","onPositiveHalf":true,"ballPlacementFailures":0,"ballPlacementFailuresReached":false,"canPlaceBall":true,"maxAllowedBots":11,"challengeFlags":3,"botSubstitutionAllowed":false,"botSubstitutionsLeft":0,"botSubstitutionTimeLeft":"0s"},"YELLOW":{"name":"KIKS","goals":0,"goalkeeper":0,"timeoutsLeft":4,"timeoutTimeLeft":"300s","onPositiveHalf":false,"ballPlacementFailures":0,"ballPlacementFailuresReached":false,"canPlaceBall":true,"maxAllowedBots":11,"challengeFlags":3,"botSubstitutionAllowed":false,"botSubstitutionsLeft":0,"botSubstitutionTimeLeft":"0s"}},"currentActionTimeRemaining":"5.675756090s","division":"DIV_A","firstKickoffTeam":"BLUE","matchType":"UNKNOWN_MATCH","maxBotsPerTeam":11},"change":{"origin":"UI","revertible":true,"updateConfigChange":{"division":"DIV_A"}},"timestamp":"2024-04-20T11:46:44.212761844Z"} {"id":11,"statePre":{"stage":"NORMAL_FIRST_HALF","command":{"type":"NORMAL_START","forTeam":"UNKNOWN"},"gameState":{"type":"KICKOFF","forTeam":"BLUE"},"stageTimeElapsed":"10.000010125s","stageTimeLeft":"289.999989875s","matchTimeStart":"1970-01-01T00:00:00Z","teamState":{"BLUE":{"name":"ibis","goals":0,"goalkeeper":0,"timeoutsLeft":4,"timeoutTimeLeft":"300s","onPositiveHalf":true,"ballPlacementFailures":0,"ballPlacementFailuresReached":false,"canPlaceBall":true,"maxAllowedBots":11,"challengeFlags":3,"botSubstitutionAllowed":false,"botSubstitutionsLeft":0,"botSubstitutionTimeLeft":"0s"},"YELLOW":{"name":"KIKS","goals":0,"goalkeeper":0,"timeoutsLeft":4,"timeoutTimeLeft":"300s","onPositiveHalf":false,"ballPlacementFailures":0,"ballPlacementFailuresReached":false,"canPlaceBall":true,"maxAllowedBots":11,"challengeFlags":3,"botSubstitutionAllowed":false,"botSubstitutionsLeft":0,"botSubstitutionTimeLeft":"0s"}},"currentActionTimeRemaining":"-0.000010125s","division":"DIV_A","firstKickoffTeam":"BLUE","matchType":"UNKNOWN_MATCH","maxBotsPerTeam":11},"state":{"stage":"NORMAL_FIRST_HALF","command":{"type":"NORMAL_START","forTeam":"UNKNOWN"},"gameState":{"type":"RUNNING"},"stageTimeElapsed":"10.000010125s","stageTimeLeft":"289.999989875s","matchTimeStart":"1970-01-01T00:00:00Z","teamState":{"BLUE":{"name":"ibis","goals":0,"goalkeeper":0,"timeoutsLeft":4,"timeoutTimeLeft":"300s","onPositiveHalf":true,"ballPlacementFailures":0,"ballPlacementFailuresReached":false,"canPlaceBall":true,"maxAllowedBots":11,"challengeFlags":3,"botSubstitutionAllowed":false,"botSubstitutionsLeft":0,"botSubstitutionTimeLeft":"0s"},"YELLOW":{"name":"KIKS","goals":0,"goalkeeper":0,"timeoutsLeft":4,"timeoutTimeLeft":"300s","onPositiveHalf":false,"ballPlacementFailures":0,"ballPlacementFailuresReached":false,"canPlaceBall":true,"maxAllowedBots":11,"challengeFlags":3,"botSubstitutionAllowed":false,"botSubstitutionsLeft":0,"botSubstitutionTimeLeft":"0s"}},"currentActionTimeRemaining":"-0.000010125s","division":"DIV_A","firstKickoffTeam":"BLUE","matchType":"UNKNOWN_MATCH","maxBotsPerTeam":11},"change":{"origin":"Engine","revertible":false,"newGameStateChange":{"gameState":{"type":"RUNNING"}}},"timestamp":"2024-04-20T11:46:49.863776310Z"} +{"id":12, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"NORMAL_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"-58.348240355s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:46:19.845387306Z", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"STOP", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:46:17.845330624Z"} +{"id":13, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:46:19.845387306Z", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:46:19.845387306Z", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "updateTeamStateChange":{"forTeam":"YELLOW", "onPositiveHalf":true}}, "timestamp":"2024-05-13T22:46:36.905261473Z"} +{"id":14, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:46:19.845387306Z", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"HALT", "forTeam":"UNKNOWN"}, "gameState":{"type":"HALT"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"HALT", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:47:32.237898365Z"} +{"id":15, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"HALT", "forTeam":"UNKNOWN"}, "gameState":{"type":"HALT"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:47:42.993880415Z", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"STOP", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:47:32.993795977Z"} +{"id":16, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:47:42.993880415Z", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"68.348240355s", "stageTimeLeft":"231.651759645s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:47:33.949136719Z"} +{"id":17, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"246.237480135s", "stageTimeLeft":"53.762519865s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"-177.889239780s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"246.237480135s", "stageTimeLeft":"53.762519865s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:50:33.855845814Z", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"STOP", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:50:31.855787746Z"} +{"id":18, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"246.237480135s", "stageTimeLeft":"53.762519865s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:50:33.855845814Z", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"246.237480135s", "stageTimeLeft":"53.762519865s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:50:33.370829065Z"} +{"id":19, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"379.661198994s", "stageTimeLeft":"-79.661198994s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"-133.423718859s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"379.661198994s", "stageTimeLeft":"-79.661198994s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:52:48.798417872Z", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"STOP", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:52:46.798164669Z"} +{"id":20, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"379.661198994s", "stageTimeLeft":"-79.661198994s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:52:48.798417872Z", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"379.661198994s", "stageTimeLeft":"-79.661198994s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:52:58.920437726Z"} +{"id":21, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"485.411057148s", "stageTimeLeft":"-185.411057148s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"-105.749858154s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"485.411057148s", "stageTimeLeft":"-185.411057148s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:54:46.674507001Z", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"STOP", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:54:44.674433514Z"} +{"id":22, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"485.411057148s", "stageTimeLeft":"-185.411057148s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T22:54:46.674507001Z", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"485.411057148s", "stageTimeLeft":"-185.411057148s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T22:54:45.147778704Z"} +{"id":23, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"843.420834256s", "stageTimeLeft":"-543.420834256s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"-358.009777108s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"843.420834256s", "stageTimeLeft":"-543.420834256s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T23:00:45.150655186Z", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"STOP", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T23:00:43.150594953Z"} +{"id":24, "statePre":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"STOP", "forTeam":"UNKNOWN"}, "gameState":{"type":"STOP"}, "stageTimeElapsed":"843.420834256s", "stageTimeLeft":"-543.420834256s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "readyContinueTime":"2024-05-13T23:00:45.150655186Z", "maxBotsPerTeam":11}, "state":{"stage":"NORMAL_FIRST_HALF", "command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}, "gameState":{"type":"RUNNING"}, "stageTimeElapsed":"843.420834256s", "stageTimeLeft":"-543.420834256s", "matchTimeStart":"1970-01-01T00:00:00Z", "teamState":{"BLUE":{"name":"ibis", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":false, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}, "YELLOW":{"name":"KIKS", "goals":0, "goalkeeper":0, "timeoutsLeft":4, "timeoutTimeLeft":"300s", "onPositiveHalf":true, "ballPlacementFailures":0, "ballPlacementFailuresReached":false, "canPlaceBall":true, "maxAllowedBots":11, "challengeFlags":3, "botSubstitutionAllowed":false, "botSubstitutionsLeft":0, "botSubstitutionTimeLeft":"0s"}}, "currentActionTimeRemaining":"0s", "division":"DIV_A", "firstKickoffTeam":"BLUE", "matchType":"UNKNOWN_MATCH", "maxBotsPerTeam":11}, "change":{"origin":"UI", "revertible":true, "newCommandChange":{"command":{"type":"FORCE_START", "forTeam":"UNKNOWN"}}}, "timestamp":"2024-05-13T23:00:45.035097399Z"} diff --git a/docker/scenario/Dockerfile b/docker/scenario/Dockerfile new file mode 100644 index 000000000..4f1372c89 --- /dev/null +++ b/docker/scenario/Dockerfile @@ -0,0 +1,18 @@ +FROM ghcr.io/ibis-ssl/crane:base + +ENV ROS_OVERLAY /root/ibis_ws +ENV PATH $PATH:/usr/local/go/bin +WORKDIR $ROS_OVERLAY/src +SHELL ["/bin/bash", "-c"] + +COPY . crane + +RUN vcs import . < crane/dependency_${ROS_DISTRO}.repos + +RUN rosdep update && \ + rosdep install -iy --from-paths . && \ + rm -rf /var/lib/apt/lists/ + +RUN cd .. && \ + source /opt/ros/humble/setup.bash && \ + colcon build --symlink-install --cmake-args '-DCMAKE_BUILD_TYPE=Release' diff --git a/docker/scenario/docker-compose.yaml b/docker/scenario/docker-compose.yaml new file mode 100644 index 000000000..959d16c60 --- /dev/null +++ b/docker/scenario/docker-compose.yaml @@ -0,0 +1,24 @@ +version: "3.1" +services: + grsim: + image: ghcr.io/ssl-roots/docker_images/grsim:main + container_name: grsim + network_mode: host + command: | + grSim --headless -platform offscreen + tty: true + stdin_open: true + restart: "no" + + crane: + image: ghcr.io/ibis-ssl/crane:${CRANE_TAG} + container_name: crane + network_mode: host + command: | + bash -c " + source ../install/setup.bash && + ros2 launch crane_bringup crane.launch.xml sim:=true gui:=false speak:=false original_grsim:=true vision_port:=10020 referee_port:=10003 + " + tty: true + stdin_open: true + restart: "no" diff --git a/future/crane_enemy_motion_player/CMakeLists.txt b/future/crane_enemy_motion_player/CMakeLists.txt deleted file mode 100755 index f3096b920..000000000 --- a/future/crane_enemy_motion_player/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(crane_enemy_motion_player) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -g) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_executable(enemy_motion_player_node - src/enemy_motion_player_node.cpp -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/future/crane_enemy_motion_player/package.xml b/future/crane_enemy_motion_player/package.xml deleted file mode 100755 index d8ab66b15..000000000 --- a/future/crane_enemy_motion_player/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - crane_enemy_motion_player - 0.0.0 - Simulate game situation for debugging. - ibis-ssl - MIT - - ament_cmake_auto - - crane_msgs - rclcpp - std_msgs - - ament_lint_auto - crane_lint_common - - - ament_cmake - - diff --git a/future/crane_enemy_motion_player/src/enemy_motion_player_node.cpp b/future/crane_enemy_motion_player/src/enemy_motion_player_node.cpp deleted file mode 100755 index 5cbb281b6..000000000 --- a/future/crane_enemy_motion_player/src/enemy_motion_player_node.cpp +++ /dev/null @@ -1,13 +0,0 @@ -// Copyright (c) 2022 ibis-ssl -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#include - -int main() -{ - std::cout << "hello, this is enemy_motion_player_node" << std::endl; - return 0; -} diff --git a/scenario_test/STOP_AVOID_BALL.py b/scenario_test/STOP_AVOID_BALL.py new file mode 100644 index 000000000..c6011d24e --- /dev/null +++ b/scenario_test/STOP_AVOID_BALL.py @@ -0,0 +1,43 @@ +import math +import time +from rcst.communication import Communication +from rcst import calc +from rcst.ball import Ball +from rcst.robot import RobotDict + + +def test_avoid_ball(rcst_comm: Communication): + rcst_comm.send_empty_world() + for i in range(11): + rcst_comm.send_yellow_robot(i, -1.0, 3.0 - i * 0.5, math.radians(0)) + + def yellow_robot_did_not_avoid_ball( + ball: Ball, blue_robots: RobotDict, yellow_robots: RobotDict + ) -> bool: + for robot in yellow_robots.values(): + if calc.distance_robot_and_ball(robot, ball) < 0.4: + return True + return False + + rcst_comm.observer.customized().register_sticky_true_callback( + "yellow_robot_did_not_avoid_ball", yellow_robot_did_not_avoid_ball + ) + + rcst_comm.change_referee_command("STOP", 1.0) + + def check(x: float, y: float, vx: float = 0.0, vy: float = 0.0): + rcst_comm.send_ball(x, y, vx, vy) + time.sleep(2) + rcst_comm.observer.reset() + + success = True + for _ in range(5): + if rcst_comm.observer.customized().get_result("yellow_robot_did_not_avoid_ball"): + success = False + break + time.sleep(1) + assert success is True + + check(0, 0) + check(3.5, 0) + check(3.5, 0, vx=-2.0) diff --git a/scenario_test/STOP_ROBOT_SPEED.py b/scenario_test/STOP_ROBOT_SPEED.py new file mode 100644 index 000000000..cb03f268b --- /dev/null +++ b/scenario_test/STOP_ROBOT_SPEED.py @@ -0,0 +1,29 @@ +import math +import time +from rcst.communication import Communication +from rcst import calc +from rcst.ball import Ball +from rcst.robot import RobotDict + + +def test_robot_speed(rcst_comm: Communication): + rcst_comm.send_empty_world() + ball_x = 1.0 + rcst_comm.send_ball(ball_x, 0) + for i in range(11): + rcst_comm.send_yellow_robot(i, -1.0, 3.0 - i * 0.5, math.radians(0)) + + rcst_comm.change_referee_command("STOP", 3.0) + + rcst_comm.observer.reset() + success = True + rcst_comm.send_ball(ball_x, 0, 5.0, 0.0) # Move the ball + for _ in range(10): + if rcst_comm.observer.robot_speed().some_yellow_robots_over(1.5): + velocities = rcst_comm.observer.robot_speed().yellow_max_velocities() + for robot_id in velocities.keys(): + print(f"Robot {robot_id} has speed {velocities[robot_id]}") + success = False + break + time.sleep(1) + assert success is True diff --git a/scenario_test/emit_from_penalty_01.py b/scenario_test/emit_from_penalty_01.py new file mode 100644 index 000000000..ff5a79bd5 --- /dev/null +++ b/scenario_test/emit_from_penalty_01.py @@ -0,0 +1,24 @@ +import math +import time +from rcst.communication import Communication +from rcst import calc +from rcst.ball import Ball +from rcst.robot import RobotDict + + +def is_in_penalty_area(x: float, y: float) -> bool: + return math.fabs(x) >= 6.0 and math.fabs(y) <= 1.8 + + +def test_emit_from_penalty_01(rcst_comm: Communication): + rcst_comm.send_empty_world() + rcst_comm.send_ball(5.0, 1.0) + rcst_comm.send_yellow_robot(0, 6.0, 0, 0) + rcst_comm.change_referee_command("FORCE_START", 3.0) + + rcst_comm.observer.reset() + time.sleep(5) + success = not is_in_penalty_area( + rcst_comm.observer.get_world().get_ball().x, rcst_comm.observer.get_world().get_ball().y + ) + assert success is True diff --git a/session/crane_planner_base/CMakeLists.txt b/session/crane_planner_base/CMakeLists.txt index 8269fe7b8..4ed720d48 100755 --- a/session/crane_planner_base/CMakeLists.txt +++ b/session/crane_planner_base/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_planner_base) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/session/crane_planner_plugins/CMakeLists.txt b/session/crane_planner_plugins/CMakeLists.txt index 2d505b190..323e38bc0 100755 --- a/session/crane_planner_plugins/CMakeLists.txt +++ b/session/crane_planner_plugins/CMakeLists.txt @@ -3,9 +3,9 @@ cmake_minimum_required(VERSION 3.12) project(crane_planner_plugins) -# Default to C++17 +# Default to C++20 if (NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp index 6987a929b..c19e3e564 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp @@ -44,26 +44,7 @@ class AttackerPlanner : public PlannerBase protected: auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - auto selected = this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - // ボールに近いほどスコアが高い - return 100.0 - std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); - }, - prev_roles, - [this](const std::shared_ptr & robot) { - // ヒステリシスは1m - return 1.; - }); - if (selected.empty()) { - return {}; - } else { - attacker_ = std::make_shared(selected.front(), world_model); - return {selected.front()}; - } - } + const std::unordered_map & prev_roles) -> std::vector override; std::shared_ptr attacker_; }; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/catch_ball_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/catch_ball_planner.hpp index 9c3d81a38..3f2ab68ce 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/catch_ball_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/catch_ball_planner.hpp @@ -42,123 +42,11 @@ class CatchBallPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - std::vector commands; - for (const auto & robot : robots) { - crane::RobotCommandWrapper target(robot.robot_id, world_model); - - Point target_point = default_point; - auto ball = world_model->ball.pos; - - // シュートチェック - Vector2 norm_vec = getVerticalVec(getNormVec(target.robot->pose.theta)) * 0.8; - Segment receive_line(target.robot->pose.pos + norm_vec, target.robot->pose.pos - norm_vec); - Segment ball_line(ball, ball + world_model->ball.vel.normalized() * 20.f); - std::vector intersections; - bg::intersection(ball_line, Segment{receive_line.first, receive_line.second}, intersections); - - if (not intersections.empty() && world_model->ball.vel.norm() > 0.3f) { - // シュートブロック - std::cout << "シュートブロック" << std::endl; - ClosestPoint result; - bg::closest_point(ball_line, target.robot->pose.pos, result); - target.setTargetPosition(result.closest_point); - target.setTargetTheta(getAngle(-world_model->ball.vel)); - if (target.robot->getDistance(result.closest_point) > 0.2) { - target.setTerminalVelocity(2.0); - } - } else { - if (world_model->ball.isStopped(0.3) && not world_model->isFriendDefenseArea(ball)) { - // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す - std::cout << "ボール排出" << std::endl; - // パスできるロボットのリストアップ - auto passable_robot_list = world_model->ours.getAvailableRobots(target.robot->id); - passable_robot_list.erase( - std::remove_if( - passable_robot_list.begin(), passable_robot_list.end(), - [&](const RobotInfo::SharedPtr & r) { - // 敵に塞がれていたら除外 - Segment ball_to_robot_line(ball, r->pose.pos); - for (const auto & enemy : world_model->theirs.getAvailableRobots()) { - auto dist = bg::distance(ball_to_robot_line, enemy->pose.pos); - if (dist < 0.2) { - return true; - } - } - return false; - }), - passable_robot_list.end()); - - std::cout << "パスターゲット: "; - for (auto a : passable_robot_list) { - std::cout << static_cast(a->id) << ","; - } - std::cout << std::endl; - - Point pass_target = [&]() { - if (not passable_robot_list.empty()) { - // TODO(HansRobo): いい感じのロボットを選ぶようにする - return passable_robot_list.front()->pose.pos; - } else { - return Point{0, 0}; - } - }(); - - std::cout << pass_target.x() << ", " << pass_target.y() << std::endl; - Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f; - double angle_ball_to_target = getAngle(pass_target - ball); - double dot = (world_model->ball.pos - target.robot->pose.pos) - .normalized() - .dot((pass_target - world_model->ball.pos).normalized()); - // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 - if ( - dot < 0.9 || - std::abs(getAngleDiff(angle_ball_to_target, target.robot->pose.theta)) > 0.1) { - std::cout << "中間ポイント経由" << std::endl; - target.setTargetPosition(intermediate_point); - target.enableCollisionAvoidance(); - } else { - std::cout << "ボール突撃" << std::endl; - target.setTargetPosition(world_model->ball.pos); - target.liftUpDribbler(); - target.kickStraight(0.1).disableCollisionAvoidance(); - target.enableCollisionAvoidance(); - target.disableBallAvoidance(); - } - target.setTargetTheta(getAngle(pass_target - ball)); - target.disableGoalAreaAvoidance(); - } else { - // phase = ""; - const double BLOCK_DIST = 0.15; - // 範囲外のときは正面に構える - if (not world_model->isFieldInside(ball)) { - // phase += "正面で"; - ball << 0, 0; - } - - // phase = "ボールを待ち受ける"; - target.setTargetPosition(default_point); - target.lookAtBall(); - } - } - - commands.push_back(target.getMsg()); - } - return {PlannerBase::Status::RUNNING, commands}; - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - return this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - return 100. / world_model->getSquareDistanceFromRobot(robot->id, default_point); - }, - prev_roles); - } + const std::unordered_map & prev_roles) -> std::vector override; private: rclcpp::TimerBase::SharedPtr timer; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp index 0497f1b90..9c6896446 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp @@ -181,24 +181,8 @@ class DefenderPlanner : public PlannerBase }, prev_roles); - if (selected.size() > 2) { - // 一番遠いのを採用 - double max_sq_dist = 0.; - for (auto robot : selected) { - auto sq_dist = world_model->getSquareDistanceFromRobot(robot, defense_point); - if (sq_dist > max_sq_dist) { - max_sq_dist = sq_dist; - second_threat_defender = robot; - } - } - } else { - second_threat_defender = std::nullopt; - } - return selected; } - - std::optional second_threat_defender = std::nullopt; }; } // namespace crane diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/formation_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/formation_planner.hpp index 4129e7d11..e0b7e9a2c 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/formation_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/formation_planner.hpp @@ -42,16 +42,7 @@ class FormationPlanner : public PlannerBase auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - return this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - // choose id smaller first - return 15. - static_cast(-robot->id); - }, - prev_roles); - } + const std::unordered_map & prev_roles) -> std::vector override; }; } // namespace crane diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp index 6d59071e6..f86d2433d 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp @@ -75,21 +75,7 @@ class ReceivePlanner : public PlannerBase auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - auto selected = this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - return 100. / world_model->getSquareDistanceFromRobotToBall(robot->id); - }, - prev_roles); - if (selected.empty()) { - return {}; - } else { - receiver_skill = std::make_shared(selected.front(), world_model); - return {selected.front()}; - } - } + const std::unordered_map & prev_roles) -> std::vector override; private: std::shared_ptr receiver_skill = nullptr; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp index 47b43829e..ae7885b4f 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp @@ -76,16 +76,7 @@ class GoalieSkillPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - if (not skill) { - return {PlannerBase::Status::RUNNING, {}}; - } else { - std::vector robot_commands; - auto status = skill->run(visualizer); - return {static_cast(status), {skill->getRobotCommand()}}; - } - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, @@ -109,45 +100,11 @@ class BallPlacementSkillPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - if (not skill) { - return {PlannerBase::Status::RUNNING, {}}; - } else { - if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { - skill->setParameter("placement_x", target->x()); - skill->setParameter("placement_y", target->y()); - } - std::vector robot_commands; - auto status = skill->run(visualizer); - return {static_cast(status), {skill->getRobotCommand()}}; - } - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - // ボールに近いロボットを1台選択 - auto selected_robots = this->getSelectedRobotsByScore( - 1, selectable_robots, - [this](const std::shared_ptr & robot) { - // ボールに近いほどスコアが高い - return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); - }, - prev_roles); - if (selected_robots.empty()) { - return {}; - } else { - skill = std::make_shared(selected_robots.front(), world_model); - - if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { - skill->setParameter("placement_x", target->x()); - skill->setParameter("placement_y", target->y()); - } - return {selected_robots.front()}; - } - } + const std::unordered_map & prev_roles) -> std::vector override; }; class ReceiverSkillPlanner : public PlannerBase @@ -163,35 +120,11 @@ class ReceiverSkillPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - if (not skill) { - return {PlannerBase::Status::RUNNING, {}}; - } else { - std::vector robot_commands; - auto status = skill->run(visualizer); - return {static_cast(status), {skill->getRobotCommand()}}; - } - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - auto selected = this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - return 100. / world_model->getSquareDistanceFromRobotToBall(robot->id); - }, - prev_roles); - - if (selected.empty()) { - return {}; - } else { - skill = std::make_shared(selected.front(), world_model); - return {selected.front()}; - } - } + const std::unordered_map & prev_roles) -> std::vector override; }; class StealBallSkillPlanner : public PlannerBase @@ -207,54 +140,11 @@ class StealBallSkillPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - if (not skill) { - return {PlannerBase::Status::RUNNING, {}}; - } else { - std::vector robot_commands; - auto status = skill->run(visualizer); - return {static_cast(status), {skill->getRobotCommand()}}; - } - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - auto selected_robots = [&]() { - if (world_model->ball.vel.norm() < 0.5) { - // ボールが遅いときはボールに近いロボットを1台選択 - return this->getSelectedRobotsByScore( - 1, selectable_robots, - [this](const std::shared_ptr & robot) { - // ボールに近いほどスコアが高い - return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); - }, - prev_roles); - } else { - // ボールが速いときはボールラインに近いロボットを1台選択 - return this->getSelectedRobotsByScore( - 1, selectable_robots, - [this](const std::shared_ptr & robot) { - // ボールラインに近いほどスコアが高い - Segment ball_line{ - world_model->ball.pos, - world_model->ball.pos + world_model->ball.vel.normalized() * 10.0}; - ClosestPoint result; - bg::closest_point(robot->pose.pos, ball_line, result); - return 100.0 / std::max(result.distance, 0.01); - }, - prev_roles); - } - }(); - if (selected_robots.empty()) { - return {}; - } else { - skill = std::make_shared(selected_robots.front(), world_model); - return {selected_robots.front()}; - } - } + const std::unordered_map & prev_roles) -> std::vector override; }; class FreeKickSaverSkillPlanner : public PlannerBase @@ -270,35 +160,11 @@ class FreeKickSaverSkillPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - if (not skill) { - return {PlannerBase::Status::RUNNING, {}}; - } else { - std::vector robot_commands; - auto status = skill->run(visualizer); - return {static_cast(status), {skill->getRobotCommand()}}; - } - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - auto selected = this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - return 100. / world_model->getSquareDistanceFromRobotToBall(robot->id); - }, - prev_roles); - - if (selected.empty()) { - return {}; - } else { - skill = std::make_shared(selected.front(), world_model); - return {selected.front()}; - } - } + const std::unordered_map & prev_roles) -> std::vector override; }; class SimpleKickOffSkillPlanner : public PlannerBase @@ -314,35 +180,11 @@ class SimpleKickOffSkillPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - if (not skill) { - return {PlannerBase::Status::RUNNING, {}}; - } else { - std::vector robot_commands; - auto status = skill->run(visualizer); - return {static_cast(status), {skill->getRobotCommand()}}; - } - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - auto selected = this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - return 100. / world_model->getSquareDistanceFromRobotToBall(robot->id); - }, - prev_roles); - - if (selected.empty()) { - return {}; - } else { - skill = std::make_shared(selected.front(), world_model); - return {selected.front()}; - } - } + const std::unordered_map & prev_roles) -> std::vector override; }; } // namespace crane #endif // CRANE_PLANNER_PLUGINS__SKILL_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp index 46e126d63..6fb6160f0 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp @@ -41,55 +41,11 @@ class TheirPenaltyKickPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - std::vector robot_commands; - - for (auto & robot_command : other_robots) { - // 関係ないロボットはボールより1m以上下がる(ルール5.3.5.3) - Point target{}; - target << (world_model->getTheirGoalCenter().x() + world_model->ball.pos.x()) / 2, - robot_command->robot->pose.pos.y(); - robot_command->setTargetPosition(target); - robot_command->disableGoalAreaAvoidance(); - robot_command->disableRuleAreaAvoidance(); - robot_command->setMaxVelocity(1.5); - robot_commands.push_back(robot_command->getMsg()); - } - if (goalie) { - if ( - world_model->play_situation.getSituationCommandID() == - crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION) { - goalie->commander()->setTargetPosition(world_model->getOurGoalCenter()); - goalie->commander()->lookAtBall(); - goalie->commander()->setMaxVelocity(1.5); - } else { - auto status = goalie->run(visualizer); - } - robot_commands.emplace_back(goalie->getRobotCommand()); - } - return {PlannerBase::Status::RUNNING, robot_commands}; - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - goalie = std::make_shared(world_model->getOurGoalieId(), world_model); - auto robots_sorted = this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [&](const std::shared_ptr & robot) { - // ボールに近いほうが先頭 - return 100. / robot->getDistance(world_model->ball.pos); - }, - prev_roles); - for (auto it = robots_sorted.begin(); it != robots_sorted.end(); it++) { - if (*it != world_model->getOurGoalieId()) { - other_robots.emplace_back(std::make_shared(*it, world_model)); - } - } - return robots_sorted; - } + const std::unordered_map & prev_roles) -> std::vector override; }; } // namespace crane #endif // CRANE_PLANNER_PLUGINS__THEIR_PENALTY_KICK_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/tigers_goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/tigers_goalie_planner.hpp index 24ac72f4e..a495ff098 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/tigers_goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/tigers_goalie_planner.hpp @@ -48,140 +48,7 @@ class TigersGoaliePlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - auto robot = world_model->getRobot(robots.front()); - crane::RobotCommandWrapper command(robot->id, world_model); - switch (state) { - case State::STOP: - // KeeperStoppedState - if (not isStopped()) { - state = State::DEFEND; - } - break; - case State::PREPARE_PENALTY: - // PreparePenaltyState - if (not isPreparePenalty()) { - state = State::DEFEND; - } - break; - case State::MOVE_TO_PENALTY_AREA: { - // MoveToPenaltyAreaState - Status status; - if (status == Status::SUCCESS) { - state = State::DEFEND; - } else if (isKeeperWellInsidePenaltyArea()) { - state = State::DEFEND; - } else if (isStopped()) { - state = State::STOP; - } else if (isPreparePenalty()) { - state = State::PREPARE_PENALTY; - } - break; - } - case State::DEFEND: { - auto status = doCriticalKeeper(robot, command); - if (ballCanBePassedOutOfPenaltyArea()) { - state = State::PASS; - } else if (canGoOut()) { - state = State::RAMBO; - } else if (isBallBetweenGoalieAndGoal()) { - state = State::GET_BALL_CONTACT; - } else if (isOutsidePenaltyArea()) { - state = State::MOVE_TO_PENALTY_AREA; - } else if (isStopped()) { - state = State::STOP; - } else if (isPreparePenalty()) { - state = State::PREPARE_PENALTY; - } else if (canInterceptSafely()) { - state = State::INTERCEPT; - } - break; - } - case State::PASS: { - // PassState - if (isBallMoving()) { - state = State::DEFEND; - } else if (isBallPlacementRequired()) { - state = State::MOVE_IN_FRONT_OF_BALL; - } else if (isStopped()) { - state = State::STOP; - } else if (isPreparePenalty()) { - state = State::PREPARE_PENALTY; - } - break; - } - case State::INTERCEPT: { - // InterceptRollingBallState - if (hasInterceptionFailed(robot)) { - state = State::DEFEND; - } else if (ballCanBePassedOutOfPenaltyArea()) { - state = State::PASS; - } else if (isStopped()) { - state = State::STOP; - } else if (isPreparePenalty()) { - state = State::PREPARE_PENALTY; - } - break; - } - case State::RAMBO: { - // RamboKeeper - if (world_model->isDefenseArea(world_model->ball.pos) or isGoalKick()) { - state = State::DEFEND; - } else if (isStopped()) { - state = State::STOP; - } else if (isPreparePenalty()) { - state = State::PREPARE_PENALTY; - } - break; - } - case State::MOVE_IN_FRONT_OF_BALL: { - // MoveInFrontOfBallState - Status status; - if (isBallMoving()) { - state = State::DEFEND; - } else if (isBallPlaced()) { - state = State::DEFEND; - } else if (status == Status::SUCCESS) { - state = State::GET_BALL_CONTACT; - } else if (isStopped()) { - state = State::STOP; - } else if (isPreparePenalty()) { - state = State::PREPARE_PENALTY; - } - break; - } - case State::GET_BALL_CONTACT: { - // doGetBallContact - Status status; - if (status == Status::SUCCESS) { - state = State::MOVE_WITH_BALL; - } else if (status == Status::FAILURE) { - state = State::MOVE_IN_FRONT_OF_BALL; - } else if (isStopped()) { - state = State::STOP; - } else if (isPreparePenalty()) { - state = State::PREPARE_PENALTY; - } - break; - } - case State::MOVE_WITH_BALL: { - // MoveWithBallState - Status status; - if (status == Status::SUCCESS) { - state = State::DEFEND; - } else if (status == Status::FAILURE) { - state = State::MOVE_IN_FRONT_OF_BALL; - } else if (isStopped()) { - state = State::STOP; - } else if (isPreparePenalty()) { - state = State::PREPARE_PENALTY; - } - break; - } - } - return {PlannerBase::Status::RUNNING, {}}; - } + const std::vector & robots) override; Status doCriticalKeeper(const std::shared_ptr & robot, RobotCommandWrapper & command) { @@ -238,16 +105,7 @@ class TigersGoaliePlanner : public PlannerBase auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - return this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - // choose id smaller first - return 15. - static_cast(-robot->id); - }, - prev_roles); - } + const std::unordered_map & prev_roles) -> std::vector override; State state = State::DEFEND; }; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp index 4e4983138..9e04236c6 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/waiter_planner.hpp @@ -34,42 +34,14 @@ class WaiterPlanner : public PlannerBase } std::pair> calculateRobotCommand( - const std::vector & robots) override - { - std::vector robot_commands; - for (auto robot_id : robots) { - crane::RobotCommandWrapper target(robot_id.robot_id, world_model); - target.stopHere(); - target.setVelocity(0., 0.); - if (target.robot->vel.linear.norm() < 0.5) { - target.stopEmergency(); - } - robot_commands.emplace_back(target.getMsg()); - } - return {PlannerBase::Status::RUNNING, robot_commands}; - } + const std::vector & robots) override; auto getSelectedRobots( uint8_t selectable_robots_num, const std::vector & selectable_robots, - const std::unordered_map & prev_roles) -> std::vector override - { - auto selected = this->getSelectedRobotsByScore( - selectable_robots_num, selectable_robots, - [this](const std::shared_ptr & robot) { - // choose id smaller first - return 15. - static_cast(-robot->id); - }, - prev_roles); - for (auto robot : selected) { - stop_poses.emplace(robot, world_model->getOurRobot(robot)->pose); - } - return selected; - } + const std::unordered_map & prev_roles) -> std::vector override; private: - // rclcpp::TimerBase::SharedPtr timer; std::unordered_map stop_poses; }; - } // namespace crane #endif // CRANE_PLANNER_PLUGINS__WAITER_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/src/attacker_planner.cpp b/session/crane_planner_plugins/src/attacker_planner.cpp index 6b7f1bf54..637b65821 100644 --- a/session/crane_planner_plugins/src/attacker_planner.cpp +++ b/session/crane_planner_plugins/src/attacker_planner.cpp @@ -46,4 +46,27 @@ AttackerPlanner::calculateRobotCommand(const std::vector & robo auto status = attacker_->run(visualizer); return {static_cast(status), {attacker_->getRobotCommand()}}; } + +auto AttackerPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + auto selected = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + // ボールに近いほどスコアが高い + return 100.0 - std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); + }, + prev_roles, + [this](const std::shared_ptr & robot) { + // ヒステリシスは1m + return 1.; + }); + if (selected.empty()) { + return {}; + } else { + attacker_ = std::make_shared(selected.front(), world_model); + return {selected.front()}; + } +} } // namespace crane diff --git a/session/crane_planner_plugins/src/catch_ball_planner.cpp b/session/crane_planner_plugins/src/catch_ball_planner.cpp new file mode 100644 index 000000000..c4e7e1b88 --- /dev/null +++ b/session/crane_planner_plugins/src/catch_ball_planner.cpp @@ -0,0 +1,128 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane +{ +std::pair> +CatchBallPlanner::calculateRobotCommand(const std::vector & robots) +{ + std::vector commands; + for (const auto & robot : robots) { + crane::RobotCommandWrapper target(robot.robot_id, world_model); + + Point target_point = default_point; + auto ball = world_model->ball.pos; + + // シュートチェック + Vector2 norm_vec = getVerticalVec(getNormVec(target.robot->pose.theta)) * 0.8; + Segment receive_line(target.robot->pose.pos + norm_vec, target.robot->pose.pos - norm_vec); + Segment ball_line(ball, ball + world_model->ball.vel.normalized() * 20.f); + auto intersections = + getIntersections(ball_line, Segment{receive_line.first, receive_line.second}); + + if (not intersections.empty() && world_model->ball.vel.norm() > 0.3f) { + // シュートブロック + std::cout << "シュートブロック" << std::endl; + auto result = getClosestPointAndDistance(ball_line, target.robot->pose.pos); + target.setTargetPosition(result.closest_point); + target.setTargetTheta(getAngle(-world_model->ball.vel)); + if (target.robot->getDistance(result.closest_point) > 0.2) { + target.setTerminalVelocity(2.0); + } + } else { + if (world_model->ball.isStopped(0.3) && not world_model->isFriendDefenseArea(ball)) { + // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す + std::cout << "ボール排出" << std::endl; + // パスできるロボットのリストアップ + auto passable_robot_list = world_model->ours.getAvailableRobots(target.robot->id); + passable_robot_list.erase( + std::remove_if( + passable_robot_list.begin(), passable_robot_list.end(), + [&](const RobotInfo::SharedPtr & r) { + // 敵に塞がれていたら除外 + Segment ball_to_robot_line(ball, r->pose.pos); + for (const auto & enemy : world_model->theirs.getAvailableRobots()) { + auto dist = bg::distance(ball_to_robot_line, enemy->pose.pos); + if (dist < 0.2) { + return true; + } + } + return false; + }), + passable_robot_list.end()); + + std::cout << "パスターゲット: "; + for (auto a : passable_robot_list) { + std::cout << static_cast(a->id) << ","; + } + std::cout << std::endl; + + Point pass_target = [&]() { + if (not passable_robot_list.empty()) { + // TODO(HansRobo): いい感じのロボットを選ぶようにする + return passable_robot_list.front()->pose.pos; + } else { + return Point{0, 0}; + } + }(); + + std::cout << pass_target.x() << ", " << pass_target.y() << std::endl; + Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f; + double angle_ball_to_target = getAngle(pass_target - ball); + double dot = (world_model->ball.pos - target.robot->pose.pos) + .normalized() + .dot((pass_target - world_model->ball.pos).normalized()); + // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 + if ( + dot < 0.9 || + std::abs(getAngleDiff(angle_ball_to_target, target.robot->pose.theta)) > 0.1) { + std::cout << "中間ポイント経由" << std::endl; + target.setTargetPosition(intermediate_point); + target.enableCollisionAvoidance(); + } else { + std::cout << "ボール突撃" << std::endl; + target.setTargetPosition(world_model->ball.pos); + target.liftUpDribbler(); + target.kickStraight(0.1).disableCollisionAvoidance(); + target.enableCollisionAvoidance(); + target.disableBallAvoidance(); + } + target.setTargetTheta(getAngle(pass_target - ball)); + target.disableGoalAreaAvoidance(); + } else { + // phase = ""; + const double BLOCK_DIST = 0.15; + // 範囲外のときは正面に構える + if (not world_model->isFieldInside(ball)) { + // phase += "正面で"; + ball << 0, 0; + } + + // phase = "ボールを待ち受ける"; + target.setTargetPosition(default_point); + target.lookAtBall(); + } + } + + commands.push_back(target.getMsg()); + } + return {PlannerBase::Status::RUNNING, commands}; +} + +auto crane::CatchBallPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + return this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + return 100. / world_model->getSquareDistanceFromRobot(robot->id, default_point); + }, + prev_roles); +} +} // namespace crane diff --git a/session/crane_planner_plugins/src/defender_planner.cpp b/session/crane_planner_plugins/src/defender_planner.cpp index 785b4c5d7..8b73303ba 100644 --- a/session/crane_planner_plugins/src/defender_planner.cpp +++ b/session/crane_planner_plugins/src/defender_planner.cpp @@ -11,6 +11,10 @@ namespace crane std::pair> DefenderPlanner::calculateRobotCommand(const std::vector & robots) { + if (robots.empty()) { + return {PlannerBase::Status::RUNNING, {}}; + } + auto ball = world_model->ball.pos; const double OFFSET_X = 0.1; const double OFFSET_Y = 0.1; @@ -23,8 +27,7 @@ DefenderPlanner::calculateRobotCommand(const std::vector & robo // シュート判定 auto goal_posts = world_model->getOurGoalPosts(); Segment goal_line(goal_posts.first, goal_posts.second); - std::vector intersections; - bg::intersection(ball_line, goal_line, intersections); + auto intersections = getIntersections(ball_line, goal_line); if (intersections.empty()) { // シュートがなければ通常の動き ball_line.first = world_model->goal; @@ -32,28 +35,19 @@ DefenderPlanner::calculateRobotCommand(const std::vector & robo } } - auto first_defenders = robots; - if (second_threat_defender) { - first_defenders.erase( - std::remove_if( - first_defenders.begin(), first_defenders.end(), - [&](const auto & robot_id) { return robot_id.robot_id == second_threat_defender.value(); }), - first_defenders.end()); - } - - std::vector defense_points = getDefensePoints(first_defenders.size(), ball_line); + std::vector defense_points = getDefensePoints(robots.size(), ball_line); if (not defense_points.empty()) { std::vector robot_points; - for (auto robot_id : first_defenders) { + for (auto robot_id : robots) { robot_points.emplace_back(world_model->getRobot(robot_id)->pose.pos); } auto solution = getOptimalAssignments(robot_points, defense_points); std::vector robot_commands; - for (auto robot_id = first_defenders.begin(); robot_id != first_defenders.end(); ++robot_id) { - int index = std::distance(first_defenders.begin(), robot_id); + for (auto robot_id = robots.begin(); robot_id != robots.end(); ++robot_id) { + int index = std::distance(robots.begin(), robot_id); Point target_point = defense_points[index]; crane::RobotCommandWrapper target(robot_id->robot_id, world_model); @@ -66,40 +60,11 @@ DefenderPlanner::calculateRobotCommand(const std::vector & robo robot_commands.emplace_back(target.getMsg()); } - - if (second_threat_defender) { - auto ball_handler = world_model->getNearestRobotsWithDistanceFromPoint( - world_model->ball.pos, world_model->theirs.getAvailableRobots()); - auto enemy_robots = world_model->theirs.getAvailableRobots(ball_handler.first->id); - if (enemy_robots.empty()) { - crane::RobotCommandWrapper target(second_threat_defender.value(), world_model); - target.stopHere(); - robot_commands.emplace_back(target.getMsg()); - } else { - double max_goal_width = 0.; - uint8_t second_threat_bot_id = 0; - for (const auto enemy : enemy_robots) { - double goal_width = - world_model->getLargestOurGoalAngleRangeFromPoint(enemy->pose.pos).second; - if (goal_width > max_goal_width) { - max_goal_width = goal_width; - second_threat_bot_id = enemy->id; - } - } - auto second_threat_bot = world_model->getRobot({false, second_threat_bot_id}); - Point mark_point = second_threat_bot->pose.pos + - (world_model->goal - second_threat_bot->pose.pos).normalized() * 0.3; - crane::RobotCommandWrapper target(second_threat_defender.value(), world_model); - target.setTargetPosition(mark_point); - target.setTargetTheta(getAngle(second_threat_bot->pose.pos - mark_point)); - robot_commands.emplace_back(target.getMsg()); - } - } return {PlannerBase::Status::RUNNING, robot_commands}; } else { std::vector robot_commands; - for (auto robot_id = first_defenders.begin(); robot_id != first_defenders.end(); ++robot_id) { - int index = std::distance(first_defenders.begin(), robot_id); + for (auto robot_id = robots.begin(); robot_id != robots.end(); ++robot_id) { + int index = std::distance(robots.begin(), robot_id); Point target_point = defense_points[index]; crane::RobotCommandWrapper target(robot_id->robot_id, world_model); diff --git a/session/crane_planner_plugins/src/formation_planner.cpp b/session/crane_planner_plugins/src/formation_planner.cpp index 267414adf..5255be24e 100644 --- a/session/crane_planner_plugins/src/formation_planner.cpp +++ b/session/crane_planner_plugins/src/formation_planner.cpp @@ -60,4 +60,17 @@ FormationPlanner::calculateRobotCommand(const std::vector & rob } return {PlannerBase::Status::RUNNING, robot_commands}; } + +auto FormationPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + return this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + // choose id smaller first + return 15. - static_cast(-robot->id); + }, + prev_roles); +} } // namespace crane diff --git a/session/crane_planner_plugins/src/receive_planner.cpp b/session/crane_planner_plugins/src/receive_planner.cpp index dd3d77299..83c396069 100644 --- a/session/crane_planner_plugins/src/receive_planner.cpp +++ b/session/crane_planner_plugins/src/receive_planner.cpp @@ -19,4 +19,22 @@ ReceivePlanner::calculateRobotCommand(const std::vector & robot return {static_cast(status), {receiver_skill->getRobotCommand()}}; } } + +auto ReceivePlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + auto selected = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + return 100. / world_model->getSquareDistanceFromRobotToBall(robot->id); + }, + prev_roles); + if (selected.empty()) { + return {}; + } else { + receiver_skill = std::make_shared(selected.front(), world_model); + return {selected.front()}; + } +} } // namespace crane diff --git a/session/crane_planner_plugins/src/skill_planners.cpp b/session/crane_planner_plugins/src/skill_planners.cpp new file mode 100644 index 000000000..d1f5a3927 --- /dev/null +++ b/session/crane_planner_plugins/src/skill_planners.cpp @@ -0,0 +1,205 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane +{ +std::pair> +GoalieSkillPlanner::calculateRobotCommand(const std::vector & robots) +{ + if (not skill) { + return {PlannerBase::Status::RUNNING, {}}; + } else { + std::vector robot_commands; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; + } +} +std::pair> + +BallPlacementSkillPlanner::calculateRobotCommand(const std::vector & robots) +{ + if (not skill) { + return {PlannerBase::Status::RUNNING, {}}; + } else { + if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { + skill->setParameter("placement_x", target->x()); + skill->setParameter("placement_y", target->y()); + } + std::vector robot_commands; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; + } +} + +auto BallPlacementSkillPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + // ボールに近いロボットを1台選択 + auto selected_robots = this->getSelectedRobotsByScore( + 1, selectable_robots, + [this](const std::shared_ptr & robot) { + // ボールに近いほどスコアが高い + return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); + }, + prev_roles); + if (selected_robots.empty()) { + return {}; + } else { + skill = std::make_shared(selected_robots.front(), world_model); + + if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { + skill->setParameter("placement_x", target->x()); + skill->setParameter("placement_y", target->y()); + } + return {selected_robots.front()}; + } +} + +std::pair> +ReceiverSkillPlanner::calculateRobotCommand(const std::vector & robots) +{ + if (not skill) { + return {PlannerBase::Status::RUNNING, {}}; + } else { + std::vector robot_commands; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; + } +} + +auto ReceiverSkillPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + auto selected = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + return 100. / world_model->getSquareDistanceFromRobotToBall(robot->id); + }, + prev_roles); + + if (selected.empty()) { + return {}; + } else { + skill = std::make_shared(selected.front(), world_model); + return {selected.front()}; + } +} + +std::pair> +StealBallSkillPlanner::calculateRobotCommand(const std::vector & robots) +{ + if (not skill) { + return {PlannerBase::Status::RUNNING, {}}; + } else { + std::vector robot_commands; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; + } +} + +auto StealBallSkillPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + auto selected_robots = [&]() { + if (world_model->ball.vel.norm() < 0.5) { + // ボールが遅いときはボールに近いロボットを1台選択 + return this->getSelectedRobotsByScore( + 1, selectable_robots, + [this](const std::shared_ptr & robot) { + // ボールに近いほどスコアが高い + return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); + }, + prev_roles); + } else { + // ボールが速いときはボールラインに近いロボットを1台選択 + return this->getSelectedRobotsByScore( + 1, selectable_robots, + [this](const std::shared_ptr & robot) { + // ボールラインに近いほどスコアが高い + Segment ball_line{ + world_model->ball.pos, + world_model->ball.pos + world_model->ball.vel.normalized() * 10.0}; + return 100.0 / + std::max(getClosestPointAndDistance(robot->pose.pos, ball_line).distance, 0.01); + }, + prev_roles); + } + }(); + if (selected_robots.empty()) { + return {}; + } else { + skill = std::make_shared(selected_robots.front(), world_model); + return {selected_robots.front()}; + } +} + +std::pair> +FreeKickSaverSkillPlanner::calculateRobotCommand(const std::vector & robots) +{ + if (not skill) { + return {PlannerBase::Status::RUNNING, {}}; + } else { + std::vector robot_commands; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; + } +} + +auto FreeKickSaverSkillPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + auto selected = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + return 100. / world_model->getSquareDistanceFromRobotToBall(robot->id); + }, + prev_roles); + + if (selected.empty()) { + return {}; + } else { + skill = std::make_shared(selected.front(), world_model); + return {selected.front()}; + } +} + +std::pair> +SimpleKickOffSkillPlanner::calculateRobotCommand(const std::vector & robots) +{ + if (not skill) { + return {PlannerBase::Status::RUNNING, {}}; + } else { + std::vector robot_commands; + auto status = skill->run(visualizer); + return {static_cast(status), {skill->getRobotCommand()}}; + } +} + +auto SimpleKickOffSkillPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + auto selected = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + return 100. / world_model->getSquareDistanceFromRobotToBall(robot->id); + }, + prev_roles); + + if (selected.empty()) { + return {}; + } else { + skill = std::make_shared(selected.front(), world_model); + return {selected.front()}; + } +} +} // namespace crane diff --git a/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp b/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp new file mode 100644 index 000000000..98570ed30 --- /dev/null +++ b/session/crane_planner_plugins/src/their_penalty_kick_planner.cpp @@ -0,0 +1,62 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane +{ + +std::pair> +TheirPenaltyKickPlanner::calculateRobotCommand(const std::vector & robots) +{ + std::vector robot_commands; + + for (auto & robot_command : other_robots) { + // 関係ないロボットはボールより1m以上下がる(ルール5.3.5.3) + Point target{}; + target << (world_model->getTheirGoalCenter().x() + world_model->ball.pos.x()) / 2, + robot_command->robot->pose.pos.y(); + robot_command->setTargetPosition(target); + robot_command->disableGoalAreaAvoidance(); + robot_command->disableRuleAreaAvoidance(); + robot_command->setMaxVelocity(1.5); + robot_commands.push_back(robot_command->getMsg()); + } + if (goalie) { + if ( + world_model->play_situation.getSituationCommandID() == + crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION) { + goalie->commander()->setTargetPosition(world_model->getOurGoalCenter()); + goalie->commander()->lookAtBall(); + goalie->commander()->setMaxVelocity(1.5); + } else { + auto status = goalie->run(visualizer); + } + robot_commands.emplace_back(goalie->getRobotCommand()); + } + return {PlannerBase::Status::RUNNING, robot_commands}; +} + +auto TheirPenaltyKickPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + goalie = std::make_shared(world_model->getOurGoalieId(), world_model); + auto robots_sorted = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [&](const std::shared_ptr & robot) { + // ボールに近いほうが先頭 + return 100. / robot->getDistance(world_model->ball.pos); + }, + prev_roles); + for (auto it = robots_sorted.begin(); it != robots_sorted.end(); it++) { + if (*it != world_model->getOurGoalieId()) { + other_robots.emplace_back(std::make_shared(*it, world_model)); + } + } + return robots_sorted; +} +} // namespace crane diff --git a/session/crane_planner_plugins/src/tigers_goalie_planner.cpp b/session/crane_planner_plugins/src/tigers_goalie_planner.cpp new file mode 100644 index 000000000..305693477 --- /dev/null +++ b/session/crane_planner_plugins/src/tigers_goalie_planner.cpp @@ -0,0 +1,159 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane +{ +std::pair> +TigersGoaliePlanner::calculateRobotCommand(const std::vector & robots) +{ + auto robot = world_model->getRobot(robots.front()); + crane::RobotCommandWrapper command(robot->id, world_model); + switch (state) { + case State::STOP: + // KeeperStoppedState + if (not isStopped()) { + state = State::DEFEND; + } + break; + case State::PREPARE_PENALTY: + // PreparePenaltyState + if (not isPreparePenalty()) { + state = State::DEFEND; + } + break; + case State::MOVE_TO_PENALTY_AREA: { + // MoveToPenaltyAreaState + Status status; + if (status == Status::SUCCESS) { + state = State::DEFEND; + } else if (isKeeperWellInsidePenaltyArea()) { + state = State::DEFEND; + } else if (isStopped()) { + state = State::STOP; + } else if (isPreparePenalty()) { + state = State::PREPARE_PENALTY; + } + break; + } + case State::DEFEND: { + auto status = doCriticalKeeper(robot, command); + if (ballCanBePassedOutOfPenaltyArea()) { + state = State::PASS; + } else if (canGoOut()) { + state = State::RAMBO; + } else if (isBallBetweenGoalieAndGoal()) { + state = State::GET_BALL_CONTACT; + } else if (isOutsidePenaltyArea()) { + state = State::MOVE_TO_PENALTY_AREA; + } else if (isStopped()) { + state = State::STOP; + } else if (isPreparePenalty()) { + state = State::PREPARE_PENALTY; + } else if (canInterceptSafely()) { + state = State::INTERCEPT; + } + break; + } + case State::PASS: { + // PassState + if (isBallMoving()) { + state = State::DEFEND; + } else if (isBallPlacementRequired()) { + state = State::MOVE_IN_FRONT_OF_BALL; + } else if (isStopped()) { + state = State::STOP; + } else if (isPreparePenalty()) { + state = State::PREPARE_PENALTY; + } + break; + } + case State::INTERCEPT: { + // InterceptRollingBallState + if (hasInterceptionFailed(robot)) { + state = State::DEFEND; + } else if (ballCanBePassedOutOfPenaltyArea()) { + state = State::PASS; + } else if (isStopped()) { + state = State::STOP; + } else if (isPreparePenalty()) { + state = State::PREPARE_PENALTY; + } + break; + } + case State::RAMBO: { + // RamboKeeper + if (world_model->isDefenseArea(world_model->ball.pos) or isGoalKick()) { + state = State::DEFEND; + } else if (isStopped()) { + state = State::STOP; + } else if (isPreparePenalty()) { + state = State::PREPARE_PENALTY; + } + break; + } + case State::MOVE_IN_FRONT_OF_BALL: { + // MoveInFrontOfBallState + Status status; + if (isBallMoving()) { + state = State::DEFEND; + } else if (isBallPlaced()) { + state = State::DEFEND; + } else if (status == Status::SUCCESS) { + state = State::GET_BALL_CONTACT; + } else if (isStopped()) { + state = State::STOP; + } else if (isPreparePenalty()) { + state = State::PREPARE_PENALTY; + } + break; + } + case State::GET_BALL_CONTACT: { + // doGetBallContact + Status status; + if (status == Status::SUCCESS) { + state = State::MOVE_WITH_BALL; + } else if (status == Status::FAILURE) { + state = State::MOVE_IN_FRONT_OF_BALL; + } else if (isStopped()) { + state = State::STOP; + } else if (isPreparePenalty()) { + state = State::PREPARE_PENALTY; + } + break; + } + case State::MOVE_WITH_BALL: { + // MoveWithBallState + Status status; + if (status == Status::SUCCESS) { + state = State::DEFEND; + } else if (status == Status::FAILURE) { + state = State::MOVE_IN_FRONT_OF_BALL; + } else if (isStopped()) { + state = State::STOP; + } else if (isPreparePenalty()) { + state = State::PREPARE_PENALTY; + } + break; + } + } + return {PlannerBase::Status::RUNNING, {}}; +} + +auto TigersGoaliePlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + return this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + // choose id smaller first + return 15. - static_cast(-robot->id); + }, + prev_roles); +} +} // namespace crane diff --git a/session/crane_planner_plugins/src/waiter_planner.cpp b/session/crane_planner_plugins/src/waiter_planner.cpp new file mode 100644 index 000000000..2728bf5b9 --- /dev/null +++ b/session/crane_planner_plugins/src/waiter_planner.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include + +namespace crane +{ +std::pair> +WaiterPlanner::calculateRobotCommand(const std::vector & robots) +{ + std::vector robot_commands; + for (auto robot_id : robots) { + crane::RobotCommandWrapper target(robot_id.robot_id, world_model); + target.stopHere(); + target.setVelocity(0., 0.); + if (target.robot->vel.linear.norm() < 0.5) { + target.stopEmergency(); + } + robot_commands.emplace_back(target.getMsg()); + } + return {PlannerBase::Status::RUNNING, robot_commands}; +} + +auto WaiterPlanner::getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots, + const std::unordered_map & prev_roles) -> std::vector +{ + auto selected = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, + [this](const std::shared_ptr & robot) { + // choose id smaller first + return 15. - static_cast(-robot->id); + }, + prev_roles); + for (auto robot : selected) { + stop_poses.emplace(robot, world_model->getOurRobot(robot)->pose); + } + return selected; +} +} // namespace crane diff --git a/session/crane_session_controller/CMakeLists.txt b/session/crane_session_controller/CMakeLists.txt index 06c04ad05..786490fd0 100755 --- a/session/crane_session_controller/CMakeLists.txt +++ b/session/crane_session_controller/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_session_controller) -# Default to C++17 +# Default to C++20 if (NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index 32367f78d..9bb947983 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -206,6 +206,36 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions msg.on_positive_half = world_model->onPositiveHalf(); msg.is_yellow = world_model->isYellow(); try { + // ロボットが過不足なく割り当てられているか確認 + bool robot_changed = [&]() { + std::vector assigned_robot_ids; + for (const auto & planner : available_planners) { + for (const auto & robot : planner->getRobots()) { + assigned_robot_ids.push_back(robot.robot_id); + } + } + std::sort(assigned_robot_ids.begin(), assigned_robot_ids.end()); + + std::vector observed_robot_ids = world_model->ours.getAvailableRobotIds(); + std::sort(observed_robot_ids.begin(), observed_robot_ids.end()); + + if (assigned_robot_ids.size() != observed_robot_ids.size()) { + return true; + } else { + for (size_t i = 0; i < assigned_robot_ids.size(); i++) { + if (assigned_robot_ids[i] != observed_robot_ids[i]) { + return true; + } + } + return false; + } + }(); + + if (robot_changed) { + RCLCPP_INFO(get_logger(), "ロボットの数か変動していますので再割当を行います"); + request(play_situation.getSituationCommandText(), world_model->ours.getAvailableRobotIds()); + } + for (const auto & planner : available_planners) { auto commands_msg = planner->getRobotCommands(); msg.robot_commands.insert( diff --git a/utility/crane_clock_publisher/CMakeLists.txt b/utility/crane_clock_publisher/CMakeLists.txt index 612cb4b9a..4bbe3651d 100755 --- a/utility/crane_clock_publisher/CMakeLists.txt +++ b/utility/crane_clock_publisher/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_clock_publisher) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/utility/crane_geometry/CMakeLists.txt b/utility/crane_geometry/CMakeLists.txt index d69c158db..5ded80e29 100755 --- a/utility/crane_geometry/CMakeLists.txt +++ b/utility/crane_geometry/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_geometry) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp b/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp index 6405122bd..c132b226b 100644 --- a/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp +++ b/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp @@ -7,22 +7,24 @@ #ifndef CRANE_GEOMETRY__GEOMETRY_OPERATIONS_HPP_ #define CRANE_GEOMETRY__GEOMETRY_OPERATIONS_HPP_ +#include + #include "boost_geometry.hpp" namespace crane { -inline bool isInBox(const Box & box, const Point & p) { return bg::within(p, box); } +inline auto isInBox(const Box & box, const Point & p) -> bool { return bg::within(p, box); } -inline bool isInBox(Box box, const Point & p, const double offset) +inline auto isInBox(Box box, const Point & p, const double offset) -> bool { box.max_corner() += Point(offset, offset); box.min_corner() -= Point(offset, offset); return bg::within(p, box); } -inline double getAngle(const Vector2 & vec) { return atan2(vec.y(), vec.x()); } +inline auto getAngle(const Vector2 & vec) -> double { return atan2(vec.y(), vec.x()); } -inline double normalizeAngle(double angle_rad) +inline auto normalizeAngle(double angle_rad) -> double { while (angle_rad > M_PI) { angle_rad -= 2.0f * M_PI; @@ -33,7 +35,7 @@ inline double normalizeAngle(double angle_rad) return angle_rad; } -inline double getAngleDiff(double angle_rad1, double angle_rad2) +inline auto getAngleDiff(double angle_rad1, double angle_rad2) -> double { angle_rad1 = normalizeAngle(angle_rad1); angle_rad2 = normalizeAngle(angle_rad2); @@ -48,22 +50,22 @@ inline double getAngleDiff(double angle_rad1, double angle_rad2) } } -inline double getAngleDiff(Pose2D pose1, Pose2D pose2) +inline auto getAngleDiff(Pose2D pose1, Pose2D pose2) -> double { return getAngleDiff(pose1.theta, pose2.theta); } -inline double getAngleDiff(Pose2D pose1, double angle_rad) +inline auto getAngleDiff(Pose2D pose1, double angle_rad) -> double { return getAngleDiff(pose1.theta, angle_rad); } -inline double getAngleDiff(double angle_rad, Pose2D pose1) +inline auto getAngleDiff(double angle_rad, Pose2D pose1) -> double { return getAngleDiff(angle_rad, pose1.theta); } -inline double getIntermediateAngle(double angle_rad1, double angle_rad2) +inline auto getIntermediateAngle(double angle_rad1, double angle_rad2) -> double { angle_rad1 = normalizeAngle(angle_rad1); angle_rad2 = normalizeAngle(angle_rad2); @@ -75,16 +77,16 @@ inline double getIntermediateAngle(double angle_rad1, double angle_rad2) } } -inline Vector2 getNormVec(const double angle) { return {cos(angle), sin(angle)}; } +inline auto getNormVec(const double angle) -> Vector2 { return {cos(angle), sin(angle)}; } -inline Point getVerticalVec(Point v) +inline auto getVerticalVec(Point v) -> Point { Point vertical_v; vertical_v << v.y(), -v.x(); return vertical_v; } -inline double getReachTime(double distance, double v0, double acc, double max_vel) +inline auto getReachTime(double distance, double v0, double acc, double max_vel) -> double { // x = v0*t + 1/2*a*t^2 より double t = (sqrt(v0 * v0 + 2.0f * acc * distance) - v0) / acc; @@ -99,6 +101,22 @@ inline double getReachTime(double distance, double v0, double acc, double max_ve } } } + +inline auto getIntersections(const Segment & line1, const Segment & line2) -> std::vector +{ + std::vector intersections; + bg::intersection(line1, line2, intersections); + return intersections; +} + +template +inline auto getClosestPointAndDistance(const Geometry1 & geometry1, const Geometry2 & geometry2) + -> ClosestPoint +{ + ClosestPoint result; + bg::closest_point(geometry1, geometry2, result); + return result; +} } // namespace crane #endif // CRANE_GEOMETRY__GEOMETRY_OPERATIONS_HPP_ diff --git a/utility/crane_grsim_operator/CMakeLists.txt b/utility/crane_grsim_operator/CMakeLists.txt index 5c6a8ef96..45766f9af 100755 --- a/utility/crane_grsim_operator/CMakeLists.txt +++ b/utility/crane_grsim_operator/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_grsim_operator) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/utility/crane_msg_wrappers/CMakeLists.txt b/utility/crane_msg_wrappers/CMakeLists.txt index f55bcfde9..ac8e990d6 100755 --- a/utility/crane_msg_wrappers/CMakeLists.txt +++ b/utility/crane_msg_wrappers/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_msg_wrappers) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -12,13 +12,13 @@ endif() add_compile_options(-fPIC) +add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS") + # find dependencies find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(${PROJECT_NAME} - src/geometry_wrapper.cpp - src/play_situation_wrapper.cpp) +ament_auto_add_library(${PROJECT_NAME} DIRECTORY src) ament_target_dependencies(${PROJECT_NAME} ${requirement_dependencies}) diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index 030a96888..2cf8f21fb 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp @@ -29,19 +29,7 @@ struct BallContact std::chrono::system_clock::time_point last_contact_end_time; std::chrono::system_clock::time_point last_contact_start_time; - void update(bool is_contacted) - { - auto now = std::chrono::system_clock::now(); - if (is_contacted) { - last_contact_end_time = now; - if (not is_contacted_pre_frame) { - last_contact_start_time = now; - } - } else { - last_contact_start_time = last_contact_end_time; - } - is_contacted_pre_frame = is_contacted; - } + void update(bool is_contacted); [[nodiscard]] auto getContactDuration() const { @@ -105,16 +93,7 @@ struct TeamInfo std::vector> robots; - std::vector> getAvailableRobots(uint8_t my_id = 255) - { - std::vector> available_robots; - for (auto robot : robots) { - if (robot->available && robot->id != my_id) { - available_robots.emplace_back(robot); - } - } - return available_robots; - } + std::vector> getAvailableRobots(uint8_t my_id = 255); std::vector getAvailableRobotIds(uint8_t my_id = 255) { @@ -139,18 +118,7 @@ struct Hysteresis std::function upper_callback = []() {}; std::function lower_callback = []() {}; - void update(double value) - { - if (not is_high && value > upper_threshold) { - is_high = true; - upper_callback(); - } - - if (is_high && value < lower_threshold) { - is_high = false; - lower_callback(); - } - } + void update(double value); }; struct Ball @@ -172,15 +140,7 @@ struct Ball } [[nodiscard]] bool isMovingTowards( - const Point & p, double angle_threshold_deg = 60.0, double near_threshold = 0.2) const - { - if ((pos - p).norm() < near_threshold) { - return false; - } else { - auto dir = (p - pos).normalized(); - return dir.dot(vel.normalized()) > cos(angle_threshold_deg * M_PI / 180.0); - } - } + const Point & p, double angle_threshold_deg = 60.0, double near_threshold = 0.2) const; private: Hysteresis ball_speed_hysteresis = Hysteresis(0.1, 0.6); @@ -193,95 +153,9 @@ struct WorldModelWrapper typedef std::unique_ptr UniquePtr; - explicit WorldModelWrapper(rclcpp::Node & node) - { - // メモリ確保 - // ヒトサッカーの台数は超えないはず - constexpr uint8_t MAX_ROBOT_NUM = 20; - for (int i = 0; i < MAX_ROBOT_NUM; i++) { - ours.robots.emplace_back(std::make_shared()); - theirs.robots.emplace_back(std::make_shared()); - } + explicit WorldModelWrapper(rclcpp::Node & node); - subscriber = node.create_subscription( - "/world_model", 10, [this](const crane_msgs::msg::WorldModel::SharedPtr msg) -> void { - latest_msg = *msg; - this->update(*msg); - has_updated = true; - for (auto & callback : callbacks) { - callback(); - } - }); - } - - void update(const crane_msgs::msg::WorldModel & world_model) - { - play_situation.update(world_model.play_situation); - - for (auto & our_robot : ours.robots) { - our_robot->available = false; - } - - for (auto & their_robot : theirs.robots) { - their_robot->available = false; - } - - ball.pos << world_model.ball_info.pose.x, world_model.ball_info.pose.y; - ball.vel << world_model.ball_info.velocity.x, world_model.ball_info.velocity.y; - ball.ball_speed_hysteresis.update(ball.vel.norm()); - - for (auto & robot : world_model.robot_info_ours) { - auto & info = ours.robots.at(robot.id); - info->available = !robot.disappeared; - if (info->available) { - info->id = robot.id; - info->pose.pos << robot.pose.x, robot.pose.y; - info->pose.theta = robot.pose.theta; - info->vel.linear << robot.velocity.x, robot.velocity.y; - info->ball_contact.update((info->kicker_center() - ball.pos).norm() < 0.1); - } else { - info->ball_contact.update(false); - } - } - - for (auto robot : world_model.robot_info_theirs) { - auto & info = theirs.robots.at(robot.id); - info->available = !robot.disappeared; - if (info->available) { - info->id = robot.id; - info->ball_contact.update( - robot.ball_contact.current_time == robot.ball_contact.last_contacted_time); - info->pose.pos << robot.pose.x, robot.pose.y; - info->pose.theta = robot.pose.theta; - info->vel.linear << robot.velocity.x, robot.velocity.y; - // todo : omega - } else { - info->ball_contact.update(false); - } - } - - field_size << world_model.field_info.x, world_model.field_info.y; - defense_area_size << world_model.defense_area_size.x, world_model.defense_area_size.y; - - goal_size << world_model.goal_size.x, world_model.goal_size.y; - goal << getOurSideSign() * field_size.x() * 0.5, 0.; - - if (onPositiveHalf()) { - ours.defense_area.max_corner() << goal.x(), goal.y() + world_model.defense_area_size.y / 2.; - ours.defense_area.min_corner() << goal.x() - world_model.defense_area_size.x, - goal.y() - world_model.defense_area_size.y / 2.; - } else { - ours.defense_area.max_corner() << goal.x() + world_model.defense_area_size.x, - goal.y() + world_model.defense_area_size.y / 2.; - ours.defense_area.min_corner() << goal.x(), goal.y() - world_model.defense_area_size.y / 2.; - } - theirs.defense_area.max_corner() - << std::max(-ours.defense_area.max_corner().x(), -ours.defense_area.min_corner().x()), - ours.defense_area.max_corner().y(); - theirs.defense_area.min_corner() - << std::min(-ours.defense_area.max_corner().x(), -ours.defense_area.min_corner().x()), - ours.defense_area.min_corner().y(); - } + void update(const crane_msgs::msg::WorldModel & world_model); [[nodiscard]] const crane_msgs::msg::WorldModel & getMsg() const { return latest_msg; } @@ -297,7 +171,7 @@ struct WorldModelWrapper [[nodiscard]] bool isTheirBall() const { return latest_msg.ball_info.is_their_ball; } - [[nodiscard]] const bool isBallPossessionStateChanged() const + [[nodiscard]] bool isBallPossessionStateChanged() const { return latest_msg.ball_info.state_changed; } @@ -340,16 +214,7 @@ struct WorldModelWrapper return getSquareDistanceFromRobot({true, our_id}, ball.pos); } - [[nodiscard]] auto generateFieldPoints(float grid_size) const - { - std::vector points; - for (float x = 0.f; x <= field_size.x() / 2.f; x += grid_size) { - for (float y = 0.f; y <= field_size.y() / 2.f; y += grid_size) { - points.emplace_back(x, y); - } - } - return points; - } + [[nodiscard]] auto generateFieldPoints(float grid_size) const; auto getDistanceFromRobot(RobotIdentifier id, const Point & point) const -> double { @@ -383,22 +248,7 @@ struct WorldModelWrapper auto getNearestRobotsWithDistanceFromPoint( const Point & point, const std::vector> robots) const - -> std::pair, double> - { - std::shared_ptr nearest_robot = nullptr; - double min_sq_distance = std::numeric_limits::max(); - for (const auto & robot : robots) { - if (!robot->available) { - continue; - } - double sq_distance = (robot->pose.pos - point).squaredNorm(); - if (sq_distance < min_sq_distance) { - min_sq_distance = sq_distance; - nearest_robot = robot; - } - } - return {nearest_robot, std::sqrt(min_sq_distance)}; - } + -> std::pair, double>; [[nodiscard]] bool isEnemyDefenseArea(const Point & p) const { @@ -415,30 +265,11 @@ struct WorldModelWrapper return isFriendDefenseArea(p) || isEnemyDefenseArea(p); } - [[nodiscard]] bool isFieldInside(const Point & p, double offset = 0.) const - { - Box field_box; - field_box.min_corner() << -field_size.x() / 2.f - offset, -field_size.y() / 2.f - offset; - field_box.max_corner() << field_size.x() / 2.f + offset, field_size.y() / 2.f + offset; - return isInBox(field_box, p); - } + [[nodiscard]] bool isFieldInside(const Point & p, double offset = 0.) const; [[nodiscard]] double getFieldMargin() const { return 0.3; } - [[nodiscard]] bool isBallPlacementArea(const Point & p, double offset = 0.) const - { - // During ball placement, all robots of the non-placing team have to keep - // at least 0.5 meters distance to the line between the ball and the placement position - // (the forbidden area forms a stadium shape). - // ref: https://robocup-ssl.github.io/ssl-rules/sslrules.html#_ball_placement_interference - // Segment ball_placement_line; - // {Point(ball_placement_target), Point(ball.pos)}; - if (auto area = getBallPlacementArea(offset)) { - return bg::distance(area.value(), p) < 0.001; - } else { - return false; - } - } + [[nodiscard]] bool isBallPlacementArea(const Point & p, double offset = 0.) const; [[nodiscard]] double getDefenseWidth() const { @@ -470,32 +301,10 @@ struct WorldModelWrapper [[nodiscard]] Point getTheirGoalCenter() const { return Point(-goal.x(), goal.y()); } - [[nodiscard]] std::optional getBallPlacementTarget() const - { - if ( - play_situation.getSituationCommandID() == - crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT or - play_situation.getSituationCommandID() == - crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT) { - return play_situation.placement_position; - } else { - return std::nullopt; - } - } + [[nodiscard]] std::optional getBallPlacementTarget() const; // rule 8.4.3 - [[nodiscard]] std::optional getBallPlacementArea(const double offset = 0.) const - { - if (auto target = getBallPlacementTarget()) { - Capsule area; - area.segment.first = ball.pos; - area.segment.second = target.value(); - area.radius = 0.5 + offset; - return area; - } else { - return std::nullopt; - } - } + [[nodiscard]] std::optional getBallPlacementArea(const double offset = 0.) const; [[nodiscard]] uint8_t getOurGoalieId() const { return latest_msg.our_goalie_id; } @@ -506,92 +315,10 @@ struct WorldModelWrapper * @param from * @return {angle, width} */ - auto getLargestGoalAngleRangeFromPoint(Point from) -> std::pair - { - Interval goal_range; - - auto goal_posts = getTheirGoalPosts(); - if (goal_posts.first.x() < 0.) { - goal_range.append( - normalizeAngle(getAngle(goal_posts.first - from) + M_PI), - normalizeAngle(getAngle(goal_posts.second - from) + M_PI)); - } else { - goal_range.append(getAngle(goal_posts.first - from), getAngle(goal_posts.second - from)); - } - - for (auto & enemy : theirs.getAvailableRobots()) { - double distance = enemy->getDistance(from); - constexpr double MACHINE_RADIUS = 0.1; - - double center_angle = [&]() { - if (goal_posts.first.x() < 0.) { - return normalizeAngle(getAngle(enemy->pose.pos - from) + M_PI); - } else { - return getAngle(enemy->pose.pos - from); - } - }(); - double diff_angle = - atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS)); - - goal_range.erase(center_angle - diff_angle, center_angle + diff_angle); - } - - auto largest_interval = goal_range.getLargestInterval(); - - double target_angle = [&]() { - if (goal_posts.first.x() < 0.) { - return normalizeAngle((largest_interval.first + largest_interval.second) / 2.0 - M_PI); - } else { - return (largest_interval.first + largest_interval.second) / 2.0; - } - }(); - - return {target_angle, largest_interval.second - largest_interval.first}; - } + auto getLargestGoalAngleRangeFromPoint(Point from) -> std::pair; auto getLargestOurGoalAngleRangeFromPoint( - Point from, std::vector> robots) -> std::pair - { - Interval goal_range; - - auto goal_posts = getOurGoalPosts(); - if (goal_posts.first.x() < 0.) { - goal_range.append( - normalizeAngle(getAngle(goal_posts.first - from) + M_PI), - normalizeAngle(getAngle(goal_posts.second - from) + M_PI)); - } else { - goal_range.append(getAngle(goal_posts.first - from), getAngle(goal_posts.second - from)); - } - - for (auto & enemy : robots) { - double distance = enemy->getDistance(from); - constexpr double MACHINE_RADIUS = 0.1; - - double center_angle = [&]() { - if (goal_posts.first.x() < 0.) { - return normalizeAngle(getAngle(enemy->pose.pos - from) + M_PI); - } else { - return getAngle(enemy->pose.pos - from); - } - }(); - double diff_angle = - atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS)); - - goal_range.erase(center_angle - diff_angle, center_angle + diff_angle); - } - - auto largest_interval = goal_range.getLargestInterval(); - - double target_angle = [&]() { - if (goal_posts.first.x() < 0.) { - return normalizeAngle((largest_interval.first + largest_interval.second) / 2.0 - M_PI); - } else { - return (largest_interval.first + largest_interval.second) / 2.0; - } - }(); - - return {target_angle, largest_interval.second - largest_interval.first}; - } + Point from, std::vector> robots) -> std::pair; auto getLargestOurGoalAngleRangeFromPoint(Point from) -> std::pair { diff --git a/utility/crane_msg_wrappers/src/world_model_wrapper.cpp b/utility/crane_msg_wrappers/src/world_model_wrapper.cpp new file mode 100644 index 000000000..907296c9e --- /dev/null +++ b/utility/crane_msg_wrappers/src/world_model_wrapper.cpp @@ -0,0 +1,312 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include "crane_msg_wrappers/world_model_wrapper.hpp" + +namespace crane +{ +void BallContact::update(bool is_contacted) +{ + auto now = std::chrono::system_clock::now(); + if (is_contacted) { + last_contact_end_time = now; + if (not is_contacted_pre_frame) { + last_contact_start_time = now; + } + } else { + last_contact_start_time = last_contact_end_time; + } + is_contacted_pre_frame = is_contacted; +} + +std::vector> TeamInfo::getAvailableRobots(uint8_t my_id) +{ + std::vector> available_robots; + for (auto robot : robots) { + if (robot->available && robot->id != my_id) { + available_robots.emplace_back(robot); + } + } + return available_robots; +} + +void Hysteresis::update(double value) +{ + if (not is_high && value > upper_threshold) { + is_high = true; + upper_callback(); + } + + if (is_high && value < lower_threshold) { + is_high = false; + lower_callback(); + } +} + +bool Ball::isMovingTowards(const Point & p, double angle_threshold_deg, double near_threshold) const +{ + if ((pos - p).norm() < near_threshold) { + return false; + } else { + auto dir = (p - pos).normalized(); + return dir.dot(vel.normalized()) > cos(angle_threshold_deg * M_PI / 180.0); + } +} + +WorldModelWrapper::WorldModelWrapper(rclcpp::Node & node) +{ + // メモリ確保 + // ヒトサッカーの台数は超えないはず + constexpr uint8_t MAX_ROBOT_NUM = 20; + for (int i = 0; i < MAX_ROBOT_NUM; i++) { + ours.robots.emplace_back(std::make_shared()); + theirs.robots.emplace_back(std::make_shared()); + } + + subscriber = node.create_subscription( + "/world_model", 10, [this](const crane_msgs::msg::WorldModel::SharedPtr msg) -> void { + latest_msg = *msg; + this->update(*msg); + has_updated = true; + for (auto & callback : callbacks) { + callback(); + } + }); +} + +void WorldModelWrapper::update(const crane_msgs::msg::WorldModel & world_model) +{ + play_situation.update(world_model.play_situation); + + for (auto & our_robot : ours.robots) { + our_robot->available = false; + } + + for (auto & their_robot : theirs.robots) { + their_robot->available = false; + } + + ball.pos << world_model.ball_info.pose.x, world_model.ball_info.pose.y; + ball.vel << world_model.ball_info.velocity.x, world_model.ball_info.velocity.y; + ball.ball_speed_hysteresis.update(ball.vel.norm()); + + for (auto & robot : world_model.robot_info_ours) { + auto & info = ours.robots.at(robot.id); + info->available = !robot.disappeared; + if (info->available) { + info->id = robot.id; + info->pose.pos << robot.pose.x, robot.pose.y; + info->pose.theta = robot.pose.theta; + info->vel.linear << robot.velocity.x, robot.velocity.y; + info->ball_contact.update((info->kicker_center() - ball.pos).norm() < 0.1); + } else { + info->ball_contact.update(false); + } + } + + for (auto robot : world_model.robot_info_theirs) { + auto & info = theirs.robots.at(robot.id); + info->available = !robot.disappeared; + if (info->available) { + info->id = robot.id; + info->ball_contact.update( + robot.ball_contact.current_time == robot.ball_contact.last_contacted_time); + info->pose.pos << robot.pose.x, robot.pose.y; + info->pose.theta = robot.pose.theta; + info->vel.linear << robot.velocity.x, robot.velocity.y; + // todo : omega + } else { + info->ball_contact.update(false); + } + } + + field_size << world_model.field_info.x, world_model.field_info.y; + defense_area_size << world_model.defense_area_size.x, world_model.defense_area_size.y; + + goal_size << world_model.goal_size.x, world_model.goal_size.y; + goal << getOurSideSign() * field_size.x() * 0.5, 0.; + + if (onPositiveHalf()) { + ours.defense_area.max_corner() << goal.x(), goal.y() + world_model.defense_area_size.y / 2.; + ours.defense_area.min_corner() << goal.x() - world_model.defense_area_size.x, + goal.y() - world_model.defense_area_size.y / 2.; + } else { + ours.defense_area.max_corner() << goal.x() + world_model.defense_area_size.x, + goal.y() + world_model.defense_area_size.y / 2.; + ours.defense_area.min_corner() << goal.x(), goal.y() - world_model.defense_area_size.y / 2.; + } + theirs.defense_area.max_corner() + << std::max(-ours.defense_area.max_corner().x(), -ours.defense_area.min_corner().x()), + ours.defense_area.max_corner().y(); + theirs.defense_area.min_corner() + << std::min(-ours.defense_area.max_corner().x(), -ours.defense_area.min_corner().x()), + ours.defense_area.min_corner().y(); +} +auto WorldModelWrapper::generateFieldPoints(float grid_size) const +{ + std::vector points; + for (float x = 0.f; x <= field_size.x() / 2.f; x += grid_size) { + for (float y = 0.f; y <= field_size.y() / 2.f; y += grid_size) { + points.emplace_back(x, y); + } + } + return points; +} + +auto WorldModelWrapper::getNearestRobotsWithDistanceFromPoint( + const Point & point, const std::vector> robots) const + -> std::pair, double> +{ + std::shared_ptr nearest_robot = nullptr; + double min_sq_distance = std::numeric_limits::max(); + for (const auto & robot : robots) { + if (!robot->available) { + continue; + } + double sq_distance = (robot->pose.pos - point).squaredNorm(); + if (sq_distance < min_sq_distance) { + min_sq_distance = sq_distance; + nearest_robot = robot; + } + } + return {nearest_robot, std::sqrt(min_sq_distance)}; +} + +bool WorldModelWrapper::isFieldInside(const Point & p, double offset) const +{ + Box field_box; + field_box.min_corner() << -field_size.x() / 2.f - offset, -field_size.y() / 2.f - offset; + field_box.max_corner() << field_size.x() / 2.f + offset, field_size.y() / 2.f + offset; + return isInBox(field_box, p); +} + +bool WorldModelWrapper::isBallPlacementArea(const Point & p, double offset) const +{ + // During ball placement, all robots of the non-placing team have to keep + // at least 0.5 meters distance to the line between the ball and the placement position + // (the forbidden area forms a stadium shape). + // ref: https://robocup-ssl.github.io/ssl-rules/sslrules.html#_ball_placement_interference + // Segment ball_placement_line; + // {Point(ball_placement_target), Point(ball.pos)}; + if (auto area = getBallPlacementArea(offset)) { + return bg::distance(area.value(), p) < 0.001; + } else { + return false; + } +} + +std::optional WorldModelWrapper::getBallPlacementTarget() const +{ + if ( + play_situation.getSituationCommandID() == crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT or + play_situation.getSituationCommandID() == + crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT) { + return play_situation.placement_position; + } else { + return std::nullopt; + } +} + +std::optional WorldModelWrapper::getBallPlacementArea(const double offset) const +{ + if (auto target = getBallPlacementTarget()) { + Capsule area; + area.segment.first = ball.pos; + area.segment.second = target.value(); + area.radius = 0.5 + offset; + return area; + } else { + return std::nullopt; + } +} + +auto WorldModelWrapper::getLargestGoalAngleRangeFromPoint(Point from) -> std::pair +{ + Interval goal_range; + + auto goal_posts = getTheirGoalPosts(); + if (goal_posts.first.x() < 0.) { + goal_range.append( + normalizeAngle(getAngle(goal_posts.first - from) + M_PI), + normalizeAngle(getAngle(goal_posts.second - from) + M_PI)); + } else { + goal_range.append(getAngle(goal_posts.first - from), getAngle(goal_posts.second - from)); + } + + for (auto & enemy : theirs.getAvailableRobots()) { + double distance = enemy->getDistance(from); + constexpr double MACHINE_RADIUS = 0.1; + + double center_angle = [&]() { + if (goal_posts.first.x() < 0.) { + return normalizeAngle(getAngle(enemy->pose.pos - from) + M_PI); + } else { + return getAngle(enemy->pose.pos - from); + } + }(); + double diff_angle = + atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS)); + + goal_range.erase(center_angle - diff_angle, center_angle + diff_angle); + } + + auto largest_interval = goal_range.getLargestInterval(); + + double target_angle = [&]() { + if (goal_posts.first.x() < 0.) { + return normalizeAngle((largest_interval.first + largest_interval.second) / 2.0 - M_PI); + } else { + return (largest_interval.first + largest_interval.second) / 2.0; + } + }(); + + return {target_angle, largest_interval.second - largest_interval.first}; +} + +auto WorldModelWrapper::getLargestOurGoalAngleRangeFromPoint( + Point from, std::vector> robots) -> std::pair +{ + Interval goal_range; + + auto goal_posts = getOurGoalPosts(); + if (goal_posts.first.x() < 0.) { + goal_range.append( + normalizeAngle(getAngle(goal_posts.first - from) + M_PI), + normalizeAngle(getAngle(goal_posts.second - from) + M_PI)); + } else { + goal_range.append(getAngle(goal_posts.first - from), getAngle(goal_posts.second - from)); + } + + for (auto & enemy : robots) { + double distance = enemy->getDistance(from); + constexpr double MACHINE_RADIUS = 0.1; + + double center_angle = [&]() { + if (goal_posts.first.x() < 0.) { + return normalizeAngle(getAngle(enemy->pose.pos - from) + M_PI); + } else { + return getAngle(enemy->pose.pos - from); + } + }(); + double diff_angle = + atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS)); + + goal_range.erase(center_angle - diff_angle, center_angle + diff_angle); + } + + auto largest_interval = goal_range.getLargestInterval(); + + double target_angle = [&]() { + if (goal_posts.first.x() < 0.) { + return normalizeAngle((largest_interval.first + largest_interval.second) / 2.0 - M_PI); + } else { + return (largest_interval.first + largest_interval.second) / 2.0; + } + }(); + + return {target_angle, largest_interval.second - largest_interval.first}; +} +} // namespace crane diff --git a/utility/crane_teleop/CMakeLists.txt b/utility/crane_teleop/CMakeLists.txt index 92d0c68a8..e62720c09 100755 --- a/utility/crane_teleop/CMakeLists.txt +++ b/utility/crane_teleop/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(crane_teleop) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/utility/gridmap_to_image/CMakeLists.txt b/utility/gridmap_to_image/CMakeLists.txt index 70576e602..6301e3790 100755 --- a/utility/gridmap_to_image/CMakeLists.txt +++ b/utility/gridmap_to_image/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 3.5) project(gridmap_to_image) -# Default to C++17 +# Default to C++20 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")