Lidar3D

clearpath_config/common.py

@@ -141,7 +141,7 @@ def assert_valid(ip: str) -> None:
 class Port:
     def __init__(self, port: int) -> None:
         self.assert_valid(port)
-        self.port = port
+        self.port = int(port)
 
     def __str__(self) -> str:
         return str(self.port)

clearpath_config/parser.py

@@ -29,9 +29,12 @@
     BaseCamera,
     FlirBlackfly,
     IntelRealsense,
-    # Lidars
+    # Lidar2D
     Lidar2D,
     BaseLidar2D,
+    # Lidar3D
+    Lidar3D,
+    BaseLidar3D,
     # IMU
     InertialMeasurementUnit,
     BaseIMU,
@@ -575,23 +578,6 @@ def __new__(cls, config: dict) -> BaseLidar2D:
         )
 
 
-class IMUParser(BaseConfigParser):
-    MODEL = "model"
-
-    def __new__(cls, config: dict) -> BaseIMU:
-        base = BaseSensorParser(config)
-        model = cls.get_required_val(IMUParser.MODEL, config)
-        imu = InertialMeasurementUnit(model)
-        # Set Base Parameters
-        imu.set_parent(base.get_parent())
-        imu.set_xyz(base.get_xyz())
-        imu.set_rpy(base.get_rpy())
-        imu.set_urdf_enabled(base.get_urdf_enabled())
-        imu.set_launch_enabled(base.get_launch_enabled())
-        imu.set_ros_parameters(base.get_ros_parameters())
-        return imu
-
-
 class Lidar2DParser(BaseConfigParser):
     MODEL = "model"
 
@@ -614,6 +600,66 @@ def __new__(cls, config: dict) -> BaseLidar2D:
         return lidar2d
 
 
+class BaseLidar3DParser(BaseConfigParser):
+    # Keys
+    IP = "ip"
+    PORT = "port"
+
+    def __new__(cls, config: dict) -> BaseLidar3D:
+        sensor = BaseSensorParser(config)
+        ip = cls.get_optional_val(
+            BaseLidar3DParser.IP, config, BaseLidar3D.IP_ADDRESS)
+        port = cls.get_optional_val(
+            BaseLidar3DParser.PORT, config, BaseLidar3D.IP_PORT)
+        return BaseLidar3D(
+            parent=sensor.get_parent(),
+            xyz=sensor.get_xyz(),
+            rpy=sensor.get_rpy(),
+            urdf_enabled=sensor.get_urdf_enabled(),
+            launch_enabled=sensor.get_launch_enabled(),
+            ros_parameters=sensor.get_ros_parameters(),
+            ip=ip,
+            port=port,
+        )
+
+
+class Lidar3DParser(BaseConfigParser):
+    MODEL = "model"
+
+    def __new__(cls, config: dict) -> BaseLidar3D:
+        base = BaseLidar3DParser(config)
+        model = cls.get_required_val(Lidar3DParser.MODEL, config)
+        lidar3d = Lidar3D(model)
+        # Set Base Parameters
+        lidar3d.set_parent(base.get_parent())
+        lidar3d.set_xyz(base.get_xyz())
+        lidar3d.set_rpy(base.get_rpy())
+        lidar3d.set_urdf_enabled(base.get_urdf_enabled())
+        lidar3d.set_launch_enabled(base.get_launch_enabled())
+        lidar3d.set_frame_id(base.get_frame_id())
+        lidar3d.set_ip(base.get_ip())
+        lidar3d.set_port(base.get_port())
+        lidar3d.set_ros_parameters(base.get_ros_parameters())
+        return lidar3d
+
+
+class IMUParser(BaseConfigParser):
+    MODEL = "model"
+
+    def __new__(cls, config: dict) -> BaseIMU:
+        base = BaseSensorParser(config)
+        model = cls.get_required_val(IMUParser.MODEL, config)
+        imu = InertialMeasurementUnit(model)
+        # Set Base Parameters
+        imu.set_parent(base.get_parent())
+        imu.set_xyz(base.get_xyz())
+        imu.set_rpy(base.get_rpy())
+        imu.set_urdf_enabled(base.get_urdf_enabled())
+        imu.set_launch_enabled(base.get_launch_enabled())
+        imu.set_ros_parameters(base.get_ros_parameters())
+        return imu
+
+
 class BaseCameraParser(BaseConfigParser):
     FPS = "fps"
     SERIAL = "serial"
@@ -759,6 +805,8 @@ def __new__(cls, model: str, config: dict) -> BaseSensor:
         Sensor.assert_type(model)
         if model == Sensor.LIDAR2D:
             return Lidar2DParser(config)
