県大練習会変更監視

consai_ros2/consai_vision_tracker/src/ball_tracker.cpp

@@ -231,6 +231,10 @@ void BallTracker::reset_prior()
 
 bool BallTracker::is_outlier(const TrackedBall & observation) const
 {
+  // ハーフコート練習用
+  //  if(observation.pos.x < 0) {
+  //    return true;
+  //  }
   // 観測が外れ値かどうか判定する
   // Reference: https://myenigma.hatenablog.com/entry/20140825/1408975706
   const double THRESHOLD = 5.99;  // 自由度2、棄却率5%のしきい値

crane_bringup/launch/crane.launch.py

@@ -89,10 +89,10 @@ def generate_launch_description():
                         output="screen",
                         parameters=[
                             {"planner": "rvo2"},
-                            {"p_gain": 2.0},
+                            {"p_gain": 5.0},
                             {"i_gain": 0.00},
                             {"i_saturation": 0.00},
-                            {"d_gain": 3.0},
+                            {"d_gain": 1.0},
                             {"max_vel": LaunchConfiguration("max_vel")},
                             {"max_acc": 3.0},
                             {"deceleration_factor": 1.5},
@@ -118,11 +118,12 @@ def generate_launch_description():
                         output="screen",
                         parameters=[
                             {"planner": "rvo2"},
-                            {"p_gain": 3.0},
+                            {"p_gain": 5.5},
                             {"i_gain": 0.0},
                             {"i_saturation": 0.0},
-                            {"d_gain": 1.5},
+                            {"d_gain": 4.0},
                             {"max_vel": LaunchConfiguration("max_vel")},
+                            {"max_acc": 4.0},
                             {"deceleration_factor": 1.5},
                         ],
                         on_exit=default_exit_behavior,
@@ -157,7 +158,7 @@ def generate_launch_description():
                     {"no_movement": False},
                     {"latency_ms": 0.0},
                     {"sim_mode": LaunchConfiguration("sim")},
-                    {"kick_power_limit_straight": 1.0},
+                    {"kick_power_limit_straight": 0.30},
                     {"kick_power_limit_chip": 1.0},
                 ],
                 on_exit=default_exit_behavior,
@@ -213,8 +214,10 @@ def generate_launch_description():
                 package="crane_play_switcher",
                 executable="play_switcher_node",
                 output="screen",
-                parameters=[{"team_name": LaunchConfiguration("team")}],
-                on_exit=default_exit_behavior,
+                parameters=[
+                    {"team_name": LaunchConfiguration("team")},
+                ],
+                on_exit=Shutdown(),
             ),
             # Group with speak condition
             GroupAction(
@@ -223,7 +226,6 @@ def generate_launch_description():
                     Node(
                         package="crane_speaker",
                         executable="crane_speaker_node",
-                        on_exit=default_exit_behavior,
                     )
                 ],
             ),
@@ -232,8 +234,8 @@ def generate_launch_description():
                 executable="speak_ros_node",
                 parameters=[
                     {"plugin_name": "voicevox_plugin::VoiceVoxPlugin"},
-                    {"voicevox_plugin/speaker": 14},
-                    {"voicevox_plugin/speedScale": 1.0},
+                    {"voicevox_plugin/speaker": 13},
+                    {"voicevox_plugin/speedScale": 0.8},
                     {"voicevox_plugin/volumeScale": 1.0},
                 ],
                 on_exit=default_exit_behavior,

crane_bringup/launch/data.launch.py

