豊田高専練習試合用ブランチ

.vscode/settings.json

@@ -20,5 +20,137 @@
     "chrono": "cpp",
     "*.ipp": "cpp",
     "functional": "cpp"
-  }
+  },
+  "cSpell.words": [
+    "grSim",
+    "robocup",
+    "robocupssl",
+    "teleop",
+    "inplay",
+    "matplotlibcpp",
+    "DPPS",
+    "consai",
+    "DBOOST",
+    "ZJUNlict",
+    "autoref",
+    "yellowteam",
+    "isteamyellow ",
+    "wheelsspeed",
+    "kickspeedx",
+    "kickspeedz",
+    "veltangent",
+    "velnormal",
+    "velangular",
+    "STRGREATER",
+    "bringup",
+    "gitflow",
+    "sint32",
+    "oneof",
+    "customwidget",
+    "customwidgets",
+    "Midlight",
+    "brushstyle",
+    "colorrole",
+    "statustip",
+    "Robotstatus",
+    "AIS",
+    "AMC",
+    "CMμs",
+    "ER-Force",
+    "erforce",
+    "ibis",
+    "ITAndroids",
+    "Immortals",
+    "KIKS",
+    "KgpKubs",
+    "MCT",
+    "Susano",
+    "Roboteam",
+    "MRL",
+    "NAELIC",
+    "NEUIslanders",
+    "OMID",
+    "OP-AmP",
+    "Parsian",
+    "RFC Cambridge",
+    "Ri-One",
+    "Ri-one",
+    "RoboDragons",
+    "RoboFEI",
+    "RoboIME",
+    "RoboJackets",
+    "Twente",
+    "RobôCin",
+    "SRC",
+    "SSH",
+    "STOx’s",
+    "Sysmic",
+    "TIGERs",
+    "Mannheim",
+    "Tritons",
+    "RCSC",
+    "Thunderbots",
+    "ULtron",
+    "UMass",
+    "Minutebots",
+    "URoboRus",
+    "Unknown",
+    "Warthog",
+    "ZJUNlict",
+    "luhbots",
+    "nAMeC",
+    "TRAPS",
+    "Shinobi",
+    "reRo",
+    "roboticserlangen",
+    "tigersmannheim",
+    "simulatorcli",
+    "sizepolicy",
+    "hsizetype",
+    "vsizetype",
+    "horstretch",
+    "verstretch",
+    "notr",
+    "rects",
+    "qcolor",
+    "frootspi",
+    "buttongroup",
+    "buttongroups",
+    "AUTOUIC",
+    "AUTORCC",
+    "FORCESTART",
+    "PETG",
+    "Waitables",
+    "zdixwfbhdipwhaaxtiqcxwywwkekaxog",
+    "callgrind",
+    "rowwise",
+    "colwise",
+    "MPPI",
+    "Kwargs",
+    "voicevox",
+    "FREEKICK",
+    "rcst",
+    "GLFW",
+    "GLEW",
+    "libglfw",
+    "libglew",
+    "opengl",
+    "imgui",
+    "Framebuffer",
+    "vsync",
+    "CHECKVERSION",
+    "BLAUE",
+    "Hackentrick",
+    "3rdparty",
+    "PROTOS",
+    "libprotobuf",
+    "RELWITHDEBINFO",
+    "NODEBUG",
+    "remmina",
+    "gltf",
+    "svgs",
+    "teleporting",
+    "DDPS",
+    "cooldown"
+  ]
 }

crane_bringup/launch/crane.launch.py

@@ -47,27 +47,22 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "sim", default_value="true", description="シミュレータフラグ"
             ),