+        elif model == Sensor.LIDAR3D:
+            return Lidar3DParser(config)
         elif model == Sensor.CAMERA:
             return CameraParser(config)
         elif model == Sensor.IMU:
@@ -777,6 +825,8 @@ def __new__(cls, config: dict) -> SensorConfig:
             return snrconfig
         # Lidar2D
         snrconfig.set_all_lidar_2d(cls.get_sensors(sensors, Sensor.LIDAR2D))
+        # Lidar3D
+        snrconfig.set_all_lidar_3d(cls.get_sensors(sensors, Sensor.LIDAR3D))
         # Camera
         snrconfig.set_all_camera(cls.get_sensors(sensors, Sensor.CAMERA))
         # IMU

clearpath_config/sample/a200_config.yaml

@@ -103,6 +103,17 @@ sensors:
           pointcloud:
             enable: true
           anything: true
+    lidar3d:
+      - model: velodyne_lidar
+        urdf_enabled: true
+        launch_enabled: true
+        parent: base_link
+        xyz: [0.0, 0.0, 0.0]
+        rpy: [0.0, 0.0, 0.0]
+        ros_parameters:
+          device_ip: 192.168.131.20
+          port: 2368
+          model: VLP16
     lidar2d:
       - model: sick_lms1xx
         urdf_enabled: true

clearpath_config/sensors/lidars_3d.py

