From d78b26bfaaef9e2a7a056b3a271abc6088fe2d61 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 15 Oct 2023 08:46:12 +0900 Subject: [PATCH 01/26] WIP --- .../launch/play_switcher_y.launch.py | 12 ++- crane_robot_skills/.clang-format | 18 +++++ crane_robot_skills/CMakeLists.txt | 32 ++++++++ .../include/crane_robot_skills/skill_base.hpp | 78 +++++++++++++++++++ crane_robot_skills/package.xml | 28 +++++++ 5 files changed, 165 insertions(+), 3 deletions(-) create mode 100755 crane_robot_skills/.clang-format create mode 100755 crane_robot_skills/CMakeLists.txt create mode 100644 crane_robot_skills/include/crane_robot_skills/skill_base.hpp create mode 100755 crane_robot_skills/package.xml diff --git a/crane_bringup/launch/play_switcher_y.launch.py b/crane_bringup/launch/play_switcher_y.launch.py index a2c8b8dbd..fe3e63310 100755 --- a/crane_bringup/launch/play_switcher_y.launch.py +++ b/crane_bringup/launch/play_switcher_y.launch.py @@ -28,6 +28,8 @@ from launch.launch_context import LaunchContext _logger = logging.getLogger(name='launch') + + class ShutdownOnce(EmitEvent): shutdown_called = False """Action that shuts down a launched system by emitting Shutdown when executed.""" @@ -61,6 +63,7 @@ def execute(self, context: LaunchContext): super().execute(context) + def generate_launch_description(): declare_arg_vision_addr = DeclareLaunchArgument( "vision_addr", @@ -114,7 +117,8 @@ def generate_launch_description(): waiter = Node(package="crane_planner_plugins", executable="waiter_node") - formation = Node(package="crane_planner_plugins", executable="formation_node") + formation = Node(package="crane_planner_plugins", + executable="formation_node") goalie = Node(package="crane_planner_plugins", executable="goalie_node", @@ -151,7 +155,8 @@ def generate_launch_description(): grsim = Node(package="robocup_ssl_comm", executable="grsim_node") - vision_tracker = Node(package="consai_vision_tracker", executable="vision_tracker_node") + vision_tracker = Node(package="consai_vision_tracker", + executable="vision_tracker_node") world_model_publisher = Node( package="crane_world_model_publisher", @@ -186,7 +191,8 @@ def generate_launch_description(): package="crane_play_switcher", executable="play_switcher_node", output="screen" ) - visualizer = Node(package="consai_visualizer", executable="consai_visualizer", output="screen") + visualizer = Node(package="consai_visualizer", + executable="consai_visualizer", output="screen") sim_sender = Node( package="crane_sender", diff --git a/crane_robot_skills/.clang-format b/crane_robot_skills/.clang-format new file mode 100755 index 000000000..aea802ff3 --- /dev/null +++ b/crane_robot_skills/.clang-format @@ -0,0 +1,18 @@ +--- +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false diff --git a/crane_robot_skills/CMakeLists.txt b/crane_robot_skills/CMakeLists.txt new file mode 100755 index 000000000..15de56b76 --- /dev/null +++ b/crane_robot_skills/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(crane_world_model_publisher) + +# 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) +endif () + +add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS") + +find_package(ament_cmake_auto REQUIRED) +find_package(rclcpp_components REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(${PROJECT_NAME}_component SHARED + src/world_model_publisher.cpp include/crane_world_model_publisher/world_model_publisher.hpp) + +rclcpp_components_register_nodes(${PROJECT_NAME}_component "crane::WorldModelPublisherComponent") + +ament_auto_add_executable(${PROJECT_NAME}_node src/world_model_publisher_node.cpp) +target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_component) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp new file mode 100644 index 000000000..5da243712 --- /dev/null +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -0,0 +1,78 @@ +// +// Created by hans on 23/10/14. +// + +#ifndef CRANE_PLANNER_PLUGINS_SKILL_BASE_HPP +#define CRANE_PLANNER_PLUGINS_SKILL_BASE_HPP + +#include +#include +#include +#include +#include +#include + +namespace crane +{ + +enum class DefaultStates { + DEFAULT, +}; + +template +class StateMachine +{ +public: + void addTransition( + const StatesType & from, const StatesType & to, std::function condition) + { + transitions.emplace_back({from, to}, condition); + } + void setInitialState(StatesType state) + { + initial_state = state; + current_state = initial_state; + } + void setFinalState(StatesType state) { final_state = state; } + void update() + { + while (currentState != finalState) { + auto skill = states.at(currentState); + auto status = skill->run(); + if (status == Status::SUCCESS) { + current_state = transitions.at(currentState); + } + } + } + +protected: + StatesType current_state; + StatesType initial_state; + StatesType final_state; + std::vector, std::function> transitions; +}; + +class SkillBase +{ +public: + enum class Status { + SUCCESS, + FAILURE, + RUNNING, + }; + SkillBase(const std::string & name, uint8_t id, std::shared_ptr & world_model) + : name(name), world_model(world_model), robot(world_model->getRobot({true, id})) + { + } + + const std::string name; + + virtual Status run(const std::shared_ptr &, crane::RobotCommandWrapper &) = 0; + +protected: + Status status = Status::RUNNING; + std::shared_ptr world_model; + std::shared_ptr robot; +}; +} // namespace crane +#endif //CRANE_PLANNER_PLUGINS_SKILL_BASE_HPP diff --git a/crane_robot_skills/package.xml b/crane_robot_skills/package.xml new file mode 100755 index 000000000..a7a0b6a1a --- /dev/null +++ b/crane_robot_skills/package.xml @@ -0,0 +1,28 @@ + + + + crane_world_model_publisher + 0.0.0 + crane_world_model_publisher + akshota + MIT + + ament_cmake_auto + + boost + crane_geometry + crane_msgs + rclcpp + rclcpp_components + robocup_ssl_msgs + std_msgs + + ament_index_python + launch_ros + ament_lint_auto + crane_lint_common + + + ament_cmake + + From b69497598ed3c4a1fbdc6b7fbb481ed2e2a42108 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 15 Oct 2023 18:25:30 +0900 Subject: [PATCH 02/26] =?UTF-8?q?StateMachine=E3=81=A8SkillBase=E3=82=92?= =?UTF-8?q?=E3=81=9D=E3=82=8C=E3=81=BD=E3=81=8F=E4=BD=BF=E3=81=88=E3=81=9D?= =?UTF-8?q?=E3=81=86=E3=81=AA=E6=84=9F=E3=81=98=E3=81=AB=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/skill_base.hpp | 80 ++++++++++++++----- 1 file changed, 61 insertions(+), 19 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 5da243712..1c0395647 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -23,35 +23,42 @@ template class StateMachine { public: + struct Transition + { + StatesType from; + StatesType to; + std::function condition; + }; + + StateMachine(StatesType init_state) : current_state(init_state) {} + void addTransition( const StatesType & from, const StatesType & to, std::function condition) { transitions.emplace_back({from, to}, condition); } - void setInitialState(StatesType state) - { - initial_state = state; - current_state = initial_state; - } - void setFinalState(StatesType state) { final_state = state; } + void update() { - while (currentState != finalState) { - auto skill = states.at(currentState); - auto status = skill->run(); - if (status == Status::SUCCESS) { - current_state = transitions.at(currentState); - } + if (auto it = std::find_if( + transitions.begin(), transitions.end(), + [this](const Transition & transition) { + return transition.from == current_state && transition.condition(); + }); + it != transitions.end()) { + current_state = it->to; } } + StatesType getCurrentState() { return current_state; } + protected: StatesType current_state; - StatesType initial_state; - StatesType final_state; - std::vector, std::function> transitions; + + std::vector transitions; }; +template class SkillBase { public: @@ -60,19 +67,54 @@ class SkillBase FAILURE, RUNNING, }; - SkillBase(const std::string & name, uint8_t id, std::shared_ptr & world_model) - : name(name), world_model(world_model), robot(world_model->getRobot({true, id})) + + using StateFunctionType = std::function &, const std::shared_ptr &, crane::RobotCommandWrapper &)>; + + SkillBase( + const std::string & name, uint8_t id, std::shared_ptr & world_model, + StatesType & init_state) + : name(name), + world_model(world_model), + robot(world_model->getRobot({true, id})), + state_machine(init_state) { } + +// SkillBase( +// const std::string & name, uint8_t id, std::shared_ptr & world_model) : SkillBase(name, id, world_model, DefaultStates::DEFAULT) {} + const std::string name; - virtual Status run(const std::shared_ptr &, crane::RobotCommandWrapper &) = 0; + Status run(RobotCommandWrapper & command){ + state_machine.update(); + return state_functions[state_machine.getCurrentState()](world_model, robot, command); + } + + void addStateFunction(const StatesType & state, StateFunctionType function){ + if(state_functions.find(state) != state_functions.end()){ + RCLCPP_WARN(rclcpp::get_logger("State: " + name), "State function already exists and is overwritten now."); + } + state_functions[state] = function; + } + + void addTransitions(const StatesType & from, std::vector>> transition_targets){ + for(const auto &transition_target : transition_targets){ + state_machine.addTransition(from, transition_target.first, transition_target.second); + } + } + protected: - Status status = Status::RUNNING; +// Status status = Status::RUNNING; + std::shared_ptr world_model; + std::shared_ptr robot; + + StateMachine state_machine; + + std::unordered_map state_functions; }; } // namespace crane #endif //CRANE_PLANNER_PLUGINS_SKILL_BASE_HPP From 41bdcff32d6817c254076964b950b0098c214022 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 15 Oct 2023 18:26:11 +0900 Subject: [PATCH 03/26] =?UTF-8?q?Skill=E3=81=AE=E5=AE=9F=E8=A3=85=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E3=81=A3=E3=81=BD=E3=81=84=E3=82=84?= =?UTF-8?q?=E3=81=A4=E3=82=92=E8=BF=BD=E5=8A=A0=E3=81=97=E3=81=9F=EF=BC=8E?= =?UTF-8?q?=E3=83=93=E3=83=AB=E3=83=89=E3=81=AF=E5=A4=9A=E5=88=86=E9=80=9A?= =?UTF-8?q?=E3=82=89=E3=82=93?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/move_to_geometry.hpp | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp 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 new file mode 100644 index 000000000..eb030e350 --- /dev/null +++ b/crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp @@ -0,0 +1,37 @@ +// 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. + +#ifndef CRANE_PLANNER_PLUGINS__MOVE_TO_GEOMETRY_HPP +#define CRANE_PLANNER_PLUGINS__MOVE_TO_GEOMETRY_HPP + +#include + +namespace crane +{ +class MoveToGeometry : public SkillBase +{ +public: + explicit MoveToGeometry(uint8_t id, std::shared_ptr & world_model) + : SkillBase("move_to_geometry", world_model, id, DefaultStates::DEFAULT) + { + addStateFunction(DefaultStates::DEFAULT, [](const std::shared_ptr & world_model, const std::shared_ptr & robot, crane::RobotCommandWrapper & command) -> SkillBase::Status { + if((robot->pose.pos - getTargetPoint()).norm() < 0.1){ + return SkillBase::Status::SUCCESS; + }else{ + command.setTargetPosition(getTargetPoint()); + return SkillBase::Status::RUNNING; + } + }); + } + + Point getTargetPoint() + { + return Point(); + } + +}; +} +#endif //CRANE_PLANNER_PLUGINS__MOVE_TO_GEOMETRY_HPP From e89b8e455b01f36b49252485470f21a8d4a6079f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 16 Oct 2023 21:01:50 +0900 Subject: [PATCH 04/26] WIP --- crane_robot_skills/CMakeLists.txt | 6 +++--- crane_robot_skills/package.xml | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/crane_robot_skills/CMakeLists.txt b/crane_robot_skills/CMakeLists.txt index 15de56b76..8084142f0 100755 --- a/crane_robot_skills/CMakeLists.txt +++ b/crane_robot_skills/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(crane_world_model_publisher) +project(crane_robot_skills) # Default to C++17 if (NOT CMAKE_CXX_STANDARD) @@ -17,11 +17,11 @@ find_package(rclcpp_components REQUIRED) ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME}_component SHARED - src/world_model_publisher.cpp include/crane_world_model_publisher/world_model_publisher.hpp) + src/robot_skills.cpp) rclcpp_components_register_nodes(${PROJECT_NAME}_component "crane::WorldModelPublisherComponent") -ament_auto_add_executable(${PROJECT_NAME}_node src/world_model_publisher_node.cpp) +ament_auto_add_executable(${PROJECT_NAME}_node src/robot_skills_node.cpp) target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_component) if(BUILD_TESTING) diff --git a/crane_robot_skills/package.xml b/crane_robot_skills/package.xml index a7a0b6a1a..fd7711cfb 100755 --- a/crane_robot_skills/package.xml +++ b/crane_robot_skills/package.xml @@ -1,10 +1,9 @@ - crane_world_model_publisher + crane_robot_skills 0.0.0 - crane_world_model_publisher - akshota + crane_robot_skills MIT ament_cmake_auto From b4f34fda94f687b4c81d046d2807a0cafae7c6b0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 16 Oct 2023 21:11:45 +0900 Subject: [PATCH 05/26] WIP --- .../crane_robot_skills/move_to_geometry.hpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) 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 eb030e350..901bfd998 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 @@ -17,21 +17,22 @@ class MoveToGeometry : public SkillBase explicit MoveToGeometry(uint8_t id, std::shared_ptr & world_model) : SkillBase("move_to_geometry", world_model, id, DefaultStates::DEFAULT) { - addStateFunction(DefaultStates::DEFAULT, [](const std::shared_ptr & world_model, const std::shared_ptr & robot, crane::RobotCommandWrapper & command) -> SkillBase::Status { - if((robot->pose.pos - getTargetPoint()).norm() < 0.1){ - return SkillBase::Status::SUCCESS; - }else{ - command.setTargetPosition(getTargetPoint()); - return SkillBase::Status::RUNNING; - } - }); - } - - Point getTargetPoint() - { - return Point(); + addStateFunction( + DefaultStates::DEFAULT, + []( + const std::shared_ptr & world_model, + const std::shared_ptr & robot, + crane::RobotCommandWrapper & command) -> SkillBase::Status { + if ((robot->pose.pos - getTargetPoint()).norm() < 0.1) { + return SkillBase::Status::SUCCESS; + } else { + command.setTargetPosition(getTargetPoint()); + return SkillBase::Status::RUNNING; + } + }); } + Point getTargetPoint() { return Point(); } }; -} +} // namespace crane #endif //CRANE_PLANNER_PLUGINS__MOVE_TO_GEOMETRY_HPP From 2ff6f24b359667b2e05aa1470d13d36274bc7467 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 16 Oct 2023 21:26:13 +0900 Subject: [PATCH 06/26] =?UTF-8?q?=E3=83=93=E3=83=AB=E3=83=89=E3=82=92?= =?UTF-8?q?=E9=80=9A=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/CMakeLists.txt | 6 +-- .../crane_robot_skills/move_to_geometry.hpp | 6 +-- .../include/crane_robot_skills/skill_base.hpp | 45 +++++++++++-------- crane_robot_skills/package.xml | 2 + 4 files changed, 35 insertions(+), 24 deletions(-) diff --git a/crane_robot_skills/CMakeLists.txt b/crane_robot_skills/CMakeLists.txt index 8084142f0..5a94ac609 100755 --- a/crane_robot_skills/CMakeLists.txt +++ b/crane_robot_skills/CMakeLists.txt @@ -19,10 +19,10 @@ ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME}_component SHARED src/robot_skills.cpp) -rclcpp_components_register_nodes(${PROJECT_NAME}_component "crane::WorldModelPublisherComponent") +#rclcpp_components_register_nodes(${PROJECT_NAME}_component "crane::WorldModelPublisherComponent") -ament_auto_add_executable(${PROJECT_NAME}_node src/robot_skills_node.cpp) -target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_component) +#ament_auto_add_executable(${PROJECT_NAME}_node src/robot_skills_node.cpp) +#target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_component) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) 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 901bfd998..be8940f93 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 @@ -11,15 +11,15 @@ namespace crane { -class MoveToGeometry : public SkillBase +class MoveToGeometry : public SkillBase { public: explicit MoveToGeometry(uint8_t id, std::shared_ptr & world_model) - : SkillBase("move_to_geometry", world_model, id, DefaultStates::DEFAULT) + : SkillBase("move_to_geometry", id, world_model, DefaultStates::DEFAULT) { addStateFunction( DefaultStates::DEFAULT, - []( + [this]( const std::shared_ptr & world_model, const std::shared_ptr & robot, crane::RobotCommandWrapper & command) -> SkillBase::Status { diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 1c0395647..491312510 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -1,9 +1,11 @@ +// Copyright (c) 2022 ibis-ssl // -// Created by hans on 23/10/14. -// +// 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. -#ifndef CRANE_PLANNER_PLUGINS_SKILL_BASE_HPP -#define CRANE_PLANNER_PLUGINS_SKILL_BASE_HPP +#ifndef CRANE_ROBOT_SKILLS__SKILL_BASE_HPP_ +#define CRANE_ROBOT_SKILLS__SKILL_BASE_HPP_ #include #include @@ -68,11 +70,13 @@ class SkillBase RUNNING, }; - using StateFunctionType = std::function &, const std::shared_ptr &, crane::RobotCommandWrapper &)>; + using StateFunctionType = std::function &, const std::shared_ptr &, + crane::RobotCommandWrapper &)>; SkillBase( const std::string & name, uint8_t id, std::shared_ptr & world_model, - StatesType & init_state) + StatesType init_state) : name(name), world_model(world_model), robot(world_model->getRobot({true, id})), @@ -80,33 +84,38 @@ class SkillBase { } - -// SkillBase( -// const std::string & name, uint8_t id, std::shared_ptr & world_model) : SkillBase(name, id, world_model, DefaultStates::DEFAULT) {} + // SkillBase( + // const std::string & name, uint8_t id, std::shared_ptr & world_model) : SkillBase(name, id, world_model, DefaultStates::DEFAULT) {} const std::string name; - Status run(RobotCommandWrapper & command){ + Status run(RobotCommandWrapper & command) + { state_machine.update(); return state_functions[state_machine.getCurrentState()](world_model, robot, command); } - void addStateFunction(const StatesType & state, StateFunctionType function){ - if(state_functions.find(state) != state_functions.end()){ - RCLCPP_WARN(rclcpp::get_logger("State: " + name), "State function already exists and is overwritten now."); + void addStateFunction(const StatesType & state, StateFunctionType function) + { + if (state_functions.find(state) != state_functions.end()) { + RCLCPP_WARN( + rclcpp::get_logger("State: " + name), + "State function already exists and is overwritten now."); } state_functions[state] = function; } - void addTransitions(const StatesType & from, std::vector>> transition_targets){ - for(const auto &transition_target : transition_targets){ + void addTransitions( + const StatesType & from, + std::vector>> transition_targets) + { + for (const auto & transition_target : transition_targets) { state_machine.addTransition(from, transition_target.first, transition_target.second); } } - protected: -// Status status = Status::RUNNING; + // Status status = Status::RUNNING; std::shared_ptr world_model; @@ -117,4 +126,4 @@ class SkillBase std::unordered_map state_functions; }; } // namespace crane -#endif //CRANE_PLANNER_PLUGINS_SKILL_BASE_HPP +#endif // CRANE_ROBOT_SKILLS__SKILL_BASE_HPP_ diff --git a/crane_robot_skills/package.xml b/crane_robot_skills/package.xml index fd7711cfb..9db598361 100755 --- a/crane_robot_skills/package.xml +++ b/crane_robot_skills/package.xml @@ -4,6 +4,7 @@ crane_robot_skills 0.0.0 crane_robot_skills + ibis ssl MIT ament_cmake_auto @@ -15,6 +16,7 @@ rclcpp_components robocup_ssl_msgs std_msgs + crane_msg_wrappers ament_index_python launch_ros From 8c5f404afd8a0d820d4e96cfa70c49fad9b061c7 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 16 Oct 2023 21:27:16 +0900 Subject: [PATCH 07/26] =?UTF-8?q?=E3=83=86=E3=83=B3=E3=83=97=E3=83=AC?= =?UTF-8?q?=E3=83=BC=E3=83=88=E7=9C=81=E7=95=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/move_to_geometry.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 be8940f93..59cf759eb 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 @@ -11,11 +11,11 @@ namespace crane { -class MoveToGeometry : public SkillBase +class MoveToGeometry : public SkillBase<> { public: explicit MoveToGeometry(uint8_t id, std::shared_ptr & world_model) - : SkillBase("move_to_geometry", id, world_model, DefaultStates::DEFAULT) + : SkillBase<>("move_to_geometry", id, world_model, DefaultStates::DEFAULT) { addStateFunction( DefaultStates::DEFAULT, From 4bad73ca1f6131216eceb633c8dc360c9cdb3656 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 16 Oct 2023 21:52:49 +0900 Subject: [PATCH 08/26] =?UTF-8?q?MoveToGeometry=E3=82=AF=E3=83=A9=E3=82=B9?= =?UTF-8?q?=E3=82=92=E4=B8=80=E8=88=AC=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/move_to_geometry.hpp | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) 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 59cf759eb..592578a82 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 @@ -4,26 +4,30 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#ifndef CRANE_PLANNER_PLUGINS__MOVE_TO_GEOMETRY_HPP -#define CRANE_PLANNER_PLUGINS__MOVE_TO_GEOMETRY_HPP +#ifndef CRANE_ROBOT_SKILLS__MOVE_TO_GEOMETRY_HPP_ +#define CRANE_ROBOT_SKILLS__MOVE_TO_GEOMETRY_HPP_ +#include #include namespace crane { +template class MoveToGeometry : public SkillBase<> { public: - explicit MoveToGeometry(uint8_t id, std::shared_ptr & world_model) - : SkillBase<>("move_to_geometry", id, world_model, DefaultStates::DEFAULT) + explicit MoveToGeometry( + uint8_t id, Geometry geometry, std::shared_ptr & world_model, + const double threshold = 0.1) + : SkillBase<>("move_to_geometry", id, world_model, DefaultStates::DEFAULT), geometry(geometry) { addStateFunction( DefaultStates::DEFAULT, - [this]( + [this, threshold]( const std::shared_ptr & world_model, const std::shared_ptr & robot, crane::RobotCommandWrapper & command) -> SkillBase::Status { - if ((robot->pose.pos - getTargetPoint()).norm() < 0.1) { + if ((robot->pose.pos - getTargetPoint()).norm() < threshold) { return SkillBase::Status::SUCCESS; } else { command.setTargetPosition(getTargetPoint()); @@ -32,7 +36,15 @@ class MoveToGeometry : public SkillBase<> }); } - Point getTargetPoint() { return Point(); } + Point getTargetPoint() + { + ClosestPoint result; + bg::closest_point(geometry, robot->pose.pos, result); + return result.closest_point; + } + +protected: + Geometry geometry; }; } // namespace crane -#endif //CRANE_PLANNER_PLUGINS__MOVE_TO_GEOMETRY_HPP +#endif // CRANE_ROBOT_SKILLS__MOVE_TO_GEOMETRY_HPP_ From 6ce30f04569b1801841cde2a9b74de6e38adfb0d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 16 Oct 2023 21:53:12 +0900 Subject: [PATCH 09/26] =?UTF-8?q?=E3=82=B3=E3=83=9F=E3=83=83=E3=83=88?= =?UTF-8?q?=E6=BC=8F=E3=82=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/robot_skills.cpp | 16 ++++++++++++++++ crane_robot_skills/src/robot_skills_node.cpp | 18 ++++++++++++++++++ 2 files changed, 34 insertions(+) create mode 100644 crane_robot_skills/src/robot_skills.cpp create mode 100644 crane_robot_skills/src/robot_skills_node.cpp diff --git a/crane_robot_skills/src/robot_skills.cpp b/crane_robot_skills/src/robot_skills.cpp new file mode 100644 index 000000000..e9976677b --- /dev/null +++ b/crane_robot_skills/src/robot_skills.cpp @@ -0,0 +1,16 @@ +// 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 + +namespace crane +{ +template class MoveToGeometry; +template class MoveToGeometry; +} // namespace crane + +//#include +//RCLCPP_COMPONENTS_REGISTER_NODE(crane::WorldModelPublisherComponent) diff --git a/crane_robot_skills/src/robot_skills_node.cpp b/crane_robot_skills/src/robot_skills_node.cpp new file mode 100644 index 000000000..6f3036ddf --- /dev/null +++ b/crane_robot_skills/src/robot_skills_node.cpp @@ -0,0 +1,18 @@ +// 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 "crane_world_model_publisher/world_model_publisher.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(rclcpp::NodeOptions()); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From ee8ea3a7da1d561acdc1e005a844615f166ef62f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 17 Oct 2023 21:50:20 +0900 Subject: [PATCH 10/26] =?UTF-8?q?Idle=E3=82=B9=E3=82=AD=E3=83=AB=E5=AE=9F?= =?UTF-8?q?=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/idle.hpp | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) create mode 100644 crane_robot_skills/include/crane_robot_skills/idle.hpp diff --git a/crane_robot_skills/include/crane_robot_skills/idle.hpp b/crane_robot_skills/include/crane_robot_skills/idle.hpp new file mode 100644 index 000000000..57b8b1ca9 --- /dev/null +++ b/crane_robot_skills/include/crane_robot_skills/idle.hpp @@ -0,0 +1,35 @@ +// 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. + +#ifndef CRANE_ROBOT_SKILLS__IDLE_HPP_ +#define CRANE_ROBOT_SKILLS__IDLE_HPP_ + +#include +#include + +namespace crane +{ +class Idle : public SkillBase<> +{ +public: + explicit Idle( + uint8_t id, std::shared_ptr & world_model) + : SkillBase<>("idle", id, world_model, DefaultStates::DEFAULT) + { + addStateFunction( + DefaultStates::DEFAULT, + [this]( + const std::shared_ptr & world_model, + const std::shared_ptr & robot, + crane::RobotCommandWrapper & command) -> SkillBase::Status { + // TODO: モーターをOFFにするようにしたほうがバッテリーに優しいかも + command.stopHere(); + return SkillBase::Status::RUNNING; + }); + } +}; +} // namespace crane +#endif // CRANE_ROBOT_SKILLS__IDLE_HPP_ From a43af09a8066126432d057f07aaaaaecd8776ebc Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 17 Oct 2023 21:50:49 +0900 Subject: [PATCH 11/26] =?UTF-8?q?=E3=81=84=E3=81=8F=E3=81=A4=E3=81=8B?= =?UTF-8?q?=E3=81=AE=E3=82=B9=E3=82=AD=E3=83=AB=E3=81=AE=E7=A9=BA=E5=AE=9F?= =?UTF-8?q?=E8=A3=85=E3=82=92=E7=94=A8=E6=84=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/get_ball_contact.hpp | 33 +++++++++++++++++++ .../crane_robot_skills/move_to_geometry.hpp | 5 +++ .../crane_robot_skills/move_with_ball.hpp | 33 +++++++++++++++++++ crane_robot_skills/src/robot_skills.cpp | 3 ++ 4 files changed, 74 insertions(+) create mode 100644 crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp create mode 100644 crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp 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 new file mode 100644 index 000000000..88e65d3ef --- /dev/null +++ b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp @@ -0,0 +1,33 @@ +// 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. + +#ifndef CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ +#define CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ + +#include +#include + +namespace crane +{ +class GetBallContact : public SkillBase<> +{ +public: + explicit GetBallContact( + uint8_t id, std::shared_ptr & world_model) + : SkillBase<>("get_ball_contact", id, world_model, DefaultStates::DEFAULT) + { + addStateFunction( + DefaultStates::DEFAULT, + [this]( + const std::shared_ptr & world_model, + const std::shared_ptr & robot, + crane::RobotCommandWrapper & command) -> SkillBase::Status { + return SkillBase::Status::RUNNING; + }); + } +}; +} // namespace crane +#endif // CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ 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 592578a82..eff64ba1b 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 @@ -43,6 +43,11 @@ class MoveToGeometry : public SkillBase<> return result.closest_point; } + void updateGeometry(Geometry geometry) + { + this->geometry = geometry; + } + protected: 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 new file mode 100644 index 000000000..e2eeaa0b4 --- /dev/null +++ b/crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp @@ -0,0 +1,33 @@ +// 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. + +#ifndef CRANE_ROBOT_SKILLS__MOVE_WITH_BALL_HPP_ +#define CRANE_ROBOT_SKILLS__MOVE_WITH_BALL_HPP_ + +#include +#include + +namespace crane +{ +class MoveWithBall : public SkillBase<> +{ +public: + explicit MoveWithBall( + uint8_t id, std::shared_ptr & world_model) + : SkillBase<>("move_with_ball", id, world_model, DefaultStates::DEFAULT) + { + addStateFunction( + DefaultStates::DEFAULT, + [this]( + const std::shared_ptr & world_model, + const std::shared_ptr & robot, + crane::RobotCommandWrapper & command) -> SkillBase::Status { + return SkillBase::Status::RUNNING; + }); + } +}; +} // namespace crane +#endif // CRANE_ROBOT_SKILLS__MOVE_WITH_BALL_HPP_ diff --git a/crane_robot_skills/src/robot_skills.cpp b/crane_robot_skills/src/robot_skills.cpp index e9976677b..4a5a7650a 100644 --- a/crane_robot_skills/src/robot_skills.cpp +++ b/crane_robot_skills/src/robot_skills.cpp @@ -5,6 +5,9 @@ // https://opensource.org/licenses/MIT. #include +#include +#include +#include namespace crane { From fe8a1b858068d7029d05cb2e158688f52317dd6c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 17 Oct 2023 21:51:05 +0900 Subject: [PATCH 12/26] =?UTF-8?q?=E3=82=AD=E3=83=83=E3=82=AB=E3=83=BC?= =?UTF-8?q?=E3=81=AE=E4=BD=8D=E7=BD=AE=E3=82=92=E5=8F=96=E5=BE=97=E3=81=99?= =?UTF-8?q?=E3=82=8B=E9=96=A2=E6=95=B0=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_msg_wrappers/world_model_wrapper.hpp | 2 ++ 1 file changed, 2 insertions(+) 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 40e7d97dd..22ab859da 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 @@ -30,6 +30,8 @@ struct RobotInfo bool available = false; using SharedPtr = std::shared_ptr; + + Point kicker_center() const { return pose.pos + Point(cos(pose.theta), sin(pose.theta)) * 0.055 ; } }; struct TeamInfo From 8c35448318826f1d8eb4e0819a49ece7c34aa550 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 17 Oct 2023 22:12:16 +0900 Subject: [PATCH 13/26] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=81=AE?= =?UTF-8?q?=E5=88=A4=E5=AE=9A=E9=96=A2=E6=95=B0=E3=82=92=E8=89=B2=E3=80=85?= =?UTF-8?q?=E3=81=A8=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model_wrapper.hpp | 50 ++++++++++++++++++- 1 file changed, 49 insertions(+), 1 deletion(-) 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 22ab859da..9c3e81c0c 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 @@ -31,7 +31,7 @@ struct RobotInfo using SharedPtr = std::shared_ptr; - Point kicker_center() const { return pose.pos + Point(cos(pose.theta), sin(pose.theta)) * 0.055 ; } + Point kicker_center() const { return pose.pos + Point(cos(pose.theta), sin(pose.theta)) * 0.055; } }; struct TeamInfo @@ -63,6 +63,31 @@ struct TeamInfo } }; +struct Hysteresis +{ + Hysteresis(double lower, double upper) : lower_threshold(lower), upper_threshold(upper){}; + + double lower_threshold, upper_threshold; + + bool is_high = false; + + 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(); + } + } +}; + struct Ball { Point pos; @@ -70,6 +95,28 @@ struct Ball Point vel; bool is_curve; + + bool isMoving(double threshold_velocity = 0.01) const { return vel.norm() > threshold_velocity; } + + bool isStopped(double threshold_velocity = 0.01) const + { + return not isMoving(threshold_velocity); + } + + 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); + } + } + +private: + Hysteresis ball_speed_hysteresis = Hysteresis(0.1, 0.6); + friend class WorldModelWrapper; }; struct RobotIdentifier @@ -134,6 +181,7 @@ struct WorldModelWrapper 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()); // ball.is_curve = world_model.ball_info.curved; field_size << world_model.field_info.x, world_model.field_info.y; From 3bfc32fb9da9d32aee2b5bf25f9a3b36dfcf07c4 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 22 Oct 2023 13:59:43 +0900 Subject: [PATCH 14/26] =?UTF-8?q?BallContact.msg=E3=82=92=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_msgs/CMakeLists.txt | 1 + crane_msgs/msg/world_model/BallContact.msg | 9 +++++++++ crane_msgs/msg/world_model/RobotInfoOurs.msg | 2 ++ crane_msgs/msg/world_model/RobotInfoTheirs.msg | 2 ++ 4 files changed, 14 insertions(+) create mode 100755 crane_msgs/msg/world_model/BallContact.msg diff --git a/crane_msgs/CMakeLists.txt b/crane_msgs/CMakeLists.txt index bb0caba35..67f80b030 100755 --- a/crane_msgs/CMakeLists.txt +++ b/crane_msgs/CMakeLists.txt @@ -16,6 +16,7 @@ ament_auto_find_build_dependencies() set(msg_files "msg/world_model/BallInfo.msg" + "msg/world_model/BallContact.msg" "msg/world_model/DefenseArea.msg" "msg/world_model/FieldSize.msg" "msg/world_model/RobotInfo.msg" diff --git a/crane_msgs/msg/world_model/BallContact.msg b/crane_msgs/msg/world_model/BallContact.msg new file mode 100755 index 000000000..ebc5f9628 --- /dev/null +++ b/crane_msgs/msg/world_model/BallContact.msg @@ -0,0 +1,9 @@ + +builtin_interfaces/Time last_contacted_end_time +builtin_interfaces/Time last_contacted_start_time +builtin_interfaces/Time current_time + +# Visionの情報を元に判定しているかどうか +# True: Visionの情報を元に判定している +# False: Robotのドリブラセンサの情報を元に判定している +bool is_vision_source diff --git a/crane_msgs/msg/world_model/RobotInfoOurs.msg b/crane_msgs/msg/world_model/RobotInfoOurs.msg index b987b35f0..8e50addc7 100755 --- a/crane_msgs/msg/world_model/RobotInfoOurs.msg +++ b/crane_msgs/msg/world_model/RobotInfoOurs.msg @@ -8,3 +8,5 @@ bool disappeared uint8 id geometry_msgs/Pose2D pose geometry_msgs/Pose2D velocity + +BallContact ball_contact diff --git a/crane_msgs/msg/world_model/RobotInfoTheirs.msg b/crane_msgs/msg/world_model/RobotInfoTheirs.msg index 9cb53e823..5eb6a1626 100755 --- a/crane_msgs/msg/world_model/RobotInfoTheirs.msg +++ b/crane_msgs/msg/world_model/RobotInfoTheirs.msg @@ -8,3 +8,5 @@ bool disappeared uint8 id geometry_msgs/Pose2D pose geometry_msgs/Pose2D velocity + +BallContact ball_contact From c5bba75e541a96d262c4536ddfc064cc034119f0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 22 Oct 2023 14:20:50 +0900 Subject: [PATCH 15/26] =?UTF-8?q?WorldModel=E3=81=ABBallContact=E6=83=85?= =?UTF-8?q?=E5=A0=B1=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_msgs/msg/world_model/BallContact.msg | 3 +- crane_msgs/msg/world_model/RobotInfo.msg | 2 + .../world_model_publisher.hpp | 2 + .../src/world_model_publisher.cpp | 52 ++++++++++++++++--- 4 files changed, 49 insertions(+), 10 deletions(-) diff --git a/crane_msgs/msg/world_model/BallContact.msg b/crane_msgs/msg/world_model/BallContact.msg index ebc5f9628..0df5b7956 100755 --- a/crane_msgs/msg/world_model/BallContact.msg +++ b/crane_msgs/msg/world_model/BallContact.msg @@ -1,6 +1,5 @@ -builtin_interfaces/Time last_contacted_end_time -builtin_interfaces/Time last_contacted_start_time +builtin_interfaces/Time last_contacted_time builtin_interfaces/Time current_time # Visionの情報を元に判定しているかどうか diff --git a/crane_msgs/msg/world_model/RobotInfo.msg b/crane_msgs/msg/world_model/RobotInfo.msg index 624d8a8c4..60f08fd67 100755 --- a/crane_msgs/msg/world_model/RobotInfo.msg +++ b/crane_msgs/msg/world_model/RobotInfo.msg @@ -21,3 +21,5 @@ geometry_msgs/Pose2D last_detection_pose # If a robot disappear from field, this field is set to true bool disappeared + +BallContact ball_contact diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp index 13ff37e6d..44788e80e 100644 --- a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp +++ b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp @@ -82,6 +82,8 @@ class WorldModelPublisherComponent : public rclcpp::Node private: void publishWorldModel(); + void updateBallContact(); + std::string team_name; Color our_color; diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index d7b636047..1d6455f2e 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -11,19 +11,23 @@ namespace crane WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOptions & options) : rclcpp::Node("world_model_publisher", options) { - sub_vision = this->create_subscription( + sub_vision = create_subscription( "/detection_tracked", 1, - std::bind( - &WorldModelPublisherComponent::visionDetectionsCallback, this, std::placeholders::_1)); - sub_geometry = this->create_subscription( - "/geometry", 1, - std::bind(&WorldModelPublisherComponent::visionGeometryCallback, this, std::placeholders::_1)); + [this](const robocup_ssl_msgs::msg::TrackedFrame::SharedPtr msg) -> void { + visionDetectionsCallback(msg); + }); + + sub_geometry = create_subscription( + "/geometry", 1, [this](const robocup_ssl_msgs::msg::GeometryData::SharedPtr msg) { + visionGeometryCallback(msg); + }); pub_world_model = create_publisher("/world_model", 1); using namespace std::chrono_literals; - timer = this->create_wall_timer( - 16ms, std::bind(&WorldModelPublisherComponent::publishWorldModel, this)); + + timer = this->create_wall_timer(16ms, [this]() { publishWorldModel(); }); + max_id = 16; robot_info[static_cast(Color::BLUE)].resize(max_id); robot_info[static_cast(Color::YELLOW)].resize(max_id); @@ -142,12 +146,15 @@ void WorldModelPublisherComponent::publishWorldModel() wm.is_yellow = (our_color == Color::YELLOW); wm.ball_info = ball_info; + updateBallContact(); + for (auto robot : robot_info[static_cast(our_color)]) { crane_msgs::msg::RobotInfoOurs info; info.id = robot.robot_id; info.disappeared = !robot.detected; info.pose = robot.pose; info.velocity = robot.velocity; + info.ball_contact = robot.ball_contact; wm.robot_info_ours.emplace_back(info); } for (auto robot : robot_info[static_cast(their_color)]) { @@ -156,6 +163,7 @@ void WorldModelPublisherComponent::publishWorldModel() info.disappeared = !robot.detected; info.pose = robot.pose; info.velocity = robot.velocity; + info.ball_contact = robot.ball_contact; wm.robot_info_theirs.emplace_back(info); } @@ -171,6 +179,34 @@ void WorldModelPublisherComponent::publishWorldModel() pub_world_model->publish(wm); } +void WorldModelPublisherComponent::updateBallContact() +{ + auto now = rclcpp::Clock().now(); + for (auto & robot : robot_info[static_cast(our_color)]) { + auto & contact = robot.ball_contact; + // TODO: ロボットのドリブラセンサを使った判定を実装する + contact.current_time = now; + if (robot.detected) { + auto ball_dist = std::hypot(ball_info.pose.x - robot.pose.x, ball_info.pose.y - robot.pose.y); + contact.is_vision_source = true; + if (ball_dist < 0.1) { + contact.last_contacted_time = contact.current_time; + } + } + } + + for (auto & robot : robot_info[static_cast(their_color)]) { + auto & contact = robot.ball_contact; + contact.current_time = now; + if (robot.detected) { + auto ball_dist = std::hypot(ball_info.pose.x - robot.pose.x, ball_info.pose.y - robot.pose.y); + contact.is_vision_source = true; + if (ball_dist < 0.1) { + contact.last_contacted_time = contact.current_time; + } + } + } +} } // namespace crane #include From 86999ddd105a30fb2adfa23770d42ae5a918dcd1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 23 Oct 2023 02:01:16 +0900 Subject: [PATCH 16/26] =?UTF-8?q?WorldModelWrapper=E3=81=AB=E3=82=82BallCo?= =?UTF-8?q?ntact=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model_wrapper.hpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) 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 9c3e81c0c..d222e82c4 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 @@ -17,6 +17,24 @@ #include "crane_geometry/geometry_operations.hpp" #include "crane_msgs/msg/world_model.hpp" +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; + }else{ + last_contact_start_time = now; + } + } + + auto getContactDuration(){ + return (last_contact_end_time - last_contact_start_time); + } +}; + namespace crane { struct RobotInfo @@ -32,6 +50,8 @@ struct RobotInfo using SharedPtr = std::shared_ptr; Point kicker_center() const { return pose.pos + Point(cos(pose.theta), sin(pose.theta)) * 0.055; } + + BallContact ball_contact; }; struct TeamInfo @@ -160,10 +180,13 @@ struct WorldModelWrapper 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); } } @@ -172,10 +195,13 @@ struct WorldModelWrapper 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); } } From 977cd5e28a92d8c16ae501f27af9369178df1c5c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 23 Oct 2023 02:01:37 +0900 Subject: [PATCH 17/26] =?UTF-8?q?=E9=80=94=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/get_ball_contact.hpp | 11 +++++++++++ 1 file changed, 11 insertions(+) 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 88e65d3ef..a20b4e355 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 @@ -25,9 +25,20 @@ class GetBallContact : public SkillBase<> const std::shared_ptr & world_model, const std::shared_ptr & robot, crane::RobotCommandWrapper & command) -> SkillBase::Status { + using std::chrono::seconds; + if(robot->ball_contact.getContactDuration() > seconds(MINIMUM_CONTACT_DURATION)){ + return SkillBase::Status::SUCCESS; + } + robot->ball_contact.getContactDuration() return SkillBase::Status::RUNNING; }); } +private: + std::optional last_contact_start_time; + builtin_interfaces::msg::Time last_contact_time; + Point last_contact_point; + + constexpr static double MINIMUM_CONTACT_DURATION = 0.5; }; } // namespace crane #endif // CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ From f90200c7b6d0a753074b98a7b0ae60f3449d5fad Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 23 Oct 2023 22:15:12 +0900 Subject: [PATCH 18/26] =?UTF-8?q?=E3=83=89=E3=83=AA=E3=83=96=E3=83=A9?= =?UTF-8?q?=E3=81=AE=E4=BD=8D=E7=BD=AE=E3=81=A7=E5=8F=B8=E4=BB=A4=E3=81=A7?= =?UTF-8?q?=E3=81=8D=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_msg_wrappers/robot_command_wrapper.hpp | 9 +++++++-- .../include/crane_msg_wrappers/world_model_wrapper.hpp | 4 +++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp index 5aef46890..ad60b690f 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp @@ -24,11 +24,10 @@ struct RobotCommandWrapper { typedef std::shared_ptr SharedPtr; - RobotCommandWrapper(uint8_t id, WorldModelWrapper::SharedPtr world_model_wrapper) + RobotCommandWrapper(uint8_t id, WorldModelWrapper::SharedPtr world_model_wrapper) : robot(world_model_wrapper->getRobot({true, id})) { latest_msg.robot_id = id; - const auto & robot = world_model_wrapper->getRobot({true, id}); latest_msg.current_pose.x = robot->pose.pos.x(); latest_msg.current_pose.y = robot->pose.pos.y(); latest_msg.current_pose.theta = robot->pose.theta; @@ -104,6 +103,10 @@ struct RobotCommandWrapper return *this; } + RobotCommandWrapper & setDribblerTargetPosition(Point position){ + return setTargetPosition(position - robot->center_to_kicker()); + } + RobotCommandWrapper & setTargetPosition(Point position) { return setTargetPosition(position.x(), position.y()); @@ -217,6 +220,8 @@ struct RobotCommandWrapper crane_msgs::msg::RobotCommand & getEditableMsg() { return latest_msg; } crane_msgs::msg::RobotCommand latest_msg; + + const std::shared_ptr robot; }; } // namespace crane 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 d222e82c4..e501a7d06 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 @@ -49,7 +49,9 @@ struct RobotInfo using SharedPtr = std::shared_ptr; - Point kicker_center() const { return pose.pos + Point(cos(pose.theta), sin(pose.theta)) * 0.055; } + Vector2 center_to_kicker() const { return Vector2 (cos(pose.theta), sin(pose.theta)) * 0.055; } + + Point kicker_center() const { return pose.pos + center_to_kicker(); } BallContact ball_contact; }; From 85771e29c1a777a971db4d2f60fc08ed6472b3b0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 23 Oct 2023 22:16:27 +0900 Subject: [PATCH 19/26] =?UTF-8?q?=E5=A4=9A=E5=88=86GetBallContact=E3=81=A7?= =?UTF-8?q?=E3=81=8D=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/get_ball_contact.hpp | 55 +++++++++++++++++-- 1 file changed, 49 insertions(+), 6 deletions(-) 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 a20b4e355..4c3115377 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 @@ -15,8 +15,7 @@ namespace crane class GetBallContact : public SkillBase<> { public: - explicit GetBallContact( - uint8_t id, std::shared_ptr & world_model) + explicit GetBallContact(uint8_t id, std::shared_ptr & world_model) : SkillBase<>("get_ball_contact", id, world_model, DefaultStates::DEFAULT) { addStateFunction( @@ -25,19 +24,63 @@ class GetBallContact : public SkillBase<> const std::shared_ptr & world_model, const std::shared_ptr & robot, crane::RobotCommandWrapper & command) -> SkillBase::Status { - using std::chrono::seconds; - if(robot->ball_contact.getContactDuration() > seconds(MINIMUM_CONTACT_DURATION)){ + // 規定時間以上接していたらOK + if ( + robot->ball_contact.getContactDuration() > + std::chrono::duration(MINIMUM_CONTACT_DURATION)) { return SkillBase::Status::SUCCESS; + } else { + double distance = (robot->pose.pos - world_model->ball.pos).norm(); + + double target_distance = std::max(distance - 0.1, 0.0); + + auto approach_vec = getApproachNormVec(world_model, robot); + command.setDribblerTargetPosition(world_model->ball.pos - approach_vec * target_distance); + command.setTargetTheta(getAngle(approach_vec)); + return SkillBase::Status::RUNNING; } - robot->ball_contact.getContactDuration() - return SkillBase::Status::RUNNING; }); } + private: + Vector2 getApproachNormVec( + const std::shared_ptr & world_model, + const std::shared_ptr & robot) + { + // 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(); + } + }(); + } + + Point getTargetPoint(std::shared_ptr & robot) + { + robot->kicker_center(); + return Point(); + } std::optional last_contact_start_time; builtin_interfaces::msg::Time last_contact_time; Point last_contact_point; + // double target_distance = 0.0; + constexpr static double MINIMUM_CONTACT_DURATION = 0.5; }; } // namespace crane From 30e476cd17046b1ff4b0cc308ef754151ce185a9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 23 Oct 2023 22:16:58 +0900 Subject: [PATCH 20/26] =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../robot_command_wrapper.hpp | 6 +++-- .../world_model_wrapper.hpp | 26 ++++++++++--------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp index ad60b690f..b3962a31f 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp @@ -24,7 +24,8 @@ struct RobotCommandWrapper { typedef std::shared_ptr SharedPtr; - RobotCommandWrapper(uint8_t id, WorldModelWrapper::SharedPtr world_model_wrapper) : robot(world_model_wrapper->getRobot({true, id})) + RobotCommandWrapper(uint8_t id, WorldModelWrapper::SharedPtr world_model_wrapper) + : robot(world_model_wrapper->getRobot({true, id})) { latest_msg.robot_id = id; @@ -103,7 +104,8 @@ struct RobotCommandWrapper return *this; } - RobotCommandWrapper & setDribblerTargetPosition(Point position){ + RobotCommandWrapper & setDribblerTargetPosition(Point position) + { return setTargetPosition(position - robot->center_to_kicker()); } 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 e501a7d06..ee793b102 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 @@ -17,22 +17,22 @@ #include "crane_geometry/geometry_operations.hpp" #include "crane_msgs/msg/world_model.hpp" -struct BallContact{ +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){ + void update(bool is_contacted) + { auto now = std::chrono::system_clock::now(); - if(is_contacted){ + if (is_contacted) { last_contact_end_time = now; - }else{ + } else { last_contact_start_time = now; } } - auto getContactDuration(){ - return (last_contact_end_time - last_contact_start_time); - } + auto getContactDuration() { return (last_contact_end_time - last_contact_start_time); } }; namespace crane @@ -49,7 +49,7 @@ struct RobotInfo using SharedPtr = std::shared_ptr; - Vector2 center_to_kicker() const { return Vector2 (cos(pose.theta), sin(pose.theta)) * 0.055; } + Vector2 center_to_kicker() const { return Vector2(cos(pose.theta), sin(pose.theta)) * 0.055; } Point kicker_center() const { return pose.pos + center_to_kicker(); } @@ -182,12 +182,13 @@ struct WorldModelWrapper 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->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{ + } else { info->ball_contact.update(false); } } @@ -197,12 +198,13 @@ struct WorldModelWrapper 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->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{ + } else { info->ball_contact.update(false); } } From 1f0c3d2edcd67d5d4d98d3fbeab4c83b8d0369e5 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 23 Oct 2023 22:39:41 +0900 Subject: [PATCH 21/26] =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/idle.hpp | 3 +- .../crane_robot_skills/move_to_geometry.hpp | 5 +--- .../crane_robot_skills/move_with_ball.hpp | 3 +- crane_robot_skills/src/robot_skills.cpp | 4 +-- .../tigers_goalie_planner.hpp | 29 ++++++++++++------- 5 files changed, 23 insertions(+), 21 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/idle.hpp b/crane_robot_skills/include/crane_robot_skills/idle.hpp index 57b8b1ca9..07a707c21 100644 --- a/crane_robot_skills/include/crane_robot_skills/idle.hpp +++ b/crane_robot_skills/include/crane_robot_skills/idle.hpp @@ -15,8 +15,7 @@ namespace crane class Idle : public SkillBase<> { public: - explicit Idle( - uint8_t id, std::shared_ptr & world_model) + explicit Idle(uint8_t id, std::shared_ptr & world_model) : SkillBase<>("idle", id, world_model, DefaultStates::DEFAULT) { addStateFunction( 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 eff64ba1b..1e90a882e 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 @@ -43,10 +43,7 @@ class MoveToGeometry : public SkillBase<> return result.closest_point; } - void updateGeometry(Geometry geometry) - { - this->geometry = geometry; - } + void updateGeometry(Geometry geometry) { this->geometry = geometry; } protected: 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 e2eeaa0b4..761c2235e 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 @@ -15,8 +15,7 @@ namespace crane class MoveWithBall : public SkillBase<> { public: - explicit MoveWithBall( - uint8_t id, std::shared_ptr & world_model) + explicit MoveWithBall(uint8_t id, std::shared_ptr & world_model) : SkillBase<>("move_with_ball", id, world_model, DefaultStates::DEFAULT) { addStateFunction( diff --git a/crane_robot_skills/src/robot_skills.cpp b/crane_robot_skills/src/robot_skills.cpp index 4a5a7650a..c864bb2c5 100644 --- a/crane_robot_skills/src/robot_skills.cpp +++ b/crane_robot_skills/src/robot_skills.cpp @@ -4,9 +4,9 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. -#include -#include #include +#include +#include #include namespace crane 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 b16ee410d..9f9dcf798 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 @@ -153,7 +153,7 @@ class TigersGoaliePlanner : public rclcpp::Node, public PlannerBase } break; } - case State::GET_BALL_CONTACT:{ + case State::GET_BALL_CONTACT: { // doGetBallContact Status status; if (status == Status::SUCCESS) { @@ -189,15 +189,19 @@ class TigersGoaliePlanner : public rclcpp::Node, public PlannerBase return Status::SUCCESS; } - bool isBallMoveToweredTo(Point point){ - double dot = (point - world_model->ball.pos).normalized().dot(world_model->ball.vel.normalized()); - return dot > 0.5 or (point - world_model->ball.pos).norm() < 0.2;; - + bool isBallMoveToweredTo(Point point) + { + double dot = + (point - world_model->ball.pos).normalized().dot(world_model->ball.vel.normalized()); + return dot > 0.5 or (point - world_model->ball.pos).norm() < 0.2; + ; } - bool isBallAimedForGoal(){ + bool isBallAimedForGoal() + { Segment goal_line{world_model->getOurGoalPosts().first, world_model->getOurGoalPosts().second}; - Segment ball_line{world_model->ball.pos, world_model->ball.pos + world_model->ball.vel.normalized() * 10.0}; + Segment ball_line{ + world_model->ball.pos, world_model->ball.pos + world_model->ball.vel.normalized() * 10.0}; return boost::geometry::intersects(goal_line, ball_line); } @@ -215,16 +219,19 @@ class TigersGoaliePlanner : public rclcpp::Node, public PlannerBase bool isOutsidePenaltyArea() const { return false; } - bool canInterceptSafely() { + bool canInterceptSafely() + { return false; -// return world_model->isDefenseArea(world_model->ball.pos) && (not isBallAimedForGoal()); + // return world_model->isDefenseArea(world_model->ball.pos) && (not isBallAimedForGoal()); } bool isBallMoving() const { return false; } bool isBallPlacementRequired() const { return false; } - bool hasInterceptionFailed(const std::shared_ptr & robot) { - return isBallMoveToweredTo(robot->pose.pos) or not world_model->isDefenseArea(world_model->ball.pos); + bool hasInterceptionFailed(const std::shared_ptr & robot) + { + return isBallMoveToweredTo(robot->pose.pos) or + not world_model->isDefenseArea(world_model->ball.pos); } bool isGoalKick() const { return false; } From 9778f57a438ffeb88e0ceb46bac36b4ff2e4aa05 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 24 Oct 2023 23:17:02 +0900 Subject: [PATCH 22/26] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=81=AE?= =?UTF-8?q?=E3=82=B3=E3=83=B3=E3=82=BF=E3=82=AF=E3=83=88=E5=B1=A5=E6=AD=B4?= =?UTF-8?q?=E3=81=AE=E6=9C=89=E7=84=A1=E3=82=92=E6=A4=9C=E7=B4=A2=E3=81=99?= =?UTF-8?q?=E3=82=8B=E9=96=A2=E6=95=B0=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model_wrapper.hpp | 26 ++++++++++++++----- 1 file changed, 19 insertions(+), 7 deletions(-) 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 ee793b102..778d07eef 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 @@ -33,14 +33,29 @@ struct BallContact } auto getContactDuration() { return (last_contact_end_time - last_contact_start_time); } + + auto findPastContact(double duration_sec) + { + auto past = std::chrono::system_clock::now() - std::chrono::duration(duration_sec); + return past < last_contact_end_time; + } }; namespace crane { +struct RobotIdentifier +{ + bool is_ours; + + uint8_t robot_id; +}; + struct RobotInfo { uint8_t id; + RobotIdentifier getID() const { return {true, id}; } + Pose2D pose; Velocity2D vel; @@ -141,13 +156,6 @@ struct Ball friend class WorldModelWrapper; }; -struct RobotIdentifier -{ - bool is_ours; - - uint8_t robot_id; -}; - struct WorldModelWrapper { typedef std::shared_ptr SharedPtr; @@ -286,6 +294,10 @@ struct WorldModelWrapper return (getRobot(id)->pose.pos - point).squaredNorm(); } + auto getDistanceFromBall(Point point) -> double { return (ball.pos - point).norm(); } + + auto getSquareDistanceFromBall(Point point) -> double { return (ball.pos - point).squaredNorm(); } + auto getNearestRobotsWithDistanceFromPoint( Point point, std::vector> & robots) -> std::pair, double> From e0ed1b2a9153bacf5afda3cffd5d1cb87cd3de57 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 24 Oct 2023 23:17:22 +0900 Subject: [PATCH 23/26] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E5=BC=95?= =?UTF-8?q?=E6=95=B0=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/get_ball_contact.hpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) 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 4c3115377..5f11604f3 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 @@ -34,7 +34,7 @@ class GetBallContact : public SkillBase<> double target_distance = std::max(distance - 0.1, 0.0); - auto approach_vec = getApproachNormVec(world_model, robot); + auto approach_vec = getApproachNormVec(); command.setDribblerTargetPosition(world_model->ball.pos - approach_vec * target_distance); command.setTargetTheta(getAngle(approach_vec)); return SkillBase::Status::RUNNING; @@ -43,9 +43,7 @@ class GetBallContact : public SkillBase<> } private: - Vector2 getApproachNormVec( - const std::shared_ptr & world_model, - const std::shared_ptr & robot) + 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 @@ -70,11 +68,6 @@ class GetBallContact : public SkillBase<> }(); } - Point getTargetPoint(std::shared_ptr & robot) - { - robot->kicker_center(); - return Point(); - } std::optional last_contact_start_time; builtin_interfaces::msg::Time last_contact_time; Point last_contact_point; From e834984a61eccf42ea504e622dca4886e509dcac Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 24 Oct 2023 23:17:51 +0900 Subject: [PATCH 24/26] =?UTF-8?q?MoveWithBallSkill=E3=82=92=E5=AE=9F?= =?UTF-8?q?=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/move_with_ball.hpp | 50 +++++++++++++++++-- 1 file changed, 47 insertions(+), 3 deletions(-) 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 761c2235e..50f166eac 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 @@ -12,11 +12,15 @@ namespace crane { +/** + * ボールを持って移動する + * 前提：GetBallContactが成功していること + */ class MoveWithBall : public SkillBase<> { public: - explicit MoveWithBall(uint8_t id, std::shared_ptr & world_model) - : SkillBase<>("move_with_ball", id, world_model, DefaultStates::DEFAULT) + explicit MoveWithBall(Pose2D pose, uint8_t id, std::shared_ptr & world_model) + : SkillBase<>("move_with_ball", id, world_model, DefaultStates::DEFAULT), target_pose(pose) { addStateFunction( DefaultStates::DEFAULT, @@ -24,9 +28,49 @@ class MoveWithBall : public SkillBase<> const std::shared_ptr & world_model, const std::shared_ptr & robot, crane::RobotCommandWrapper & command) -> SkillBase::Status { - return SkillBase::Status::RUNNING; + if (not robot->ball_contact.findPastContact(2.0)) { + // ボールが離れたら失敗 + return SkillBase::Status::FAILURE; + } else if ( + (robot->pose.pos).norm() < 0.1 && + std::abs(getAngleDiff(robot->pose.theta, target_pose.theta)) < 0.1) { + // ターゲットに到着したら成功 + return SkillBase::Status::SUCCESS; + } else { + command.setTargetPosition(getTargetPoint()); + command.setTargetTheta(getTargetAngle()); + command.dribble(0.5); + return SkillBase::Status::RUNNING; + } }); } + + Point getTargetPoint() + { + // 正しい方向でドリブルできている場合だけ前進 + if (getAngleDiff(robot->pose.theta, getTargetAngle()) < 0.3) { + if (robot->ball_contact.findPastContact(0.5)) { + return robot->pose.pos; + } + } + } + + double getTargetAngle() + { + 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; + } + } + + Pose2D target_pose; }; } // namespace crane #endif // CRANE_ROBOT_SKILLS__MOVE_WITH_BALL_HPP_ From 755ebb5607397c90362a6c796a43f1ae3c681199 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 24 Oct 2023 23:17:58 +0900 Subject: [PATCH 25/26] =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/robot_skills_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_robot_skills/src/robot_skills_node.cpp b/crane_robot_skills/src/robot_skills_node.cpp index 6f3036ddf..351ec4e28 100644 --- a/crane_robot_skills/src/robot_skills_node.cpp +++ b/crane_robot_skills/src/robot_skills_node.cpp @@ -16,3 +16,4 @@ int main(int argc, char * argv[]) rclcpp::shutdown(); return 0; } + From c8cf3394b9fcec9ce671dfcc0bec76b25d70de81 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 4 Nov 2023 16:47:13 +0900 Subject: [PATCH 26/26] =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/robot_skills_node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/crane_robot_skills/src/robot_skills_node.cpp b/crane_robot_skills/src/robot_skills_node.cpp index 351ec4e28..6f3036ddf 100644 --- a/crane_robot_skills/src/robot_skills_node.cpp +++ b/crane_robot_skills/src/robot_skills_node.cpp @@ -16,4 +16,3 @@ int main(int argc, char * argv[]) rclcpp::shutdown(); return 0; } -