@@ -0,0 +1,109 @@
+# Copyright (c) 2024 ibis-ssl
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, Shutdown
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch.conditions import IfCondition, UnlessCondition
+
+default_exit_behavior = Shutdown()
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            # Launch Arguments
+            DeclareLaunchArgument(
+                "vision_addr",
+                default_value="224.5.23.2",
+                description="Set multicast address to connect SSL-Vision.",
+            ),
+            DeclareLaunchArgument(
+                "vision_port",
+                default_value="10006",
+                description="Set multicast port to connect SSL-Vision.",
+            ),
+            DeclareLaunchArgument(
+                "referee_addr",
+                default_value="224.5.23.1",
+                description="Set multicast address to connect Game Controller.",
+            ),
+            DeclareLaunchArgument("referee_port", default_value="10003"),
+            # DeclareLaunchArgument("referee_port", default_value="11111"),
+            DeclareLaunchArgument("team", default_value="Test Team", description="team name"),
+            DeclareLaunchArgument(
+                "gui", default_value="true", description="Set true if you want to use GUI."
+            ),
+            Node(
+                package="robocup_ssl_comm",
+                executable="vision_node",
+                parameters=[
+                    {"multicast_address": LaunchConfiguration("vision_addr")},
+                    {"multicast_port": LaunchConfiguration("vision_port")},
+                ],
+                on_exit=default_exit_behavior,
+            ),
+            Node(
+                package="robocup_ssl_comm",
+                executable="game_controller_node",
+                parameters=[
+                    {"multicast_address": LaunchConfiguration("referee_addr")},
+                    {"multicast_port": LaunchConfiguration("referee_port")},
+                ],
+                on_exit=default_exit_behavior,
+            ),
+            Node(
+                package="crane_robot_receiver",
+                executable="robot_receiver_node",
+                output="screen",
+                # on_exit=default_exit_behavior,
+            ),
+            Node(
+                package="robocup_ssl_comm",
+                executable="robot_status_node",
+                parameters=[{"blue_port": 10311}, {"yellow_port": 10312}],
+                on_exit=default_exit_behavior,
+            ),
+            Node(
+                package="consai_vision_tracker",
+                executable="vision_tracker_node",
+                on_exit=default_exit_behavior,
+            ),
+            Node(
+                package="crane_world_model_publisher",
+                executable="crane_world_model_publisher_node",
+                parameters=[
+                    {"initial_team_color": "YELLOW"},
+                    {"team_name": LaunchConfiguration("team")},
+                ],
+                on_exit=default_exit_behavior,
+            ),
+            Node(
+                package="crane_play_switcher",
+                executable="play_switcher_node",
+                output="screen",
+                parameters=[
+                    {"team_name": LaunchConfiguration("team")},
+                ],
+                on_exit=Shutdown(),
+            ),
+            Node(
+                condition=IfCondition(LaunchConfiguration("gui")),
+                package="consai_visualizer",
+                executable="consai_visualizer",
+                on_exit=default_exit_behavior,
+            ),
+        ]
+    )

crane_local_planner/src/rvo2_planner.cpp

