Add A300 Observer attachments

clearpath_generator_common/clearpath_generator_common/description/attachments.py

@@ -32,6 +32,7 @@
 from typing import List
 
 from clearpath_config.platform.attachments.a200 import A200Attachment
+from clearpath_config.platform.attachments.a300 import A300Attachment
 from clearpath_config.platform.attachments.config import BaseAttachment
 from clearpath_config.platform.attachments.dd100 import DD100Attachment
 from clearpath_config.platform.attachments.dd150 import DD150Attachment
@@ -125,6 +126,13 @@ def __init__(self, attachment: BaseAttachment) -> None:
         A200Attachment.TOP_PLATE: BaseDescription,
         A200Attachment.SENSOR_ARCH: BaseDescription,
         A200Attachment.OBSERVER_BACKPACK: BaseDescription,
+        # A300
+        A300Attachment.BUMPER: BumperDescription,
+        A300Attachment.TOP_PLATE: BaseDescription,
+        A300Attachment.AMP_FRAME: BaseDescription,
+        A300Attachment.OBSERVER_BACKPACK: BaseDescription,
+        A300Attachment.OBSERVER_ARCH: BaseDescription,
+        A300Attachment.SPOTLIGHT: BaseDescription,
         # J100
         J100Attachment.FENDER: BaseDescription,
         J100Attachment.TOP_PLATE: BaseDescription,

clearpath_generator_common/clearpath_generator_common/description/sensors.py

@@ -193,15 +193,36 @@ def __init__(self, sensor: BaseINS) -> None:
     class OusterOS1Description(Lidar3dDescription):
         SAMPLES_HORIZONTAL = 'samples_h'
         SAMPLES_VERTICAL = 'samples_v'
+        BASE_TYPE = 'base'
+        CAP_TYPE = 'cap'
 
-        def __init__(self, sensor: BaseLidar3D) -> None:
+        def __init__(self, sensor: OusterOS1) -> None:
             super().__init__(sensor)
 
             del self.parameters[self.ANGULAR_RESOLUTION_H]
             del self.parameters[self.ANGULAR_RESOLUTION_V]
             self.parameters.update({
                 self.SAMPLES_HORIZONTAL: 1024,
-                self.SAMPLES_VERTICAL: 64
+                self.SAMPLES_VERTICAL: 64,
+                self.BASE_TYPE: sensor.base_type,
+                self.CAP_TYPE: sensor.cap_type,
+            })
+
+    class SeyondLidarDescription(Lidar3dDescription):
+
+        def __init__(self, sensor: BaseLidar3D) -> None:
+            super().__init__(sensor)
+
+            self.parameters.update({
+                self.ANGULAR_RESOLUTION_H: 0.01,
+                self.ANGULAR_RESOLUTION_V: 0.01,
+                self.MINIMUM_ANGLE_H: -1.0471975511965976,
+                self.MAXIMUM_ANGLE_H: 1.0471975511965976,
+                self.MINIMUM_ANGLE_V: -0.6108652381980153,
+                self.MAXIMUM_ANGLE_V: 0.6108652381980153,
+                self.MINIMUM_RANGE: 0.1,
+                self.MAXIMUM_RANGE: 150.0,
+                self.UPDATE_RATE: 20  # TODO: link to clearpath_config property
             })
 
     class ImuDescription(BaseDescription):
@@ -276,7 +297,7 @@ def __init__(self, sensor: StereolabsZed) -> None:
         AxisCamera.SENSOR_MODEL: AxisCameraDescription,
         Microstrain.SENSOR_MODEL: ImuDescription,
         OusterOS1.SENSOR_MODEL: OusterOS1Description,
-        SeyondLidar.SENSOR_MODEL: Lidar3dDescription,
+        SeyondLidar.SENSOR_MODEL: SeyondLidarDescription,
         VelodyneLidar.SENSOR_MODEL: Lidar3dDescription,
         CHRoboticsUM6.SENSOR_MODEL: ImuDescription,
         RedshiftUM7.SENSOR_MODEL: ImuDescription,

clearpath_platform_description/urdf/a300/attachments/observer_arch.urdf.xacro

