0320

crane_bringup/launch/crane.launch.xml

@@ -55,7 +55,7 @@ limitations under the License.
   <arg name="max_vel" default="8.0" description="ロボットの最大速度 [m/s]"/>
 
   <!-- Slack時間計算パラメータ -->
-  <arg name="slack.robot_max_acceleration" default="2.5" description="slack時間計算に用いるロボットの最大加速度 [m/s^2]"/>
+  <arg name="slack.robot_max_acceleration" default="1.5" description="slack時間計算に用いるロボットの最大加速度 [m/s^2]"/>
   <arg name="slack.robot_max_velocity" default="5.0" description="slack時間計算に用いるロボットの最大速度 [m/s]"/>
   <arg name="slack.time_horizon" default="3.0" description="ボール軌道予測時間範囲 [s]"/>
   <arg name="slack.time_step" default="0.1" description="ボール軌道サンプリング時間間隔 [s]"/>
@@ -69,15 +69,15 @@ limitations under the License.
   <!-- ロボット送信用の加速度パラメータ -->
   <arg name="robot_acceleration.acceleration" default="7.0" description="加速時の加速度 [m/s^2]"/>
   <arg name="robot_acceleration.deceleration_high" default="5.0" description="減速時・高速域の減速度 [m/s^2]"/>
-  <arg name="robot_acceleration.deceleration_low" default="2.0" description="減速時・低速域の減速度 [m/s^2]"/>
+  <arg name="robot_acceleration.deceleration_low" default="4.0" description="減速時・低速域の減速度 [m/s^2]"/>
   <arg name="robot_acceleration.velocity_threshold" default="1.0" description="高速域・低速域のしきい値 [m/s]"/>
 
   <!-- 経路計画用の減速度パラメータ（実行側の90%以下に設定して計画と実行の不一致によるオーバーシュートを防止） -->
-  <arg name="planning_deceleration.high_speed" default="4.5" description="高速域での減速計算用の減速度 [m/s^2]"/>
-  <arg name="planning_deceleration.low_speed" default="1.8" description="低速域での減速計算用の減速度 [m/s^2]"/>
+  <arg name="planning_deceleration.high_speed" default="2.5" description="高速域での減速計算用の減速度 [m/s^2]"/>
+  <arg name="planning_deceleration.low_speed" default="1.5" description="低速域での減速計算用の減速度 [m/s^2]"/>
   <arg name="planning_deceleration.velocity_threshold" default="1.0" description="高速域・低速域のしきい値 [m/s]"/>
   <!-- 経路計画用の加速度パラメータ（加速フェーズ専用、減速度より高い値を設定可能） -->
-  <arg name="planning_acceleration" default="5.0" description="加速フェーズの計画用加速度 [m/s^2]"/>
+  <arg name="planning_acceleration" default="3.0" description="加速フェーズの計画用加速度 [m/s^2]"/>
 
   <!-- ============================================================ -->
   <!-- フィールド/試合設定 -->
@@ -140,6 +140,10 @@ limitations under the License.
       <param name="kick_power_limit_chip" value="1.0"/>
       <param name="chip_angle_deg" value="30.0"/>
       <param name="theta_p_gain" value="3.0"/>
+      <param name="robot_acceleration.acceleration" value="$(var robot_acceleration.acceleration)"/>
+      <param name="robot_acceleration.deceleration_high" value="$(var robot_acceleration.deceleration_high)"/>
+      <param name="robot_acceleration.deceleration_low" value="$(var robot_acceleration.deceleration_low)"/>
+      <param name="robot_acceleration.velocity_threshold" value="$(var robot_acceleration.velocity_threshold)"/>
     </node>
   </group>
 

crane_local_planner/package.xml

@@ -11,6 +11,7 @@
 
   <depend>closest_point_vendor</depend>
   <depend>crane_comm</depend>
+  <depend>crane_physics</depend>
   <depend>crane_msg_wrappers</depend>
   <depend>crane_msgs</depend>
   <depend>crane_utils</depend>

crane_local_planner/src/rvo2_planner.cpp

@@ -180,36 +180,12 @@ auto RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) ->
     position_diff << pos_mode.target_x - current_position.x(),
       pos_mode.target_y - current_position.y();
 
