練習会での細かな調整・変更・リファクタなど (#525)

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

crane_local_planner/src/gridmap_planner.cpp

@@ -201,7 +201,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand(
           // ほんとうは0.2mだがバッファを0.2mとる
           return 0.2 + 0.2;
         default:
-          return 0.0;
+          return 0.1;
       }
     }();
 
@@ -259,7 +259,7 @@ crane_msgs::msg::RobotCommands GridMapPlanner::calculateRobotCommand(
     map.add("ball");
   }
   map["ball"].setZero();
-  for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.2); !iterator.isPastEnd();
+  for (grid_map::CircleIterator iterator(map, world_model->ball.pos, 0.1); !iterator.isPastEnd();
        ++iterator) {
     map.at("ball", *iterator) = 1.0;
   }

crane_robot_skills/include/crane_robot_skills/kick.hpp

@@ -56,6 +56,8 @@ class Kick : public SkillBase<RobotCommandWrapperPosition>
         }
         if (getParameter<bool>("with_dribble")) {
           command.dribble(getParameter<double>("dribble_power"));
+        } else {
+          command.dribble(0.0);
         }
         // TODO(HansRobo): 終了判定
       } else {

crane_robot_skills/include/crane_robot_skills/teleop.hpp

@@ -152,7 +152,8 @@ class Teleop : public SkillBase<RobotCommandWrapperPosition>, public rclcpp::Nod
     double angular =
       (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_R]) - (1.0 - last_joy_msg.axes[AXIS_VEL_ANGULAR_L]);
 
-    command.setTargetTheta(normalizeAngle(robot()->pose.theta + angular * MAX_VEL_ANGULAR));
+    theta += angular;
+    command.setTargetTheta(normalizeAngle(theta));
 
     if (is_kick_enable) {
       if (is_kick_mode_straight) {
@@ -183,6 +184,8 @@ class Teleop : public SkillBase<RobotCommandWrapperPosition>, public rclcpp::Nod
   double kick_power = 0.5;
 
   double dribble_power = 0.3;
+
+  double theta = 0.0;
 };
 }  // namespace crane::skills
 #endif  // CRANE_ROBOT_SKILLS__TELEOP_HPP_

crane_robot_skills/src/attacker.cpp

@@ -38,7 +38,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
       game_command == crane_msgs::msg::PlaySituation::OUR_KICKOFF_START) {
       auto best_receiver = selectPassReceiver();
       forced_pass_receiver_id = best_receiver->id;
-      forced_pass_phase = 0;
+      forced_pass_phase = 1;
       return true;
     } else {
       return false;
@@ -358,8 +358,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     AttackerState::MOVE_BALL_TO_OPPONENT_HALF,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       kick_skill.setParameter("target", world_model()->getTheirGoalCenter());
-      kick_skill.setParameter("kick_power", 1.0);
-      kick_skill.setParameter("dot_threshold", 0.9);
+      kick_skill.setParameter("kick_power", 0.8);
+      kick_skill.setParameter("dot_threshold", 0.95);
       kick_skill.setParameter("kick_with_chip", true);
       return kick_skill.run(visualizer);
     });
@@ -385,6 +385,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   addStateFunction(
     AttackerState::RECEIVE_BALL,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+      receive_skill.setParameter("dribble_power", 0.0);
+      receive_skill.setParameter("enable_software_bumper", false);
       return receive_skill.run(visualizer);
     });
 

crane_robot_skills/src/goalie.cpp