@@ -288,8 +288,14 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
       }();
       if (not command.local_planner_config.disable_goal_area_avoidance) {
         bool is_in_penalty_area = isInBox(penalty_area, target_pos, 0.2);
-        constexpr double SURROUNDING_OFFSET = 0.3;
-        constexpr double PENALTY_AREA_OFFSET = 0.1;
+        double SURROUNDING_OFFSET = 0.3;
+        double PENALTY_AREA_OFFSET = 0.1;
+        if (
+          world_model->play_situation.getSituationCommandID() ==
+          crane_msgs::msg::PlaySituation::STOP) {
+          PENALTY_AREA_OFFSET = 0.5;
+          SURROUNDING_OFFSET = 0.6;
+        }
         if (isInBox(
               penalty_area, Point(command.current_pose.x, command.current_pose.y),
               PENALTY_AREA_OFFSET)) {

crane_robot_skills/include/crane_robot_skills/sleep.hpp

@@ -24,6 +24,8 @@ class Sleep : public SkillBase<RobotCommandWrapperPosition>
 
   double getRestTime() const;
 
+  void reset() { is_started = false; }
+
   bool & is_started;
 
   std::chrono::time_point<std::chrono::steady_clock> start_time;

crane_robot_skills/src/attacker.cpp

@@ -20,7 +20,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   steal_ball_skill(base)
 {
   receive_skill.setParameter("policy", std::string("closest"));
-  setParameter("receiver_id", 0);
+  setParameter("receiver_id", -1);
   addStateFunction(
     AttackerState::ENTRY_POINT,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
@@ -37,6 +37,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;
+      setParameter("receiver_id", best_receiver->id);
       auto receiver = world_model()->getOurRobot(forced_pass_receiver_id);
       kick_skill.setParameter("target", receiver->pose.pos);
       forced_pass_phase = 1;
@@ -62,8 +63,13 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
         }
         case 1: {
           // パス
+          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;
+          }
           Segment kick_line{world_model()->ball.pos, kick_target};
           // 近くに敵ロボットがいればチップキック
           if (const auto enemy_robots = world_model()->theirs.getAvailableRobots();
@@ -196,6 +202,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     AttackerState::GOAL_KICK,
     [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       goal_kick_skill.setParameter("dot_threshold", 0.95);
+      goal_kick_skill.setParameter("キック角度の最低要求精度[deg]", 5.0);
       return goal_kick_skill.run(visualizer);
     });
 
@@ -225,6 +232,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     //    int receiver_id = getParameter<int>("receiver_id");
     double best_score = 0.0;
     Point best_target;
+    int best_id = -1;
     for (auto & our_robot : our_robots) {
       Segment ball_to_target{world_model()->ball.pos, our_robot->pose.pos};
       auto target = our_robot->pose.pos;
@@ -257,12 +265,14 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
       if (score > best_score) {
         best_score = score;
         best_target = target;
+        best_id = our_robot->id;
       }
     }
 
     auto ret = best_score > 0.5;
     if (ret) {
       kick_target = best_target;
+      setParameter("receiver_id", best_id);
     }
     return ret;
   });
@@ -275,6 +285,11 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   addStateFunction(
     AttackerState::STANDARD_PASS,
     [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+      int receiver_id = getParameter<int>("receiver_id");
+      if (receiver_id != -1) {
+        kick_target = world_model()->getOurRobot(receiver_id)->pose.pos;
+      }
+
       auto our_robots = world_model()->ours.getAvailableRobots(robot()->id);
       const auto enemy_robots = world_model()->theirs.getAvailableRobots();
 
@@ -339,6 +354,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
       kick_skill.setParameter("kick_power", 0.8);
       kick_skill.setParameter("dot_threshold", 0.95);
       kick_skill.setParameter("kick_with_chip", true);
+      command.disableBallAvoidance();
       return kick_skill.run(visualizer);
     });
 

crane_robot_skills/src/ball_nearby_positioner.cpp

@@ -20,7 +20,7 @@ BallNearByPositioner::BallNearByPositioner(RobotCommandWrapperBase::SharedPtr &
   setParameter("positioning_policy", std::string("goal"));
   // 整列距離
   setParameter("robot_interval", 0.3);
-  setParameter("margin_distance", 0.3);
+  setParameter("margin_distance", 0.6);
 }
 
 Status BallNearByPositioner::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer)

crane_robot_skills/src/goalie.cpp

@@ -116,7 +116,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
     command.setTargetPosition(target).lookAtBallFrom(target);
     if (command.getRobot()->getDistance(target) > 0.05) {
       // なりふり構わず爆加速
-      command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0);
+      // command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0);
     }
   } else {
     if (
@@ -130,7 +130,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
       const double BLOCK_DIST = getParameter<double>("block_distance");
       phase += "ボールを待ち受ける";
       // デフォルト位置設定
-      command.setTargetPosition(world_model()->getOurGoalCenter()).lookAt(Point(0, 0));
+      command.setTargetPosition(world_model()->getOurGoalCenter() * 0.9).lookAt(Point(0, 0));
       if (std::signbit(world_model()->ball.pos.x()) == std::signbit(world_model()->goal.x())) {
         phase += " (自コート警戒モード)";
         Segment ball_prediction_4s(ball.pos, ball.pos + ball.vel * 4.0);
@@ -220,7 +220,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
               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);
+                //                command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0);
               }
               phase += "(パスカットモードFRONT)";
             } else if (penalty_area_pass_to_side) {
@@ -229,7 +229,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
               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);
+                //                command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0);
               }
               phase += "(パスカットモードSIDE)";
             }
@@ -262,7 +262,7 @@ void Goalie::inplay(bool enable_emit, const ConsaiVisualizerWrapper::SharedPtr &
             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);
+              //              command.setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0);
             }
           }
         }