Merge branch 'develop' into improve_ball_around

crane_local_planner/CMakeLists.txt

@@ -15,7 +15,6 @@ add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS")
 # find dependencies
 find_package(ament_cmake_auto REQUIRED)
 find_package(rclcpp_components REQUIRED)
-# find_package(backward_ros REQUIRED)
 ament_auto_find_build_dependencies()
 
 ament_auto_add_library(${PROJECT_NAME}_component SHARED
@@ -40,9 +39,6 @@ ament_auto_add_executable(${PROJECT_NAME}_node
   src/${PROJECT_NAME}_node.cpp
 )
 
-# include(/opt/ros/$ENV{ROS_DISTRO}/share/backward_ros/cmake/BackwardConfig.cmake)
-# add_backward(${PROJECT_NAME}_node)
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

crane_local_planner/package.xml

@@ -9,7 +9,6 @@
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 
-  <!-- <depend>backward_ros</depend> -->
   <depend>closest_point_vendor</depend>
   <depend>crane_msg_wrappers</depend>
   <depend>crane_msgs</depend>

crane_robot_skills/include/crane_robot_skills/attacker.hpp

@@ -23,7 +23,6 @@ namespace crane::skills
 enum class AttackerState {
   ENTRY_POINT,
   FORCED_PASS,
-  CUT_THEIR_PASS,
   STEAL_BALL,
   REDIRECT_GOAL_KICK,
   GOAL_KICK,

crane_robot_skills/src/attacker.cpp

@@ -60,22 +60,32 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
         // パス
         command.disableBallAvoidance();
         kick_skill.setParameter("dot_threshold", 0.95);
-        kick_skill.setParameter("kick_power", 0.8);
         int receiver_id = getParameter<int>("receiver_id");
         if (receiver_id != -1) {
           kick_target = world_model()->getOurRobot(receiver_id)->pose.pos;
         }
         kick_skill.setParameter("target", kick_target);
         Segment kick_line{world_model()->ball.pos, kick_target};
         // 近くに敵ロボットがいればチップキック
+        bool chip_kick = false;
         if (const auto enemy_robots = world_model()->theirs.getAvailableRobots();
             not enemy_robots.empty()) {
           const auto & [nearest_enemy, enemy_distance] =
             world_model()->getNearestRobotWithDistanceFromSegment(kick_line, enemy_robots);
           if (enemy_distance < 0.4 && nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) {
-            kick_skill.setParameter("kick_with_chip", true);
+            chip_kick = true;
           }
         }
+        if (chip_kick) {
+          kick_skill.setParameter("kick_with_chip", true);
+          kick_skill.setParameter("kick_power", 0.9);
+          kick_skill.setParameter("with_dribble", true);
+          kick_skill.setParameter("dribble_power", 0.7);
+        } else {
+          kick_skill.setParameter("kick_power", 0.2);
+          kick_skill.setParameter("kick_with_chip", false);
+          kick_skill.setParameter("dribble_power", 0.0);
+        }
         kick_skill.run();
         break;
       }
@@ -85,23 +95,6 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     return Status::RUNNING;
   });
 
-  addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool {
-    return not world_model()->isOurBallByBallOwnerCalculator() &&
-           world_model()->ball.isMoving(0.2) &&
-           world_model()->ball.isMovingTowards(robot()->pose.pos);
-  });
-
-  addTransition(AttackerState::CUT_THEIR_PASS, AttackerState::ENTRY_POINT, [this]() -> bool {
-    return world_model()->isOurBallByBallOwnerCalculator() or world_model()->ball.isStopped(0.2);
-  });
-
-  addStateFunction(AttackerState::CUT_THEIR_PASS, [this]() -> Status {
-    visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5);
-    receive_skill.setParameter("enable_redirect", false);
-    receive_skill.setParameter("policy", std::string("min_slack"));
-    return receive_skill.run();
-  });
-
   addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool {
     // 止まっているボールを相手が持っているとき
     const auto enemy_robots = world_model()->theirs.getAvailableRobots();
@@ -168,10 +161,10 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
       }
     }();
 
