Merge pull request #151 from cogip/150-avoidance-robot-in-obstacle-goes-outside-the-table

cpp: libraries: fix ObstacleCircle nearest_point() method

Author: ecourtois

Dockerfile

@@ -19,11 +19,14 @@ ENV UV_PYTHON_INSTALL_DIR=/opt/python
 # Required because mcu-firmware is not compatible with uv
 ENV PATH="/src/.venv/bin:${PATH}"
 
+# Set frozen uv.lock globally
+ENV UV_FROZEN=1
+
 # Pre-install some Python requirements for COGIP tools
 RUN --mount=type=bind,source=uv.lock,target=uv.lock \
     --mount=type=bind,source=pyproject.toml,target=pyproject.toml \
     --mount=type=bind,source=.python-version,target=.python-version \
-    uv sync --no-install-project --frozen
+    uv sync --no-install-project
 
 FROM uv_base AS cogip-console
 
@@ -59,7 +62,7 @@ RUN group_exists=$(getent group ${GID} || true) && echo $group_exists \
 
 ADD .python-version uv.lock pyproject.toml CMakeLists.txt LICENSE /src/
 ADD cogip /src/cogip
-RUN uv sync --frozen
+RUN uv sync
 
 CMD ["sleep", "infinity"]
 

cogip/cpp/libraries/avoidance/Avoidance.cpp

@@ -58,6 +58,7 @@ bool Avoidance::avoidance(const models::Coords& start,
         }
         if (current_obstacle.is_point_inside(start_pose_)) {
             start_pose_ = current_obstacle.nearest_point(start_pose_);
+            logger_.debug() << "start pose inside obstacle, updated: " << start_pose_ << std::endl;
         }
     }
 

cogip/cpp/libraries/obstacles/ObstacleCircle.cpp

@@ -83,11 +83,26 @@ bool ObstacleCircle::is_segment_crossing(const models::Coords& a, const models::
 
 models::Coords ObstacleCircle::nearest_point(const models::Coords& p)
 {
+    // Vector from the circle center to the given point
     models::Coords vect(p.x() - data_->center.x, p.y() - data_->center.y);
     double vect_norm = std::hypot(vect.x(), vect.y());
 
-    double scale = (data_->radius * (1 + data_->bounding_box_margin)) / vect_norm;
+    // Effective radius including the bounding box margin
+    double effective_radius = data_->radius + data_->bounding_box_margin;
 
+    // Special case: if the point is exactly at the center, return a point on the circle
+    // This case should never
+    if (vect_norm == 0) {
+        return models::Coords(
+            data_->center.x + effective_radius,
+            data_->center.y
+        );
+    }
+
+    // Scale the vector to project the point onto the circle perimeter
+    double scale = effective_radius / vect_norm;
+
+    // Return the projected point on the circle
     return models::Coords(
         data_->center.x + vect.x() * scale,
         data_->center.y + vect.y() * scale

docker-compose.yml

@@ -138,7 +138,7 @@ x-robot: &robot
     - -x
     - -c
     - |
-      uv sync --frozen --reinstall-package cogip-tools -C build-dir=/src/build/$${ROBOT_ID}
+      uv sync --reinstall-package cogip-tools -C build-dir=/src/build/$${ROBOT_ID}
       gosu $${UID}:$${GID} uv run cogip-server &
       wait-for-it -t 0 localhost:809$${ROBOT_ID}
       gosu $${UID}:$${GID} uv run cogip-planner &
@@ -187,7 +187,7 @@ x-pami: &pami
     - -x
     - -c
     - |
-      uv sync --frozen --reinstall-package cogip-tools -C build-dir=/src/build/$${ROBOT_ID}
+      uv sync --reinstall-package cogip-tools -C build-dir=/src/build/$${ROBOT_ID}
       gosu $${UID}:$${GID} uv run cogip-server &
       wait-for-it -t 0 localhost:809$${ROBOT_ID}
       gosu $${UID}:$${GID} uv run cogip-planner &