feature/skills

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",

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"

crane_msgs/msg/world_model/BallContact.msg

@@ -0,0 +1,8 @@
+
+builtin_interfaces/Time last_contacted_time
+builtin_interfaces/Time current_time
+
+# Visionの情報を元に判定しているかどうか
+# True: Visionの情報を元に判定している
+# False: Robotのドリブラセンサの情報を元に判定している
+bool is_vision_source

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

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

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

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

crane_robot_skills/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.5)
+project(crane_robot_skills)
+
+# 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/robot_skills.cpp)
+
+#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)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp

@@ -0,0 +1,80 @@
+// 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 <crane_geometry/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+
+namespace crane
+{
+class GetBallContact : public SkillBase<>
+{
+public:
+  explicit GetBallContact(uint8_t id, std::shared_ptr<WorldModelWrapper> & world_model)
+  : SkillBase<>("get_ball_contact", id, world_model, DefaultStates::DEFAULT)
+  {
+    addStateFunction(
+      DefaultStates::DEFAULT,
+      [this](
+        const std::shared_ptr<WorldModelWrapper> & world_model,
+        const std::shared_ptr<RobotInfo> & robot,
+        crane::RobotCommandWrapper & command) -> SkillBase::Status {
+        // 規定時間以上接していたらOK
+        if (
+          robot->ball_contact.getContactDuration() >
+          std::chrono::duration<double>(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();
+          command.setDribblerTargetPosition(world_model->ball.pos - approach_vec * target_distance);
+          command.setTargetTheta(getAngle(approach_vec));
+          return SkillBase::Status::RUNNING;
+        }
+      });
+  }
+
+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();
+      }
+    }();
+  }
+
+  std::optional<builtin_interfaces::msg::Time> 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
+#endif  // CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_

crane_robot_skills/include/crane_robot_skills/idle.hpp

@@ -0,0 +1,34 @@
+// 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 <crane_geometry/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+
+namespace crane
+{
+class Idle : public SkillBase<>
+{
+public:
+  explicit Idle(uint8_t id, std::shared_ptr<WorldModelWrapper> & world_model)
+  : SkillBase<>("idle", id, world_model, DefaultStates::DEFAULT)
+  {
+    addStateFunction(
+      DefaultStates::DEFAULT,
+      [this](
+        const std::shared_ptr<WorldModelWrapper> & world_model,
+        const std::shared_ptr<RobotInfo> & robot,
+        crane::RobotCommandWrapper & command) -> SkillBase::Status {
+        // TODO: モーターをOFFにするようにしたほうがバッテリーに優しいかも
+        command.stopHere();
+        return SkillBase::Status::RUNNING;
+      });
+  }
+};
+}  // namespace crane
+#endif  // CRANE_ROBOT_SKILLS__IDLE_HPP_

crane_robot_skills/include/crane_robot_skills/move_to_geometry.hpp

@@ -0,0 +1,52 @@
+// 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_TO_GEOMETRY_HPP_
+#define CRANE_ROBOT_SKILLS__MOVE_TO_GEOMETRY_HPP_
+
+#include <crane_geometry/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+
+namespace crane
+{
+template <typename Geometry>
+class MoveToGeometry : public SkillBase<>
+{
+public:
+  explicit MoveToGeometry(
+    uint8_t id, Geometry geometry, std::shared_ptr<WorldModelWrapper> & world_model,
+    const double threshold = 0.1)
+  : SkillBase<>("move_to_geometry", id, world_model, DefaultStates::DEFAULT), geometry(geometry)
+  {
+    addStateFunction(
+      DefaultStates::DEFAULT,
+      [this, threshold](
+        const std::shared_ptr<WorldModelWrapper> & world_model,
+        const std::shared_ptr<RobotInfo> & robot,
+        crane::RobotCommandWrapper & command) -> SkillBase::Status {
+        if ((robot->pose.pos - getTargetPoint()).norm() < threshold) {
+          return SkillBase::Status::SUCCESS;
+        } else {
+          command.setTargetPosition(getTargetPoint());
+          return SkillBase::Status::RUNNING;
+        }
+      });
+  }
+
+  Point getTargetPoint()
+  {
+    ClosestPoint result;
+    bg::closest_point(geometry, robot->pose.pos, result);
+    return result.closest_point;
+  }
+
+  void updateGeometry(Geometry geometry) { this->geometry = geometry; }
+
+protected:
+  Geometry geometry;
+};
+}  // namespace crane
+#endif  // CRANE_ROBOT_SKILLS__MOVE_TO_GEOMETRY_HPP_

crane_robot_skills/include/crane_robot_skills/move_with_ball.hpp

@@ -0,0 +1,76 @@
+// 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 <crane_geometry/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+
+namespace crane
+{
+/**
+ * ボールを持って移動する
+ * 前提：GetBallContactが成功していること
+ */
+class MoveWithBall : public SkillBase<>
+{
+public:
+  explicit MoveWithBall(Pose2D pose, uint8_t id, std::shared_ptr<WorldModelWrapper> & world_model)
+  : SkillBase<>("move_with_ball", id, world_model, DefaultStates::DEFAULT), target_pose(pose)
+  {
+    addStateFunction(
+      DefaultStates::DEFAULT,
+      [this](
+        const std::shared_ptr<WorldModelWrapper> & world_model,
+        const std::shared_ptr<RobotInfo> & robot,
+        crane::RobotCommandWrapper & command) -> SkillBase::Status {
+        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_