-    receive_skill.setParameter("enable_redirect", false);
+    receive_skill.setParameter("enable_redirect", true);
     receive_skill.setParameter("redirect_target", target);
     receive_skill.setParameter("policy", std::string("closest"));
-    receive_skill.setParameter("redirect_kick_power", 0.8);
+    receive_skill.setParameter("redirect_kick_power", 0.2);
     return receive_skill.run();
   });
 
@@ -288,7 +281,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
         kick_skill.setParameter("kick_with_chip", true);
       }
     }
-    kick_skill.setParameter("kick_power", 0.5);
+    kick_skill.setParameter("kick_power", 0.4);
     kick_skill.setParameter("dot_threshold", 0.97);
     return kick_skill.run();
   });
@@ -361,7 +354,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   });
 
   addStateFunction(AttackerState::RECEIVE_BALL, [this]() -> Status {
-    receive_skill.setParameter("enable_redirect", true);
+    receive_skill.setParameter("enable_redirect", false);
+    receive_skill.setParameter("policy", std::string("closest"));
     receive_skill.setParameter("dribble_power", 0.0);
     receive_skill.setParameter("enable_software_bumper", false);
     return receive_skill.run();

crane_robot_skills/src/kick.cpp

@@ -15,7 +15,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
   phase(getContextReference<std::string>("phase"))
 {
   setParameter("target", Point(0, 0));
-  setParameter("kick_power", 0.5f);
+  setParameter("kick_power", 0.7f);
   setParameter("chip_kick", false);
   setParameter("with_dribble", false);
   setParameter("dribble_power", 0.3f);
@@ -109,7 +109,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
       visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL(遠い)");
       command.setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3)
         .lookAtFrom(target, ball_pos)
-        .setTerminalVelocity(0.1);
+        .setTerminalVelocity(0.3);
       return Status::RUNNING;
     } else {
       visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL（近い）");
@@ -121,9 +121,8 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
       // ボールを避けて回り込む
       using boost::math::constants::degree;
       double ratio =
-        1.0 +
-        std::clamp(
-          0.5 - calculateRatio(robot()->getDistance(world_model()->ball.pos), 0., 2.0), 0., 0.5);
+        1.5 + std::clamp(
+                -calculateRatio(robot()->getDistance(world_model()->ball.pos), 0.2, 1.5), -0.5, 0.);
 
       double move_direction = getAngle(target - robot()->pose.pos) +
                               (getAngleDiff(
@@ -132,8 +131,9 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
                                 ratio;
       Vector2 move_vec = getNormVec(move_direction);
       command.lookAtFrom(target, ball_pos)
-        .setDribblerTargetPosition(robot()->pose.pos + move_vec * 0.1)
-        .setTerminalVelocity(robot()->getDistance(world_model()->ball.pos) * 0.5 + 0.5)
+        .setDribblerTargetPosition(
+          robot()->pose.pos + move_vec * 0.3 + world_model()->ball.vel * 0.4)
+        // .setTerminalVelocity(world_model()->ball.vel.norm())
         .disableCollisionAvoidance()
         .disableBallAvoidance();
 
@@ -143,7 +143,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
         command.kickStraight(getParameter<double>("kick_power"));
       }
       if (getParameter<bool>("with_dribble")) {
-        command.dribble(getParameter<double>("dribble_power"));
+        command.withDribble(getParameter<double>("dribble_power"));
       } else {
         // ドリブラーを止める
         command.withDribble(0.0);

crane_world_model_publisher/src/world_model_publisher.cpp

@@ -275,7 +275,6 @@ void WorldModelPublisherComponent::visionDetectionsCallback(const TrackedFrame &
     int ball_info_history_size = ball_info_history.size();
     if (1 > ball_info_history_size) {
       auto last_ball_info_history = ball_info_history[ball_info_history_size - 1];
-      // double elapsed_time_since_last_detected = tracked_frame.timestamp() - last_ball_info_history.detection_time;
       double elapsed_time_since_last_detected = (now() - last_ball_detect_time).seconds();
 
       // 0.5secビジョンから見えていなければ見失った