-    double pre_vel = [&]() {
-      if (
-        auto it = ranges::find_if(
-          pre_commands.robot_commands,
-          [&](const auto & c) { return c.robot_id == command.robot_id; });
-        it != ranges::end(pre_commands.robot_commands)) {
-        if (it->position_target_mode.empty()) {
-          return 0.0;
-        }
-        return std::hypot(
-                 it->position_target_mode.front().target_x - current_position.x(),
-                 it->position_target_mode.front().target_y - current_position.y()) > 0.01
-                 ? vel
-                 : 0.0;
-      } else {
-        return 0.0;
-      }
-    }();
+    // 現在速度ベースで減速度を選択
+    double current_speed = std::hypot(command.current_velocity.x, command.current_velocity.y);
+    double max_brk = (current_speed >= planning_deceleration_velocity_threshold)
+                       ? planning_deceleration_high_speed
+                       : planning_deceleration_low_speed;
 
-    // 減速計算用の減速度を選択（現在速度に応じて高速域・低速域を選択）
-    double deceleration_for_planning;
-    if (pre_vel >= planning_deceleration_velocity_threshold) {
-      deceleration_for_planning = planning_deceleration_high_speed;
-    } else {
-      deceleration_for_planning = planning_deceleration_low_speed;
-    }
-
-    // max_brk: 停止のための減速度（planning_decelerationの値を使用）
-    double max_brk = deceleration_for_planning;
-    // max_acc: 加速フェーズの加速度（planning_accelerationの値を使用、スキルによる制限も適用）
     command.local_planner_config.max_acceleration_factors.emplace_back(
       crane_msgs::msg::NamedFloat()
         .set__name("RVO2Planner::max_acc from parameter")
@@ -228,48 +204,43 @@ auto RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) ->
     }
 
     double max_vel = resolveMaxVelocityFactors(command, MAX_VEL);
+	max_vel = std::max(0.4, max_vel);
 
+    // 1D BangBangTrajectoryで速度プロファイルを計算し、方向は目標への単位ベクトルを使用
+    // 横方向の速度成分をBangBangに渡さないことで旧実装の周回問題を解消
     Velocity target_vel;
-    target_vel << (pos_mode.target_x - current_position.x()),
-      pos_mode.target_y - current_position.y();
+    double distance_to_target = position_diff.norm();
 