@@ -0,0 +1,227 @@
+from clearpath_config.common import IP, Port
+from clearpath_config.sensors.base import BaseSensor, Accessory, List
+
+
+class BaseLidar3D(BaseSensor):
+    """
+    Base 3D Lidar Class
+        - contains all common 3d lidar parameters:
+            - frame_id: to publish PointCloud data
+            - ip_address: to connect to lidar
+            - ip_port: to connect to lidar
+    """
+    SENSOR_TYPE = "lidar3d"
+    SENSOR_MODEL = "base"
+    TOPIC = "points"
+
+    FRAME_ID = "laser"
+    IP_ADDRESS = "192.168.131.25"
+    IP_PORT = "2368"
+
+    class ROS_PARAMETER_KEYS:
+        FRAME_ID = "frame_id"
+        IP_ADDRESS = "ip_address"
+        IP_PORT = "ip_port"
+
+    def __init__(
+            self,
+            idx: int = None,
+            name: str = None,
+            topic: str = TOPIC,
+            frame_id: str = FRAME_ID,
+            ip: str = IP_ADDRESS,
+            port: int = IP_PORT,
+            urdf_enabled: bool = BaseSensor.URDF_ENABLED,
+            launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: str = BaseSensor.ROS_PARAMETERS,
+            parent: str = Accessory.PARENT,
+            xyz: List[float] = Accessory.XYZ,
+            rpy: List[float] = Accessory.RPY
+            ) -> None:
+        # Frame ID
+        self.frame_id: str = self.FRAME_ID
+        self.set_frame_id(frame_id)
+        # IP Address
+        self.ip: IP = IP(self.IP_ADDRESS)
+        self.set_ip(ip)
+        # IP Port
+        self.port: Port = Port(self.IP_PORT)
+        self.set_port(port)
+        super().__init__(
+            idx,
+            name,
+            topic,
+            urdf_enabled,
+            launch_enabled,
+            ros_parameters,
+            parent,
+            xyz,
+            rpy
+        )
+        # ROS Parameter Keys
+        pairs = {
+            # Frame ID
+            BaseLidar3D.ROS_PARAMETER_KEYS.FRAME_ID: (
+                BaseSensor.ROSParameter(
+                    key=BaseLidar3D.ROS_PARAMETER_KEYS.FRAME_ID,
+                    get=lambda obj: obj.get_frame_id(),
+                    set=lambda obj, val: obj.set_frame_id(val)
+                )
+            ),
+            # IP Address
+            BaseLidar3D.ROS_PARAMETER_KEYS.IP_ADDRESS: (
+                BaseSensor.ROSParameter(
+                    key=BaseLidar3D.ROS_PARAMETER_KEYS.IP_ADDRESS,
+                    get=lambda obj: obj.get_ip(),
+                    set=lambda obj, val: obj.set_ip(val))),
+            # IP Port
+            BaseLidar3D.ROS_PARAMETER_KEYS.IP_PORT: (
+                BaseSensor.ROSParameter(
+                    key=BaseLidar3D.ROS_PARAMETER_KEYS.IP_PORT,
+                    get=lambda obj: obj.get_port(),
+                    set=lambda obj, val: obj.set_port(val))),
+        }
+        self.ros_parameter_pairs.update(pairs)
+        self.set_ros_parameters(ros_parameters)
+
+    @classmethod
+    def get_frame_id_from_idx(cls, idx: int) -> str:
+        return "%s_%s" % (
+            cls.get_name_from_idx(idx),
+            cls.FRAME_ID
+        )
+
+    @classmethod
+    def get_ip_from_idx(cls, idx: int) -> str:
+        ip = cls.IP_ADDRESS.split('.')
+        network_id = ip[0:3]
+        host_id = int(ip[-1]) + idx
+        return '.'.join(network_id) + '.' + str(host_id)
+
+    def set_idx(self, idx: int) -> None:
+        # Set Base: Name and Topic
+        super().set_idx(idx)
+        # Set Frame ID
+        self.set_frame_id(self.get_frame_id_from_idx(idx))
+        # Set IP
+        self.set_ip(self.get_ip_from_idx(idx))
+
+    def get_frame_id(self) -> str:
+        return self.frame_id
+
+    def set_frame_id(self, link: str) -> None:
+        Accessory.assert_valid_link(link)
+        self.frame_id = link
+
+    def get_ip(self) -> str:
+        return str(self.ip)
+
+    def set_ip(self, ip: str) -> None:
+        self.ip = IP(ip)
+
+    def get_port(self) -> int:
+        return int(self.port)
+
+    def set_port(self, port: int) -> None:
+        self.port = Port(port)
+
+
+class VelodyneLidar(BaseLidar3D):
+    """
+    Velodyne Lidar Class
+        - extra ros_parameters:
+            - device_type: model of the lidar:
+                - '64E'
+                - '64E_S3'
+                - '32E'
+                - '32C'
+                - 'VLP16'
+    """
+    SENSOR_MODEL = "velodyne_lidar"
+
+    FRAME_ID = "laser"
+    IP_PORT = 2368
+
+    HDL_32E = "32E"
+    HDL_64E = "64E"
+    HDL_64E_S2 = "64E_S2"
+    HDL_64E_S3 = "64E_S3"
+    VLP_16 = "VLP16"
+    VLP_32C = "32C"
+    DEVICE_TYPE = VLP_16
+    DEVICE_TYPES = [
+        HDL_32E,
+        HDL_64E,
+        HDL_64E_S2,
+        HDL_64E_S3,
+        VLP_16,
+        VLP_32C
+    ]
+
+    class ROS_PARAMETER_KEYS:
+        DEVICE_TYPE = "model"
+
+    def __init__(
+            self,
+            idx: int = None,
+            name: str = None,
+            topic: str = BaseLidar3D.TOPIC,
+            frame_id: str = FRAME_ID,
+            ip: str = BaseLidar3D.IP_ADDRESS,
+            port: int = IP_PORT,
+            device_type: str = DEVICE_TYPE,
+            urdf_enabled: bool = BaseSensor.URDF_ENABLED,
+            launch_enabled: bool = BaseSensor.LAUNCH_ENABLED,
+            ros_parameters: str = BaseSensor.ROS_PARAMETERS,
+            parent: str = Accessory.PARENT,
+            xyz: List[float] = Accessory.XYZ,
+            rpy: List[float] = Accessory.RPY
+            ) -> None:
+        # Device Type:
+        self.device_type: str = self.DEVICE_TYPE
+        self.set_device_type(device_type)
+        super().__init__(
+            idx,
+            name,
+            topic,
+            frame_id,
+            ip,
+            port,
+            urdf_enabled,
+            launch_enabled,
+            ros_parameters,
+            parent,
+            xyz,
+            rpy
+        )
+        # ROS Parameter Keys
+        self.ros_parameter_pairs[
+            BaseLidar3D.ROS_PARAMETER_KEYS.FRAME_ID].key = "frame_id"
+        self.ros_parameter_pairs[
+            BaseLidar3D.ROS_PARAMETER_KEYS.IP_ADDRESS].key = "device_ip"
+        self.ros_parameter_pairs[
+            BaseLidar3D.ROS_PARAMETER_KEYS.IP_PORT].key = "port"
+        pairs = {
+            # Device Type
+            self.ROS_PARAMETER_KEYS.DEVICE_TYPE: (
+                BaseSensor.ROSParameter(
+                    key=self.ROS_PARAMETER_KEYS.DEVICE_TYPE,
+                    get=lambda obj: obj.get_device_type(),
+                    set=lambda obj, val: obj.set_device_type(val)
+                )
+            ),
+        }
+        self.ros_parameter_pairs.update(pairs)
+        self.set_ros_parameters(ros_parameters)
+
+    def get_device_type(self) -> str:
+        return self.device_type
+
+    def set_device_type(self, device_type: str) -> None:
+        assert device_type in self.DEVICE_TYPES, (
+            "Device type '%s' is not one of '%s'" % (
+                device_type,
+                self.DEVICE_TYPES
+            )
+        )
+        self.device_type = device_type