reachy2_sdk.sensors.lidar

  1"""Reachy Lidar module.
  2
  3Handles all specific methods to the Lidar sensor.
  4"""
  5
  6import logging
  7from typing import Optional
  8
  9import cv2
 10import grpc
 11import numpy as np
 12import numpy.typing as npt
 13from google.protobuf.wrappers_pb2 import BoolValue, FloatValue
 14from reachy2_sdk_api.mobile_base_lidar_pb2 import (
 15    LidarObstacleDetectionEnum,
 16    LidarSafety,
 17)
 18from reachy2_sdk_api.mobile_base_lidar_pb2_grpc import MobileBaseLidarServiceStub
 19
 20from ..parts.part import Part
 21
 22
 23class Lidar:
 24    """LIDAR class for mobile base SDK."""
 25
 26    def __init__(self, initial_state: LidarSafety, grpc_channel: grpc.Channel, part: Part) -> None:
 27        """Initialize the LIDAR class."""
 28        self._logger = logging.getLogger(__name__)
 29        self._stub = MobileBaseLidarServiceStub(grpc_channel)
 30        self._part = part
 31
 32        self._safety_enabled: bool = initial_state.safety_on.value
 33        self._safety_distance: float = initial_state.safety_distance.value
 34        self._critical_distance: float = initial_state.critical_distance.value
 35
 36        self._obstacle_detection_status: str = LidarObstacleDetectionEnum.Name(initial_state.obstacle_detection_status.status)
 37
 38    def __repr__(self) -> str:
 39        """Clean representation of a Reachy."""
 40        return f"""<Lidar safety_enabled={self.safety_enabled}>"""
 41
 42    def get_map(self) -> Optional[npt.NDArray[np.uint8]]:
 43        """Retrieve the current map of the environment using lidar data.
 44
 45        Returns:
 46            The current map of the environment as an image (NumPy array) if the lidar map is successfully
 47            retrieved. Returns `None` if no lidar map is retrieved.
 48        """
 49        compressed_map = self._stub.GetLidarMap(self._part._part_id)
 50        if compressed_map.data == b"":
 51            self._logger.error("No lidar map retrieved. (Note that the lidar map is not available in FAKE mode)")
 52            return None
 53        np_data = np.frombuffer(compressed_map.data, np.uint8)
 54        img = cv2.imdecode(np_data, cv2.IMREAD_COLOR)
 55        return img.astype(np.uint8)
 56
 57    @property
 58    def safety_slowdown_distance(self) -> float:
 59        """Get the safety distance from obstacles in meters for the mobile base.
 60
 61        The mobile base's speed is slowed down if the direction of speed matches the direction of
 62        at least one LIDAR point within the safety distance range.
 63        """
 64        return float(self._safety_distance)
 65
 66    @safety_slowdown_distance.setter
 67    def safety_slowdown_distance(self, value: float) -> None:
 68        """Set the safety distance for a Lidar sensor.
 69
 70        Args:
 71            value: The safety distance to set for the LidarSafety object. This value specifies
 72                the distance at which a safety slowdown should be initiated.
 73        """
 74        self._stub.SetZuuuSafety(
 75            LidarSafety(
 76                safety_on=BoolValue(value=self.safety_enabled),
 77                safety_distance=FloatValue(value=value),
 78                critical_distance=FloatValue(value=self.safety_critical_distance),
 79            )
 80        )
 81
 82    @property
 83    def safety_critical_distance(self) -> float:
 84        """Get the critical distance in meters of the mobile base from obstacles.
 85
 86        The mobile base's speed is reduced to zero if the direction of speed matches the direction
 87        of at least one LIDAR point within the critical distance range. If at least one point is
 88        within the critical distance, even movements that move away from the obstacles are slowed
 89        down to the "safety_zone" speed.
 90        """
 91        return float(self._critical_distance)
 92
 93    @safety_critical_distance.setter
 94    def safety_critical_distance(self, value: float) -> None:
 95        """Set the critical distance for a Lidar safety feature.
 96
 97        Args:
 98            value: The critical distance in meters for safety. This value specifies the distance
 99                at which the mobile base should stop if moving in the direction of an obstacle.
100                If at least one point is within the critical distance, even movements that move
101                away from the obstacles are slowed down to the "safety_zone" speed.
102        """
103        self._stub.SetZuuuSafety(
104            LidarSafety(
105                safety_on=BoolValue(value=self.safety_enabled),
106                safety_distance=FloatValue(value=self.safety_slowdown_distance),
107                critical_distance=FloatValue(value=value),
108            )
109        )
110
111    @property
112    def safety_enabled(self) -> bool:
113        """Get the current status of the safety feature.
114
115        Returns:
116            A boolean indicating whether the safety feature is enabled. If `True`, the safety feature is enabled.
117        """
118        return self._safety_enabled
119
120    @safety_enabled.setter
121    def safety_enabled(self, value: bool) -> None:
122        """Set the safety status for the Lidar device.
123
124        Args:
125            value: A boolean indicating whether the safety features are enabled or disabled. If `True`, the safety feature
126                is enabled.
127        """
128        self._stub.SetZuuuSafety(
129            LidarSafety(
130                safety_on=BoolValue(value=value),
131                safety_distance=FloatValue(value=self.safety_slowdown_distance),
132                critical_distance=FloatValue(value=self.safety_critical_distance),
133            )
134        )
135
136    @property
137    def obstacle_detection_status(self) -> str:
138        """Get the status of the lidar obstacle detection.
139
140        Returns:
141            The status of the lidar obstacle detection, which can be one of the following values:
142            NO_OBJECT_DETECTED, OBJECT_DETECTED_SLOWDOWN, OBJECT_DETECTED_STOP, or DETECTION_ERROR.
143        """
144        return self._obstacle_detection_status
145
146    def reset_safety_default_distances(self) -> None:
147        """Reset default distance values for safety detection.
148
149        The reset values include:
150        - safety_critical_distance
151        - safety_slowdown_distance.
152        """
153        self._stub.ResetDefaultSafetyDistances(self._part._part_id)
154
155    def _update_with(self, new_lidar_state: LidarSafety) -> None:
156        """Update lidar information with a new state received from a gRPC server.
157
158        Args:
159            new_lidar_state: Contains information about the lidar state received from the gRPC server.
160        """
161        self._safety_enabled = new_lidar_state.safety_on.value
162        self._safety_distance = new_lidar_state.safety_distance.value
163        self._critical_distance = new_lidar_state.critical_distance.value
164
165        self._obstacle_detection_status = LidarObstacleDetectionEnum.Name(new_lidar_state.obstacle_detection_status.status)