-    // 速度超過クランプ: Vision観測誤差でmax_velをわずかに超えた速度を正規化（Sumatra adaptVel相当）
-    constexpr double MAX_VEL_TOLERANCE = 0.2;
-    Eigen::Vector2d v0(command.current_velocity.x, command.current_velocity.y);
-    if (vel > max_vel && vel < max_vel + MAX_VEL_TOLERANCE) {
-      v0 = v0 * (max_vel / vel);
-    }
+    if (distance_to_target > 0.01) {
+      Eigen::Vector2d dir = position_diff.normalized();
 
-    BangBangTrajectory2D trajectory;
-    trajectory.generate(
-      Eigen::Vector2d(current_position.x(), current_position.y()),
-      Eigen::Vector2d(pos_mode.target_x, pos_mode.target_y), v0, max_vel, max_acc, max_brk);
-
-    // BangBangTrajectoryから速度を取得
-    // Vision遅延（~100ms）を考慮してlookahead時間を設定
-    const double lookahead_time = 0.1;
-    Eigen::Vector2d next_vel = trajectory.getVelocity(lookahead_time);
-
-    // 目標との距離を計算
-    double distance_to_target = std::hypot(
-      pos_mode.target_x - current_position.x(), pos_mode.target_y - current_position.y());
-
-    // terminal_velocity: スキルが明示的に設定した場合のみ疑似I項として適用する
-    // 0（デフォルト・未指定）のときはBangBang軌道の出力に従い、目標で停止する
-    const double terminal_vel = command.local_planner_config.terminal_velocity;
-    const double min_distance =
-      std::max(static_cast<double>(pos_mode.position_tolerance) * 2, 0.03);  // 最低3cm
-
-    if (terminal_vel > 0 && next_vel.norm() < terminal_vel && distance_to_target > min_distance) {
-      // terminal_velocityが指定されており、低速かつ目標から離れている場合に補正
-      Eigen::Vector2d direction =
-        (Eigen::Vector2d(pos_mode.target_x, pos_mode.target_y) - current_position).normalized();
-      target_vel = direction * terminal_vel;
-    } else if (next_vel.norm() < 1e-6) {
-      // 目標到達済み
-      target_vel.setZero();
+      // 現在速度を目標方向に射影してスカラー初速度を算出
+      Eigen::Vector2d v0(command.current_velocity.x, command.current_velocity.y);
+      double v0_along_target = std::clamp(v0.dot(dir), 0.0, max_vel);
+
+      // 1D BangBangで目標方向の速度プロファイルを計算
+      BangBangTrajectory1D bb_trajectory;
+      bb_trajectory.generate(0.0, distance_to_target, v0_along_target, max_vel, max_acc, max_brk);
+
+      const double lookahead_time = 0.1;
+      double speed = bb_trajectory.getVelocity(lookahead_time);
+
+      addOrUpdatePlanningFactor(command, "BBAccel", formatPlanningDouble(max_acc, 1));
+      addOrUpdatePlanningFactor(command, "BBBrk", formatPlanningDouble(max_brk, 1));
+      addOrUpdatePlanningFactor(command, "BBMaxVel", formatPlanningDouble(max_vel, 1));
+      addOrUpdatePlanningFactor(command, "BBSpeed", formatPlanningDouble(speed, 2));
+
+      target_vel = dir * speed;
+
+      // terminal_velocity: パス等で目標通過速度が指定されている場合
+      const double terminal_vel = command.local_planner_config.terminal_velocity;
+      const double min_distance =
+        std::max(static_cast<double>(pos_mode.position_tolerance) * 2, 0.03);
+      if (terminal_vel > 0 && speed < terminal_vel && distance_to_target > min_distance) {
+        target_vel = dir * terminal_vel;
+      }
     } else {
-      target_vel << next_vel.x(), next_vel.y();
+      target_vel.setZero();
     }
     // 衝突ファール (crashing) 回避:
     // SSLルールでは衝突時の速度ベクトル差をロボット間直線に射影した値が
@@ -367,23 +338,27 @@ auto RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) ->
                           ? 0.0f
                           : PENALTY_AREA_TIME_HORIZON_OBST);
 
+    // 診断用: RVO2入力速度（crash/penalty修正後）
+    addOrUpdatePlanningFactor(
+      command, "PrefVel",
+      formatPlanningDouble(target_vel.x(), 2) + "," + formatPlanningDouble(target_vel.y(), 2));
+    addOrUpdatePlanningFactor(
+      command, "PrefVelAngle",
+      formatPlanningDouble(std::atan2(target_vel.y(), target_vel.x()) * 180.0 / M_PI, 1));
+
     rvo_sim->setAgentPrefVelocity(command.robot_id, toRVO(target_vel));
     rvo_sim->setAgentMaxSpeed(command.robot_id, max_vel);
 
