Merge branch 'develop' into positive-redirect

ibis-ssl · Oct 12, 2024 · 2835117 · 2835117
2 parents e3334bc + 6a0a86b
commit 2835117
Showing 1 changed file with 23 additions and 0 deletions.
diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp
@@ -368,6 +368,29 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
         }
       }
 
+      if (not command.local_planner_config.disable_ball_avoidance) {
+        const Point current_pos = Point(command.current_pose.x, command.current_pose.y);
+        const auto & ball_pos = world_model->ball.pos;
+        const double MIN_BALL_DISTANCE = 0.2;
+        if ((target_pos - ball_pos).norm() < MIN_BALL_DISTANCE) {
+          target_pos = ball_pos + (target_pos - ball_pos).normalized() * MIN_BALL_DISTANCE;
+        }
+        if ((current_pos - ball_pos).norm() < MIN_BALL_DISTANCE) {
+          // 現在位置が近い場合は、最優先で離れる
+          target_pos =
+            ball_pos + (current_pos - ball_pos).normalized() * (MIN_BALL_DISTANCE + 0.05);
+        } else {
+          Segment move_line(current_pos, target_pos);
+          auto [distance, closest_point] = getClosestPointAndDistance(ball_pos, move_line);
+          if (
+            closest_point != ball_pos && closest_point != target_pos &&
+            distance < MIN_BALL_DISTANCE) {
+            // 少しずらす
+            target_pos = ball_pos + (closest_point - ball_pos).normalized() * MIN_BALL_DISTANCE;
+          }
+        }
+      }
+
       command.position_target_mode.front().target_x = target_pos.x();
       command.position_target_mode.front().target_y = target_pos.y();
       visualizer->addLine(