-            DeclareLaunchArgument(
-                "original_grsim",
-                default_value="false",
-                description="GrSimを使用する場合はtrueにする",
-            ),
             DeclareLaunchArgument(
                 "simple_ai", default_value="false", description="SimpleAIモードのフラグ"
             ),
             DeclareLaunchArgument(
-                "max_vel", default_value="3.0", description="ロボットの最大速度"
+                "max_vel", default_value="7.0", description="ロボットの最大速度"
             ),
             DeclareLaunchArgument(
-                "speak", default_value="true", description="音声ノードの起動フラグ"
+                "speak", default_value="false", description="音声ノードの起動フラグ"
             ),
             DeclareLaunchArgument(
                 "is_emplace_positive_side",
                 default_value="true",
                 description="ロボットの退場する方向",
             ),
             DeclareLaunchArgument(
-                "record", default_value="false", description="rosbag記録フラグ"
+                "record", default_value="true", description="rosbag記録フラグ"
             ),
             Node(
                 package="crane_session_controller",
@@ -96,24 +91,17 @@ def generate_launch_description():
                         output="screen",
                         parameters=[
                             {"planner": "rvo2"},
-                            {"p_gain": 5.0},
+                            {"p_gain": 3.0},
                             {"i_gain": 0.00},
                             {"i_saturation": 0.00},
                             {"d_gain": 1.0},
                             {"max_vel": LaunchConfiguration("max_vel")},
-                            {"max_acc": 3.0},
-                            {"deceleration_factor": 1.5},
+                            {"max_acc": 2.0},
+                            {"deceleration_factor": 1.0},
                             {"rvo_radius": 0.15},
                         ],
                         on_exit=default_exit_behavior,
                     ),
-                    Node(
-                        package="crane_clock_publisher",
-                        executable="crane_clock_publisher_node",
-                        output="screen",
-                        parameters=[{"time_scale": 1.00}],
-                        on_exit=default_exit_behavior,
-                    ),
                     Node(
                         package="crane_sender",
                         # executable="simulation_protocol_sender_node",
@@ -149,7 +137,7 @@ def generate_launch_description():
                             {"i_saturation": 0.0},
                             {"d_gain": 4.0},
                             {"max_vel": LaunchConfiguration("max_vel")},
-                            {"max_acc": 4.0},
+                            {"max_acc": 2.5},
                             {"deceleration_factor": 1.5},
                         ],
                         on_exit=default_exit_behavior,
@@ -159,9 +147,9 @@ def generate_launch_description():
                         executable="ibis_sender_node",
                         parameters=[
                             {"no_movement": False},
-                            {"latency_ms": 0.0},
+                            {"latency_ms": 100.0},
                             {"sim_mode": LaunchConfiguration("sim")},
-                            {"kick_power_limit_straight": 0.30},
+                            {"kick_power_limit_straight": 0.50},
                             {"kick_power_limit_chip": 1.0},
                             {
                                 "use_simple_velocity": False
@@ -189,6 +177,7 @@ def generate_launch_description():
                 package="crane_robot_receiver",
                 executable="robot_receiver_node",
                 output="screen",
+                respawn=True,
                 # on_exit=default_exit_behavior,
             ),
             Node(

crane_local_planner/include/crane_local_planner/rvo2_planner.hpp

@@ -26,7 +26,7 @@ class RVO2Planner : public LocalPlannerBase
 public:
   explicit RVO2Planner(rclcpp::Node & node);
 
-  void reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg);
+  void reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg);
 
   crane_msgs::msg::RobotCommands extractRobotCommandsFromRVOSim(
     const crane_msgs::msg::RobotCommands & msg);
@@ -53,9 +53,7 @@ class RVO2Planner : public LocalPlannerBase
   float RVO_RADIUS = 0.09f;
   float RVO_MAX_SPEED = 10.0f;
 
-  float RVO_TRAPEZOIDAL_MAX_ACC = 8.0;
   float RVO_TRAPEZOIDAL_FRAME_RATE = 60;
-  float RVO_TRAPEZOIDAL_MAX_SPEED = 4.0;
 
   double MAX_VEL = 4.0;
   double ACCELERATION = 4.0;

crane_local_planner/src/rvo2_planner.cpp

@@ -31,12 +31,8 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node)
   RVO_RADIUS = node.get_parameter("rvo_radius").as_double();
   node.declare_parameter("rvo_max_speed", RVO_MAX_SPEED);
   RVO_MAX_SPEED = node.get_parameter("rvo_max_speed").as_double();
-  node.declare_parameter("rvo_trapezoidal_max_acc", RVO_TRAPEZOIDAL_MAX_ACC);
-  RVO_TRAPEZOIDAL_MAX_ACC = node.get_parameter("rvo_trapezoidal_max_acc").as_double();
   node.declare_parameter("rvo_trapezoidal_frame_rate", RVO_TRAPEZOIDAL_FRAME_RATE);
   RVO_TRAPEZOIDAL_FRAME_RATE = node.get_parameter("rvo_trapezoidal_frame_rate").as_double();
