Merge pull request #164 from cogip/155-work-on-strategy-2025

Work on strategy 2025

Author: ecourtois

.gitignore

@@ -4,6 +4,7 @@ py.typed
 *.pyi
 *.egg-info
 .vscode
+!raspios/overlay-rootfs/root/.vscode
 dist
 tests
 *.tree.dot

cogip/cpp/drivers/ydlidar_g2/YDLidar.cpp

@@ -289,7 +289,6 @@ void YDLidar::updateSharedMemory() {
             data_write_lock_->finishWriting();
             data_write_lock_->postUpdate();
         }
-
         auto loop_end_time = std::chrono::steady_clock::now();
         auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(loop_end_time - loop_start_time);
         if (elapsed_time < std::chrono::milliseconds(refresh_interval_)) {
@@ -412,7 +411,7 @@ inline bool printVersionInfo(const device_info_t& info) {
               << "  - Serial: ";
 
     for (int i = 0; i < 16; i++) {
-        std::cout << std::hex << (info.serialnum[i] & 0xff);
+        std::cout << std::hex << (info.serialnum[i] & 0xff) << std::dec;
     }
 
     std::cout << std::endl;

cogip/cpp/drivers/ydlidar_g2/YDLidarDriver.cpp

@@ -714,7 +714,7 @@ result_t YDlidarDriver::parseResponseHeader(
                     }
                     if (csc != csr) {
                         std::cerr << "Stamp checksum error c[0x" << std::hex << static_cast<int>(csc)
-                                  << "] != r[0x" << static_cast<int>(csr) << "]" << std::endl;
+                                  << "] != r[0x" << static_cast<int>(csr) << "]" << std::dec << std::endl;
                     }
                     else {
                         stamp_package_t sp;

cogip/models/actuators.py

@@ -32,7 +32,7 @@ class ActuatorBase(BaseModel):
 class PositionalActuatorEnum(IntEnum):
     """Enum defining positional actuators IDs"""
 
-    UNDEFINED = 0
+    MOTOR_LIFT = 0
 
 
 class PositionalActuatorCommand(BaseModel):
@@ -47,6 +47,20 @@ class PositionalActuatorCommand(BaseModel):
         title="Position Command",
         description="Current positional actuator position command",
     )
+    speed: int = Field(
+        100,
+        ge=1,
+        le=100,
+        title="Speed",
+        description="Speed",
+    )
+    timeout: int = Field(
+        2000,
+        ge=100,
+        le=5000,
+        title="Timeout",
+        description="Timeout",
+    )
 
     @field_validator("kind", mode="before")
     @classmethod
@@ -77,6 +91,8 @@ def pb_copy(self, message: PB_PositionalActuatorCommand) -> None:
         """Copy values to Protobuf message"""
         message.id = self.id
         message.command = self.command
+        message.speed = self.speed
+        message.timeout = self.timeout
 
 
 class PositionalActuator(ActuatorBase, PositionalActuatorCommand):
@@ -119,4 +135,6 @@ class BoolSensor(BaseModel):
 
 
 # Actuator limits
-actuator_limits: dict[IntEnum, tuple[int, int]] = {}
+actuator_limits: dict[IntEnum, tuple[int, int]] = {
+    PositionalActuatorEnum.MOTOR_LIFT: (0, 150),
+}

cogip/models/artifacts.py

@@ -1,6 +1,6 @@
 from enum import IntEnum, auto
 
-from .models import Pose
+from .models import Pose, Vertex
 
 
 class ConstructionAreaID(IntEnum):
@@ -9,9 +9,13 @@ class ConstructionAreaID(IntEnum):
     """
 
     LocalBottomSmall = auto()
-    LocalBottomLarge = auto()
+    LocalBottomLarge1 = auto()
+    LocalBottomLarge2 = auto()
+    LocalBottomLarge3 = auto()
     OppositeBottomSmall = auto()
-    OppositeCenterLarge = auto()
+    OppositeSideLarge1 = auto()
+    OppositeSideLarge2 = auto()
+    OppositeSideLarge3 = auto()
 
 
 class ConstructionArea(Pose):
@@ -21,9 +25,9 @@ class ConstructionArea(Pose):
     """
 
     id: ConstructionAreaID
-    length: float = 450
-    width: float
-    free_slots: float
+    length: float
+    width: float = 450
+    tribune_level: int = 0
     enabled: bool = True
 
 
@@ -33,8 +37,7 @@ class ConstructionAreaSmall(ConstructionArea):
     Coordinates indicate the center of the tribune.
     """
 
-    width: float = 150
-    free_slots: int = 1
+    length: float = 150
 
 
 class ConstructionAreaLarge(ConstructionArea):
@@ -43,16 +46,19 @@ class ConstructionAreaLarge(ConstructionArea):
     Coordinates indicate the center of the tribune.
     """
 
-    width: float = 450
-    free_slots: int = 3
+    length: float = 450
 
 
 # Default positions for blue camp
 construction_area_positions: dict[ConstructionAreaID, Pose] = {
-    ConstructionAreaID.LocalBottomSmall: Pose(x=-925, y=-725, O=180),
-    ConstructionAreaID.LocalBottomLarge: Pose(x=-755, y=-275, O=180),
-    ConstructionAreaID.OppositeBottomSmall: Pose(x=-925, y=1275, O=180),
-    ConstructionAreaID.OppositeCenterLarge: Pose(x=-125, y=1275, O=90),
+    ConstructionAreaID.LocalBottomSmall: Pose(x=-925, y=-725, O=0),
+    ConstructionAreaID.LocalBottomLarge1: Pose(x=-925, y=-265, O=0),
+    ConstructionAreaID.LocalBottomLarge2: Pose(x=-775, y=-265, O=0),
+    ConstructionAreaID.LocalBottomLarge3: Pose(x=-625, y=-265, O=0),
+    ConstructionAreaID.OppositeBottomSmall: Pose(x=-925, y=1275, O=0),
+    ConstructionAreaID.OppositeSideLarge1: Pose(x=-125, y=1425, O=-90),
+    ConstructionAreaID.OppositeSideLarge2: Pose(x=-125, y=1275, O=-90),
+    ConstructionAreaID.OppositeSideLarge3: Pose(x=-125, y=1155, O=-90),
 }
 
 
@@ -63,6 +69,7 @@ class TribuneID(IntEnum):
 
     LocalCenter = auto()
     LocalTop = auto()
+    LocalTopTraining = auto()
     LocalBottom = auto()
     LocalTopSide = auto()
     LocalBottomSide = auto()
@@ -92,14 +99,42 @@ class Tribune(Pose):
 
 # Default positions for blue camp
 tribune_positions: dict[TribuneID, Pose] = {
-    TribuneID.LocalCenter: Pose(x=-50, y=-400, O=0),
-    TribuneID.LocalTop: Pose(x=725, y=-675, O=0),
-    TribuneID.LocalTopSide: Pose(x=325, y=-1425, O=-90),
-    TribuneID.LocalBottomSide: Pose(x=-600, y=-1425, O=-90),
-    TribuneID.LocalBottom: Pose(x=-750, y=-725, O=180),
-    TribuneID.OppositeCenter: Pose(x=-50, y=400, O=0),
-    # TribuneID.OppositeTop: Pose(x=725, y=675, O=0),  # Included in a fixed obstacle
-    TribuneID.OppositeTopSide: Pose(x=325, y=1425, O=90),
-    TribuneID.OppositeBottomSide: Pose(x=-600, y=1425, O=90),
-    TribuneID.OppositeBottom: Pose(x=-750, y=725, O=180),
+    TribuneID.LocalCenter: Pose(x=-50, y=-400, O=180),
+    TribuneID.LocalTop: Pose(x=725, y=-675, O=180),
+    TribuneID.LocalTopTraining: Pose(x=-50, y=-1100, O=180),
+    TribuneID.LocalTopSide: Pose(x=325, y=-1425, O=90),
+    TribuneID.LocalBottomSide: Pose(x=-600, y=-1425, O=90),
+    TribuneID.LocalBottom: Pose(x=-750, y=-725, O=0),
+    TribuneID.OppositeCenter: Pose(x=-50, y=400, O=180),
+    # TribuneID.OppositeTop: Pose(x=725, y=675, O=180),  # Included in a fixed obstacle
+    TribuneID.OppositeTopSide: Pose(x=325, y=1425, O=-90),
+    TribuneID.OppositeBottomSide: Pose(x=-600, y=1425, O=-90),
+    TribuneID.OppositeBottom: Pose(x=-750, y=725, O=0),
 }
+
+
+class FixedObstacleID(IntEnum):
+    """
+    Enum to identify fixed obstacles.
+    """
+
+    Ramp = auto()
+    Scene = auto()
+    PitArea = auto()
+    PamiStartArea = auto()
+    Pami5Path = auto()
+    OpponentRamp = auto()
+    OpponentScene = auto()
+    OpponentPitArea = auto()
+    Backstage = auto()
+
+
+class FixedObstacle(Vertex):
+    """
+    Model for fixed obstacles.
+    """
+
+    id: FixedObstacleID
+    length: float
+    width: float
+    enabled: bool = True

cogip/models/models.py

@@ -120,6 +120,7 @@ class PathPose(Pose):
         bypass_anti_blocking: send pose_reached if robot is blocked
         timeout_ms: max time is milliseconds to reach the pose, the robot stops if timeout is reached, 0 for no timeout
         bypass_final_orientation: do not set orientation pose order
+        is_intermediate: whether this pose is an intermediate pose in a path
     """
 
     max_speed_linear: int = 66
@@ -128,6 +129,7 @@ class PathPose(Pose):
     bypass_anti_blocking: bool = False
     timeout_ms: int = 0
     bypass_final_orientation: bool = False
+    is_intermediate: bool = False
 
     @property
     def pose(self) -> Pose:
@@ -149,6 +151,7 @@ def copy_pb(self, pb_path_pose: PB_PathPose) -> None:
         pb_path_pose.bypass_anti_blocking = self.bypass_anti_blocking
         pb_path_pose.timeout_ms = self.timeout_ms
         pb_path_pose.bypass_final_orientation = self.bypass_final_orientation
+        pb_path_pose.is_intermediate = self.is_intermediate
 
 
 class DynObstacleRect(BaseModel):

cogip/protobuf/PB_Actuators.proto

@@ -1 +1 @@
-../../submodules/mcu-firmware/platforms/pf-robot-actuators/PB_Actuators.proto
+../../submodules/mcu-firmware/lib/actuator/PB_Actuators.proto