ボールアプローチに速度を考慮 (#317)

* ボールアプローチに速度を考慮

* ボールアプローチの改善: しきい値の調整と速度反映箇所の修正

* パラメータの変更

consai_robot_controller/src/tactic/shoot_tactics.cpp

@@ -61,6 +61,12 @@ ShootTactics::ShootTactics()
 
       const auto robot_pose = tools::pose_state(data_set.get_my_robot());
       const auto ball_pose = tools::pose_state(data_set.get_ball());
+      const auto ball_vel = tools::velocity_state(data_set.get_ball());
+      const double dt = 1.0 / 60.0;
+      const double gain = 1.0;
+      // 速度による移動量
+      const double dx = ball_vel.x * dt;
+      const double dy = ball_vel.y * dt;
 
       // セットプレイ時は回転半径を大きくする
       double rotation_radius = ROTATE_RADIUS;
@@ -69,7 +75,7 @@ ShootTactics::ShootTactics()
       }
 
       const tools::Trans trans_BtoR(ball_pose, tools::calc_angle(ball_pose, robot_pose));
-      const auto new_pose = trans_BtoR.inverted_transform(rotation_radius, 0.0, -M_PI);
+      const auto new_pose = trans_BtoR.inverted_transform(rotation_radius + dx, gain * dy, -M_PI);
 
       data_set.set_parsed_pose(new_pose);
       data_set.set_parsed_dribble_power(DRIBBLE_RELEASE);
@@ -92,15 +98,21 @@ ShootTactics::ShootTactics()
 
       const auto robot_pose = tools::pose_state(data_set.get_my_robot());
       const auto ball_pose = tools::pose_state(data_set.get_ball());
+      const auto ball_vel = tools::velocity_state(data_set.get_ball());
       const auto target_pose = data_set.get_target();
+      const double dt = 1.0 / 60.0;
+      const double gain = 20.0;
+      // 速度による移動量
+      const double dx = ball_vel.x * dt;
+      const double dy = ball_vel.y * dt;
 
       // ボールから目標位置を結ぶ直線上で、ロボットがボールを見ながら、ボールの周りを旋回する
       const tools::Trans trans_BtoT(ball_pose, tools::calc_angle(ball_pose, target_pose));
 
       const auto robot_pose_BtoT = trans_BtoT.transform(robot_pose);
       const auto angle_robot_position = tools::calc_angle(State(), robot_pose_BtoT);
 
-      const auto ADD_ANGLE = tools::to_radians(50.0);
+      const auto ADD_ANGLE = tools::to_radians(20.0);
       // FIXME: 下2つとaim_angle_thresholdは不要だったら削除
       // const auto AIM_ANGLE_THRETHOLD = tools::to_radians(20.0);
       // const auto AIM_ANGLE_THRETHOLD_FOR_SETPLAY = tools::to_radians(10.0);
@@ -111,7 +123,7 @@ ShootTactics::ShootTactics()
       // double aim_angle_threshold = AIM_ANGLE_THRETHOLD;
       double omega_threshold = OMEGA_THRESHOLD;
       if (data_set.is_setplay()) {
-        rotation_radius = ROBOT_RADIUS * 4.0;
+        rotation_radius = ROBOT_RADIUS * 10.0;
         forward_distance = ROBOT_RADIUS * 2.0;
         // aim_angle_threshold = AIM_ANGLE_THRETHOLD_FOR_SETPLAY;
         omega_threshold = OMEGA_THRESHOLD_FOR_SETPLAY;
@@ -120,12 +132,12 @@ ShootTactics::ShootTactics()
       State new_pose;
       if (std::fabs(angle_robot_position) + ADD_ANGLE > M_PI) {
         // ボールの裏に回ったら、直進する
-        new_pose = trans_BtoT.inverted_transform(-forward_distance, 0.0, 0.0);
+        new_pose = trans_BtoT.inverted_transform(-forward_distance, gain * dy, 0.0);
       } else {
         // ボールの周りを旋回する
         const auto theta = angle_robot_position + std::copysign(ADD_ANGLE, angle_robot_position);
-        double pos_x = rotation_radius * std::cos(theta);
-        double pos_y = rotation_radius * std::sin(theta);
+        double pos_x = rotation_radius * std::cos(theta) + gain * dx;
+        double pos_y = rotation_radius * std::sin(theta) + gain * dy;
         new_pose = trans_BtoT.inverted_transform(pos_x, pos_y, theta + M_PI);
       }
 

consai_tools/include/consai_tools/geometry_tools.hpp

@@ -35,6 +35,7 @@ double distance(const State & pose1, const State & pose2);
 State pose_state(const TrackedRobot & robot);
 State pose_state(const TrackedBall & ball);
 State velocity_state(const TrackedRobot & robot);
+State velocity_state(const TrackedBall & ball);
 State gen_state(const double x, const double y, const double theta = 0.0);
 double to_radians(const double degrees);
 double to_degrees(const double radians);

consai_tools/src/geometry_tools.cpp

@@ -78,6 +78,17 @@ State velocity_state(const TrackedRobot & robot)
   return state;
 }
 
+State velocity_state(const TrackedBall & ball)
+{
+  State state;
+  if (ball.vel.size() > 0) {
+    state.x = ball.vel[0].x;
+    state.y = ball.vel[0].y;
+  }
+  state.theta = 0.0;
+  return state;
+}
+
 State gen_state(const double x, const double y, const double theta)
 {
   State state;