-  node.declare_parameter("rvo_trapezoidal_max_speed", RVO_TRAPEZOIDAL_MAX_SPEED);
-  RVO_TRAPEZOIDAL_MAX_SPEED = node.get_parameter("rvo_trapezoidal_max_speed").as_double();
 
   node.declare_parameter("max_vel", MAX_VEL);
   MAX_VEL = node.get_parameter("max_vel").as_double();
@@ -59,7 +55,7 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node)
     [this](const crane_msgs::msg::RobotFeedbackArray & msg) { latest_feedback = msg; });
 }
 
-void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg)
+void RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg)
 {
   if (
     world_model->getMsg().play_situation.command_raw.value ==
@@ -74,7 +70,7 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
     }
   }
   // 味方ロボット：RVO内の位置・速度（＝進みたい方向）の更新
-  for (const auto & command : msg.robot_commands) {
+  for (auto & command : msg.robot_commands) {
     rvo_sim->setAgentPosition(
       command.robot_id, RVO::Vector2(command.current_pose.x, command.current_pose.y));
     rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(0.f, 0.f));
@@ -155,6 +151,9 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
           max_vel = std::min(max_vel, 1.0);
         }
 
+        command.local_planner_config.final_planned_max_acceleration = acceleration;
+        command.local_planner_config.final_planned_max_velocity = max_vel;
+
         target_vel = target_vel.normalized() * max_vel;
 
         if (target_vel.norm() < command.local_planner_config.terminal_velocity) {

crane_msgs/CMakeLists.txt

@@ -37,6 +37,7 @@ set(msg_files
   "msg/analysis/Kick.msg"
   "msg/analysis/Slack.msg"
   "msg/analysis/SlackWithPoint.msg"
+  "msg/analysis/FloatWithID.msg"
 
   "msg/planner/PassPlan.msg"
   "msg/planner/PassInfo.msg"

crane_msgs/msg/analysis/FloatWithID.msg

@@ -0,0 +1,2 @@
+uint8 id
+float32 value

crane_msgs/msg/analysis/GameAnalysis.msg

@@ -5,3 +5,6 @@ Slack[] our_slack
 Slack[] their_slack
 
 float32 ball_horizon
+
+# sorted by score
+FloatWithID[] pass_scores

crane_msgs/msg/control/LocalPlannerConfig.msg

@@ -10,3 +10,6 @@ float32 terminal_velocity 0
 uint8 priority 0
 
 float32 theta_tolerance 0.0
+
+float32 final_planned_max_acceleration -1.0
+float32 final_planned_max_velocity -1.0

crane_robot_skills/include/crane_robot_skills/attacker.hpp

@@ -34,6 +34,7 @@ enum class AttackerState {
   THROUGH,
   KICK_TO_GOAL,
   STOP,
+  GOAL_FRONT_DANCE,
 };
 class Attacker : public SkillBaseWithState<AttackerState, RobotCommandWrapperPosition>
 {
@@ -59,6 +60,8 @@ class Attacker : public SkillBaseWithState<AttackerState, RobotCommandWrapperPos
   Receive receive_skill;
 
   StealBall steal_ball_skill;
+
+  std::optional<Point> goal_front_dance_target = std::nullopt;
 };
 }  // namespace crane::skills
 #endif  // CRANE_ROBOT_SKILLS__ATTACKER_HPP_

crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp

@@ -11,7 +11,6 @@
 #include <crane_robot_skills/skill_base.hpp>
 #include <memory>
 
-#include "get_ball_contact.hpp"
 #include "go_over_ball.hpp"
 #include "move_with_ball.hpp"
 #include "robot_command_as_skill.hpp"
@@ -38,8 +37,6 @@ class SingleBallPlacement
 private:
   std::shared_ptr<GoOverBall> go_over_ball;
 
-  std::shared_ptr<GetBallContact> get_ball_contact;
-
   std::shared_ptr<MoveWithBall> move_with_ball;
 
   std::shared_ptr<Sleep> sleep = nullptr;

crane_robot_skills/include/crane_robot_skills/skills.hpp

@@ -11,7 +11,6 @@
 #include <crane_robot_skills/ball_nearby_positioner.hpp>
 #include <crane_robot_skills/emplace_robot.hpp>
 #include <crane_robot_skills/freekick_saver.hpp>
-#include <crane_robot_skills/get_ball_contact.hpp>
 #include <crane_robot_skills/go_over_ball.hpp>
 #include <crane_robot_skills/goal_kick.hpp>
 #include <crane_robot_skills/goalie.hpp>