@@ -0,0 +1,96 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://wiki.ros.org.xacro">
+  <xacro:macro name="observer_arch" params="name parent_link:=default_mount model:=observer_arch *origin">
+    <link name="${name}_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://clearpath_platform_description/meshes/a300/attachments/observer_arch.stl" scale="0.001 0.001 0.001" />
+        </geometry>
+        <material name="yellow"><color rgba="0.8 0.8 0.0 1.0" /></material>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://clearpath_platform_description/meshes/a300/attachments/observer_arch.stl" scale="0.001 0.001 0.001" />
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </collision>
+    </link>
+    <gazebo reference="${name}_link">
+      <material>Gazebo/DarkYellow</material>
+    </gazebo>
+    <joint name="${name}_joint" type="fixed">
+      <parent link="${parent_link}" />
+      <child link="${name}_link" />
+      <xacro:insert_block name="origin"/>
+    </joint>
+
+    <!--
+        Sensor mounting points
+    -->
+
+    <link name="${name}_left_antenna_mount" />
+    <joint name="${name}_left_antenna_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_left_antenna_mount" />
+      <origin xyz="0 0.260 0.44536" rpy="0 0 0" />
+    </joint>
+
+    <link name="${name}_right_antenna_mount" />
+    <joint name="${name}_right_antenna_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_right_antenna_mount" />
+      <origin xyz="0 -0.260 0.44536" rpy="0 0 0" />
+    </joint>
+
+    <link name="${name}_lidar_mount" />
+    <joint name="${name}_lidar_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_lidar_mount" />
+      <origin xyz="0.130 0 0.30256" rpy="3.14159 0 0" />
+    </joint>
+
+    <!-- front-facing camera suspended under sensor arch -->
+    <link name="${name}_front_camera_mount" />
+    <joint name="${name}_front_camera_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_front_camera_mount" />
+      <origin xyz="0.0775 0 0.40896" rpy="0 0 0" />
+    </joint>
+
+    <!-- rear-facing camera suspended under sensor arch -->
+    <link name="${name}_rear_camera_mount" />
+    <joint name="${name}_rear_camera_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_rear_camera_mount" />
+      <origin xyz="-0.09053483924399536 0 0.39093965966919125" rpy="0 0.6021385919380436 3.14159" />
+    </joint>
+
+    <!-- corner mounts for lights, etc... -->
+    <link name="${name}_front_left_corner_mount" />
+    <joint name="${name}_front_left_corner_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_front_left_corner_mount" />
+      <origin xyz="0.07 0.310 0.44536" rpy="0 0 0" />
+    </joint>
+    <link name="${name}_front_right_corner_mount" />
+    <joint name="${name}_front_right_corner_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_front_right_corner_mount" />
+      <origin xyz="0.07 -0.310 0.44536" rpy="0 0 0" />
+    </joint>
+    <link name="${name}_rear_left_corner_mount" />
+    <joint name="${name}_rear_left_corner_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_rear_left_corner_mount" />
+      <origin xyz="-0.07 0.310 0.44536" rpy="0 0 3.14159" />
+    </joint>
+    <link name="${name}_rear_right_corner_mount" />
+    <joint name="${name}_rear_right_corner_mount_joint" type="fixed">
+      <parent link="${name}_link" />
+      <child link="${name}_rear_right_corner_mount" />
+      <origin xyz="-0.07 -0.310 0.44536" rpy="0 0 3.14159" />
+    </joint>
+
+  </xacro:macro>
+</robot>

clearpath_platform_description/urdf/a300/attachments/observer_backpack.urdf.xacro

@@ -0,0 +1,82 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://wiki.ros.org.xacro">
+  <xacro:macro name="observer_backpack" params="name parent_link:=base_link model:=observer_backpack *origin">
+    <link name="${name}_enclosure_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://clearpath_platform_description/meshes/a300/attachments/observer_enclosure.stl" scale="0.001 0.001 0.001" />
+        </geometry>
+        <material name="dark_grey"><color rgba="0.1 0.1 0.1 1.0" /></material>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://clearpath_platform_description/meshes/a300/attachments/observer_enclosure.stl" scale="0.001 0.001 0.001" />
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </collision>
+    </link>
+    <gazebo reference="${name}_enclosure_link">
+      <material>Gazebo/Black</material>
+    </gazebo>
+    <joint name="${name}_enclosure_joint" type="fixed">
+      <parent link="${parent_link}" />
+      <child link="${name}_enclosure_link" />
+      <xacro:insert_block name="origin"/>
+    </joint>
+
+    <link name="${name}_access_panels_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://clearpath_platform_description/meshes/a300/attachments/observer_access_panels.stl" scale="0.001 0.001 0.001" />
+        </geometry>
+        <material name="yellow"><color rgba="1 0.6038269 0 1.0" /></material>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </visual>
+    </link>
+    <joint name="${name}_access_panels_joint" type="fixed">
+      <parent link="${name}_enclosure_link" />
+      <child link="${name}_access_panels_link" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </joint>
+    <gazebo reference="${name}_access_panels_link">
+      <material>Gazebo/DarkYellow</material>
+    </gazebo>
+
+    <!--
+        Mount points for sensors, other accessories
+    -->
+
+    <!-- center of the adjustable track on top -->
+    <link name="${name}_center_mount" />
+    <joint name="${name}_center_joint" type="fixed">
+      <parent link="${name}_enclosure_link" />
+      <child link="${name}_center_mount" />
+      <origin xyz="0 0 0.4074593" rpy="0 0 0" />
+    </joint>
+
+    <!-- standard rear-mounted antennas -->
+    <link name="${name}_antenna_mount" />
+    <joint name="${name}_antenna_mount_link" type="fixed">
+      <parent link="${name}_enclosure_link" />
+      <child link="${name}_antenna_mount" />
+      <origin xyz="-0.4028 0 0.4074593" rpy="0 0 0" />
+    </joint>
+
+    <!-- rear-mounted fixposition -->
+    <link name="${name}_ins_mount" />
+    <joint name="${name}_ins_mouint_link" type="fixed">
+      <parent link="${name}_enclosure_link" />
+      <child link="${name}_ins_mount" />
+      <origin xyz="-0.4 0.0 0.3192593" rpy="3.14159 0 3.14159" />
+    </joint>
+
+    <!-- front-mounted robin -->
+    <link name="${name}_front_lidar_mount" />
+    <joint name="${name}_front_lidar_mount_link" type="fixed">
+      <parent link="${name}_enclosure_link" />
+      <child link="${name}_front_lidar_mount" />
+      <origin xyz="0.40 0 0.2708593" rpy="0 0 0" />
+    </joint>
+  </xacro:macro>
+</robot>