@@ -120,7 +120,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
     }
   } else {
     if (
-      world_model()->ball.isStopped() &&
+      world_model()->ball.isStopped(0.2) &&
       world_model()->point_checker.isFriendPenaltyArea(ball.pos) && enable_emit) {
       // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す
       phase = "ボール排出";
@@ -158,7 +158,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
           phase += "(範囲外なので正面に構える)";
           command.setTargetPosition(goal_center).lookAt(Point(0, 0));
         } else {
-          Point threat_point;
+          Point threat_point = world_model()->ball.pos;
           bool penalty_area_pass_to_side = [&]() {
             Point penalty_base_1, penalty_base_2;
             penalty_base_1 = penalty_base_2 = world_model()->getOurGoalCenter();
@@ -215,17 +215,17 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
               // TODO(HansRobo): 将来的には、パス経路を止めるのではなく適宜前進守備を行う
               // ペナルティーエリアの少し内側で待ち受ける
               Point wait_point = threat_point + (threat_point - ball.pos).normalized() * 0.2;
-              command.setTargetPosition(threat_point).lookAtBallFrom(threat_point);
-              if (command.getRobot()->getDistance(wait_point) > 0.1) {
+              command.setTargetPosition(wait_point).lookAtBallFrom(wait_point);
+              if (command.getRobot()->getDistance(wait_point) > 0.03) {
                 // なりふり構わず爆加速
                 command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0);
               }
               phase += "(パスカットモードFRONT)";
             } else if (penalty_area_pass_to_side) {
               // ペナルティーエリアの少し内側で待ち受ける
               Point wait_point = threat_point + (threat_point - ball.pos).normalized() * 0.2;
-              command.setTargetPosition(threat_point).lookAtBallFrom(threat_point);
-              if (command.getRobot()->getDistance(wait_point) > 0.1) {
+              command.setTargetPosition(wait_point).lookAtBallFrom(wait_point);
+              if (command.getRobot()->getDistance(wait_point) > 0.03) {
                 // なりふり構わず爆加速
                 command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0);
               }
@@ -258,7 +258,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
             Point wait_point = weak_point + (threat_point - weak_point).normalized() * BLOCK_DIST;
 
             command.setTargetPosition(wait_point).lookAtBallFrom(wait_point);
-            if (command.getRobot()->getDistance(wait_point) > 0.05) {
+            if (command.getRobot()->getDistance(wait_point) > 0.03) {
               // なりふり構わず爆加速
               command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0);
             }

crane_robot_skills/src/single_ball_placement.cpp

@@ -204,13 +204,14 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
         move_with_ball->setParameter("target_x", getParameter<double>("placement_x"));
         move_with_ball->setParameter("target_y", getParameter<double>("placement_y"));
         move_with_ball->setParameter("dribble_power", 0.2);
+        move_with_ball->setParameter("dribble_target_horizon", 0.5);
       }
 
       skill_status = move_with_ball->run(visualizer);
       command.disablePlacementAvoidance();
       command.disableGoalAreaAvoidance();
       command.disableRuleAreaAvoidance();
-      command.setMaxVelocity(0.5);
+      command.setMaxVelocity(1.0);
       command.setMaxAcceleration(1.0);
       return Status::RUNNING;
     });
@@ -230,7 +231,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       if (not sleep) {
         sleep = std::make_shared<Sleep>(command_base);
-        sleep->setParameter("duration", 2.0);
+        sleep->setParameter("duration", 1.0);
       }
       skill_status = sleep->run(visualizer);
       return Status::RUNNING;
@@ -266,16 +267,17 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
       if (not set_target_position) {
         set_target_position = std::make_shared<CmdSetTargetPosition>(command_base);
       }
-      // メモ：().normalized() * 0.6したらなぜかゼロベクトルが出来上がってしまう
+      // メモ：().normalized() * 0.8したらなぜかゼロベクトルが出来上がってしまう
       Vector2 diff = (robot()->pose.pos - world_model()->ball.pos);
       diff.normalize();
-      diff = diff * 0.6;
+      diff = diff * 0.8;
       auto leave_pos = world_model()->ball.pos + diff;
       set_target_position->setParameter("x", leave_pos.x());
       set_target_position->setParameter("y", leave_pos.y());
       set_target_position->setParameter("reach_threshold", 0.05);
 
       command.disablePlacementAvoidance();
+      command.disableBallAvoidance();
       command.disableGoalAreaAvoidance();
       command.disableRuleAreaAvoidance();
       return set_target_position->run(visualizer);

crane_robot_skills/src/steal_ball.cpp

@@ -73,6 +73,8 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base)
           robot()->getDistance(world_model()->ball.pos) <
             world_model()->getTheirFrontier()->robot->getDistance(world_model()->ball.pos)) {
           command.kickWithChip(0.5);
+        } else {
+          command.kickWithChip(0.5);
         }
       }
       return Status::RUNNING;

crane_robot_skills/src/sub_attacker.cpp

@@ -210,6 +210,21 @@ double SubAttacker::getPointScore(
   if (dist_to_shoot_line < 0.5) {
     score = 0.0;
   }
+
+  // 自分とボールの延長線上にゴールがある場合は避ける
+  Segment robot_to_ball_and_more{
+    p, world_model->ball.pos + (world_model->ball.pos - p).normalized() * 10.0};
+  Segment goal_line{
+    world_model->getTheirGoalPosts().first, world_model->getTheirGoalPosts().second};
+  if (double distance = bg::distance(robot_to_ball_and_more, goal_line); distance < 1.0) {
+    score *= distance;
+  }
+
+  // ボールの後側にには行かない
+  double dot = (world_model->getTheirGoalCenter() - world_model->ball.pos)
+                 .normalized()
+                 .dot((p - world_model->ball.pos).normalized());
+  score *= std::max((dot + 0.5), 0.0);
   return score;
 }
 }  // namespace crane::skills

