From 12a83e876a0d54648477cef19ab4eaea98ed1760 Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Thu, 16 Apr 2026 03:14:34 +0200
Subject: [PATCH] Fixed dockerfile to upgrade packages after updating

---
 Dockerfile                                            | 3 ++-
 toid_behaviors/include/toid_behaviors/simple_move.hpp | 8 +++++---
 2 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/Dockerfile b/Dockerfile
index d007a35..9df8837 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -3,7 +3,7 @@ FROM ros:jazzy-perception
 ENV DEBIAN_FRONTEND=noninteractive
 
 # ---------- System dependencies ----------
-RUN apt-get update && apt-get install -y \
+RUN apt-get update && apt-get upgrade && apt-get install -y \
     python3-colcon-common-extensions \
     python3-rosdep \
     meson cmake \
@@ -47,6 +47,7 @@ COPY toid_bt/package.xml                 toid_bt/package.xml
 COPY toid_spinner_controller/package.xml toid_spinner_controller/package.xml
 COPY toid_behaviors/package.xml          toid_behaviors/package.xml
 COPY toid_navigation/package.xml         toid_navigation/package.xml
+COPY toid_vision/package.xml             toid_vision/package.xml
 COPY ext/camera_ros/package.xml          ext/camera_ros/package.xml
 COPY ext/BehaviorTree.ROS2/              ext/BehaviorTree.ROS2/
 
diff --git a/toid_behaviors/include/toid_behaviors/simple_move.hpp b/toid_behaviors/include/toid_behaviors/simple_move.hpp
index fefd9f5..135e6bf 100644
--- a/toid_behaviors/include/toid_behaviors/simple_move.hpp
+++ b/toid_behaviors/include/toid_behaviors/simple_move.hpp
@@ -127,17 +127,18 @@ public:
 
   bool check_rival_collision(geometry_msgs::msg::Pose2D & pose)
   {
-    if (!rivals_) {
+    if (!rivals_ || (this->clock_->now() - rivals_->header.stamp ) > rclcpp::Duration::from_seconds(1.0)) {
       return false;
     }
     const double cosp = std::cos(pose.theta);
     const double sinp = std::sin(pose.theta);
+    int i = 0;
     for (const auto & rival : rivals_->point) {
       geometry_msgs::msg::Point local_rival;
       const double dx = rival.x - pose.x;
       const double dy = rival.y - pose.y;
-      local_rival.x = dx * cosp + dy * sinp;
-      local_rival.y = -dx * sinp + dy * cosp;
+      local_rival.x = dx * cosp - dy * sinp;
+      local_rival.y = dx * sinp + dy * cosp;
 
       const double qx = std::abs(local_rival.x) - robot_length_ / 2.0;
       const double qy = std::abs(local_rival.y) - robot_width_ / 2.0;
@@ -148,6 +149,7 @@ public:
       double length = std::sqrt(mqx * mqx + mqy * mqy);
 
       double sdf = length + std::min(std::max(qx, qy), 0.0);
+      RCLCPP_INFO(this->logger_, "Distance to rival %d: %lf", i++, sdf-rival_radius_);
       if (sdf < rival_radius_) {
         return true;
       }