-    // 速度計画トレースに計画点を追加
-    if (enable_velocity_plan_trace && !command.velocity_plan_trace.empty()) {
-      // 現在時刻から100ms後の予測位置・速度を記録
-      const int32_t target_time_us =
-        static_cast<int32_t>(lookahead_time * 1e6);  // 100ms = 100000us
-      Eigen::Vector2d predicted_pos = trajectory.getPosition(lookahead_time);
-      Eigen::Vector2d predicted_vel = next_vel;
-
-      VelocityPlanTracker::addPlanPoint(
-        command.velocity_plan_trace[0], "local_planner", predicted_pos, predicted_vel,
-        target_time_us,
-        0  // estimated_arrival_time_us（後で実装可能）
-      );
-    }
+    // [BangBang無効化] 速度計画トレース（trajectory/next_velに依存するためコメントアウト）
+    // if (enable_velocity_plan_trace && !command.velocity_plan_trace.empty()) {
+    //   const int32_t target_time_us =
+    //     static_cast<int32_t>(lookahead_time * 1e6);
+    //   Eigen::Vector2d predicted_pos = trajectory.getPosition(lookahead_time);
+    //   Eigen::Vector2d predicted_vel = next_vel;
+    //   VelocityPlanTracker::addPlanPoint(
+    //     command.velocity_plan_trace[0], "local_planner", predicted_pos, predicted_vel,
+    //     target_time_us, 0);
+    // }
   }
 
   for (const auto & enemy_robot : world_model->theirs().robots) {
@@ -420,6 +395,13 @@ auto RVO2Planner::extractVelocityCommandsFromRVOSim(
     auto pref_vel = toPoint(rvo_sim->getAgentPrefVelocity(original_command.robot_id));
     auto vel = toPoint(rvo_sim->getAgentVelocity(original_command.robot_id));
     addOrUpdatePlanningFactor(command, "RVO2PrefSpeed", formatPlanningDouble(pref_vel.norm()));
+    // 診断用: RVO2 prefVelocityと出力速度の角度差
+    addOrUpdatePlanningFactor(
+      command, "RVO2PrefVelAngle",
+      formatPlanningDouble(std::atan2(pref_vel.y(), pref_vel.x()) * 180.0 / M_PI, 1));
+    addOrUpdatePlanningFactor(
+      command, "RVO2OutVelAngle",
+      formatPlanningDouble(std::atan2(vel.y(), vel.x()) * 180.0 / M_PI, 1));
 
     // 速度修正をトレースに記録（RVO2による修正）
     if (enable_velocity_plan_trace && !command.velocity_plan_trace.empty()) {
@@ -475,8 +457,9 @@ auto RVO2Planner::extractVelocityCommandsFromRVOSim(
     command.polar_velocity_target_mode.front().target_velocity_theta =
       std::atan2(vel.y(), vel.x()) + theta_offset;
 
-    // 効率的な加速のための回転制御
-    if (command.local_planner_config.enable_rotation_stop_on_accel) {
+// 効率的な加速のための回転制御
+    // if (command.local_planner_config.enable_rotation_stop_on_accel) {
+	if(true){
       double move_angle = std::atan2(vel.y(), vel.x());
       double angle_diff = getAngleDiff(robot->pose.theta, move_angle);
 
@@ -497,7 +480,6 @@ auto RVO2Planner::extractVelocityCommandsFromRVOSim(
         command.omega_limit = 0.0;
       }
     }
-
     commands.robot_commands.emplace_back(command);
   }
 
@@ -718,15 +700,12 @@ auto RVO2Planner::adjustForPenaltyAreaAvoidance(
         } else if (isInBox(penalty_area, closest_point_2, PENALTY_AREA_OFFSET)) {
           target_pos = around_corner_2;
         } else {
-          static rclcpp::Clock steady_clock(RCL_STEADY_TIME);
-          RCLCPP_WARN_THROTTLE(
-            rclcpp::get_logger("rvo2_local_planner"), steady_clock, 1000,
-            "Failed to find a target position outside the penalty area. Fallback to current "
-            "position. current_pos:(%.3f, %.3f) target_pos:(%.3f, %.3f) "
-            "closest_point_1:(%.3f, %.3f) closest_point_2:(%.3f, %.3f)",
-            current_pos.x(), current_pos.y(), target_pos.x(), target_pos.y(), closest_point_1.x(),
-            closest_point_1.y(), closest_point_2.x(), closest_point_2.y());
-          target_pos = current_pos;
+          // フォールバック: 各角を経由した総距離が短い方を選択
+          const double dist_via_1 =
+            (around_corner_1 - current_pos).norm() + (target_pos - around_corner_1).norm();
+          const double dist_via_2 =
+            (around_corner_2 - current_pos).norm() + (target_pos - around_corner_2).norm();
+          target_pos = (dist_via_1 <= dist_via_2) ? around_corner_1 : around_corner_2;
         }
       }
     };

crane_msgs/msg/LocalPlannerConfig.msg

@@ -4,7 +4,6 @@ bool disable_placement_avoidance false
 bool disable_ball_avoidance false
 bool disable_field_boundary false
 bool disable_crash_avoidance false
-bool enable_rotation_stop_on_accel true
 
 NamedFloat[] max_velocity_factors
 NamedFloat final_planned_max_velocity

crane_msgs/msg/RobotFeedback.msg

@@ -1,6 +1,10 @@
 builtin_interfaces/Time received_stamp
 uint8 robot_id
 float32 packet_frequency_hz
+float32 receive_interval_mean_ms
+float32 receive_interval_min_ms
+float32 receive_interval_max_ms
+float32 receive_interval_stddev_ms
 
 uint8 counter