crane_sender/include/crane_sender/parameter_with_event.hpp

@@ -0,0 +1,40 @@
+// Copyright (c) 2024 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_SENDER__PARAMETER_WITH_EVENT_HPP_
+#define CRANE_SENDER__PARAMETER_WITH_EVENT_HPP_
+namespace crane
+{
+struct ParameterWithEvent
+{
+  ParameterWithEvent(std::string name, rclcpp::Node & node) : name(name)
+  {
+    parameter_subscriber = std::make_shared<rclcpp::ParameterEventHandler>(&node);
+    parameter_callback_handle =
+      parameter_subscriber->add_parameter_callback(name, [&](const rclcpp::Parameter & p) {
+        if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE && callback) {
+          value = p.as_double();
+          callback(value);
+        } else {
+          std::cout << "debug_id is not integer" << std::endl;
+        }
+      });
+  }
+
+  std::shared_ptr<rclcpp::ParameterEventHandler> parameter_subscriber;
+
+  std::shared_ptr<rclcpp::ParameterCallbackHandle> parameter_callback_handle;
+
+  std::function<void(double)> callback;
+
+  double getValue() { return value; }
+
+  double value;
+
+  std::string name;
+};
+}  // namespace crane
+#endif  // CRANE_SENDER__PARAMETER_WITH_EVENT_HPP_

crane_sender/include/crane_sender/sim_sender.hpp

@@ -17,39 +17,11 @@
 #include <std_msgs/msg/string.hpp>
 #include <string>
 
+#include "parameter_with_event.hpp"
 #include "sender_base.hpp"
 
 namespace crane
 {
-struct ParameterWithEvent
-{
-  ParameterWithEvent(std::string name, rclcpp::Node & node) : name(name)
-  {
-    parameter_subscriber = std::make_shared<rclcpp::ParameterEventHandler>(&node);
-    parameter_callback_handle =
-      parameter_subscriber->add_parameter_callback(name, [&](const rclcpp::Parameter & p) {
-        if (p.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE && callback) {
-          value = p.as_double();
-          callback(value);
-        } else {
-          std::cout << "debug_id is not integer" << std::endl;
-        }
-      });
-  }
-
-  std::shared_ptr<rclcpp::ParameterEventHandler> parameter_subscriber;
-
-  std::shared_ptr<rclcpp::ParameterCallbackHandle> parameter_callback_handle;
-
-  std::function<void(double)> callback;
-
-  double getValue() { return value; }
-
-  double value;
-
-  std::string name;
-};
-
 class SimSenderComponent : public SenderBase
 {
 public:
@@ -86,15 +58,6 @@ class SimSenderComponent : public SenderBase
       }
     };
 
-    p_gain.callback = [&](double value) {
-      for (auto & controller : vx_controllers) {
-        controller.setGain(value, i_gain.getValue(), d_gain.getValue());
-      }
-      for (auto & controller : vy_controllers) {
-        controller.setGain(value, i_gain.getValue(), d_gain.getValue());
-      }
-    };
-
     i_gain.callback = [&](double value) {
       for (auto & controller : vx_controllers) {
         controller.setGain(p_gain.getValue(), value, d_gain.getValue());

session/crane_planner_plugins/src/defender_planner.cpp

@@ -100,8 +100,8 @@ DefenderPlanner::calculateRobotCommand(const std::vector<RobotIdentifier> & robo
 std::vector<Point> DefenderPlanner::getDefenseArcPoints(
   const int robot_num, const Segment & ball_line) const
 {
-  const double DEFENSE_INTERVAL = 0.4;
-  const double RADIUS_OFFSET = 0.4;
+  const double DEFENSE_INTERVAL = 0.5;
+  const double RADIUS_OFFSET = 0.5;
   std::vector<Point> defense_points;
   // ペナルティエリアの一番遠い点を通る円の半径
   const double RADIUS =

session/crane_planner_plugins/src/skill_planners.cpp

@@ -257,6 +257,8 @@ auto BallNearByPositionerSkillPlanner::getSelectedRobots(
     skills.back()->setParameter("current_robot_index", index++);
     skills.back()->setParameter("line_policy", std::string("arc"));
     skills.back()->setParameter("positioning_policy", std::string("goal"));
+    skills.back()->setParameter("robot_interval", 0.35);
+    skills.back()->setParameter("margin_distance", 0.35);
   }
 
   return selected;

session/crane_session_controller/config/play_situation/OUR_FREE_KICK.yaml

@@ -3,5 +3,7 @@ description: OUR_FREE_KICK
 sessions:
   - name: goalie_skill
     capacity: 1
-  - name: our_direct_free
+  - name: attacker_skill
+    capacity: 1
+  - name: waiter
     capacity: 20