clearpath_platform_description/urdf/a300/attachments/spotlight.urdf.xacro

@@ -0,0 +1,74 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://wiki.ros.org.xacro">
+    <xacro:macro name="spotlight" params="name parent_link:=default_mount model:=spotlight *origin">
+        <link name="${name}_link">
+            <visual>
+                <geometry>
+                    <box size="0.05 0.05 0.05" />
+                </geometry>
+                <origin xyz="0 0 0.025" rpy="0 0 0" />
+                <material name="black">
+                    <color rgba="0 0 0 1" />
+                </material>
+            </visual>
+            <collision>
+                <geometry>
+                    <box size="0.05 0.05 0.05" />
+                </geometry>
+                <origin xyz="0 0 0.025" rpy="0 0 0" />
+            </collision>
+        </link>
+        <joint name="${name}_joint" type="fixed">
+            <parent link="${parent_link}" />
+            <child link="${name}_link" />
+            <xacro:insert_block name="origin" />
+        </joint>
+
+        <link name="${name}_light_link">
+            <visual>
+                <geometry>
+                    <cylinder radius="0.01" length="0.0001"/>
+                </geometry>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <material name="white">
+                    <color rgba="1 1 1 1" />
+                </material>
+            </visual>
+        </link>
+        <joint name="${name}_light_joint" type="fixed">
+            <parent link="${name}_link" />
+            <child link="${name}_light_link" />
+            <origin xyz="0.025 0 0.025" rpy="0 -1.5707963267948966 0" />
+        </joint>
+
+        <gazebo reference="${name}_light_link">
+            <light type="spot" name="${name}_light" relative_to="${name}_light_link">
+                <pose>0 0 0 0 0 0</pose>
+                <diffuse>1 1 1 0.5</diffuse>
+                <specular>.1 .1 .1 1</specular>
+                <attenuation>
+                    <range>20</range>
+                    <constant>0.8</constant>
+                    <linear>0.75</linear>
+                    <quadratic>0.01</quadratic>
+                </attenuation>
+                <direction>0 0 0</direction>
+                <spot>
+                    <inner_angle>1.0471975511965976</inner_angle> <!-- 60 degrees -->
+                    <outer_angle>2.0943951023931953</outer_angle> <!-- 120 degrees -->
+                    <falloff>1.0</falloff>
+                </spot>
+                <cast_shadows>true</cast_shadows>
+                <visualize>false</visualize>
+            </light>
+            <plugin name='${name}_attach_light' filename='libAttachLightPlugin.so'>
+                <link_name>${name}_light_link</link_name>
+                <light>
+                <pose>0 0 0 0 0 0</pose>
+                <light_name>${name}_light</light_name>
+                </light>
+            </plugin>
+            <selfCollide>false</selfCollide>
+        </gazebo>
+    </xacro:macro>
+</robot>

clearpath_sensors_description/urdf/fixposition.urdf.xacro

@@ -79,6 +79,14 @@
                 rpy="${gps_0_rpy_r} ${gps_0_rpy_p} ${gps_0_rpy_y}"
             />
         </xacro:gnss_antenna>
+        <gazebo reference="${name}_gps_0_link">
+            <sensor name="${name}_gps_0" type="navsat">
+                <always_on>1</always_on>
+                <update_rate>1</update_rate>
+                <gz_frame_id>${name}_gps_0_link</gz_frame_id>
+                <topic>$(arg namespace)/sensors/${name}/gps_0/fix</topic>
+            </sensor>
+        </gazebo>
 
         <xacro:if value="${num_antennas > 1}">
             <xacro:gnss_antenna
@@ -90,6 +98,14 @@
                     rpy="${gps_1_rpy_r} ${gps_1_rpy_p} ${gps_1_rpy_y}"
                 />
             </xacro:gnss_antenna>
+            <gazebo reference="${name}_gps_1_link">
+                <sensor name="${name}_gps_1" type="navsat">
+                    <always_on>1</always_on>
+                    <update_rate>1</update_rate>
+                    <gz_frame_id>${name}_gps_1_link</gz_frame_id>
+                    <topic>$(arg namespace)/sensors/${name}/gps_1/fix</topic>
+                </sensor>
+            </gazebo>
         </xacro:if>
 
     </xacro:macro>