Bishop sensors/mounts

LICENSE

@@ -0,0 +1,28 @@
+BSD 3-Clause License
+
+Copyright (c) 2023, clearpathrobotics
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

clearpath_common/package.xml

@@ -5,6 +5,7 @@
   <version>0.0.0</version>
   <description>Clearpath Common Metapackage</description>
 
+  <maintainer email="[email protected]">Luis Camero</maintainer>
   <maintainer email="[email protected]">Roni Kreinin</maintainer>
   <maintainer email="[email protected]">Tony Baltovski</maintainer>
 

clearpath_control/config/a200/localization.yaml

@@ -8,8 +8,7 @@ ekf_node:
 
     frequency: 50.0
 
-    # odom0: platform_velocity_controller/odom
-    odom0: odom
+    odom0: platform_velocity_controller/odom
     odom0_config: [false, false, false,
                   false, false, false,
                   true, true, true,

clearpath_control/config/j100/localization.yaml

@@ -9,7 +9,6 @@ ekf_node:
     frequency: 50.0
 
     odom0: platform_velocity_controller/odom
-    #odom0: odom
     odom0_config: [true, true, false,
                   false, false, true,
                   true, true, false,

clearpath_control/package.xml

@@ -12,8 +12,9 @@
   <author email="[email protected]">Tony Baltovski</author>
   <author email="[email protected]">Roni Kreinin</author>
 
-  <maintainer email="tbaltovski@clearpathrobotics.com">Tony Baltovski</maintainer>
+  <maintainer email="lcamero@clearpathrobotics.com">Luis Camero</maintainer>
   <maintainer email="[email protected]">Roni Kreinin</maintainer>
+  <maintainer email="[email protected]">Tony Baltovski</maintainer>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

clearpath_description_generator/clearpath_description_generator/description/decorations.py

@@ -81,9 +81,21 @@ def __init__(self, platform: str, decoration: TopPlate) -> None:
                 self.MODEL: decoration.get_model(),
             })
 
+    class SensorArchDescription(BaseDescription):
+        SIZE = 'size'
+        PARENT_LINK = 'parent_link'
+
+        def __init__(self, platform: str, decoration: BaseDecoration) -> None:
+            super().__init__(platform, decoration)
+            self.parameters.update({
+                self.PARENT_LINK: 'top_plate_link',
+                self.SIZE: '510'
+            })
+
     MODEL = {
         Bumper.DECORATION_MODEL: BumperDescription,
         TopPlate.DECORATION_MODEL: TopPlateDescription,
+        'sensor_arch': SensorArchDescription
     }
 
     def __new__(cls, platform, decoration: BaseDecoration) -> BaseDescription:

clearpath_description_generator/clearpath_description_generator/description/sensors.py

@@ -32,8 +32,11 @@
 
 from clearpath_config.sensors.base import BaseSensor
 from clearpath_config.sensors.lidars_2d import HokuyoUST10, SickLMS1XX
+from clearpath_config.sensors.lidars_3d import VelodyneLidar
 from clearpath_config.sensors.cameras import IntelRealsense
 from clearpath_config.sensors.imu import Microstrain
+from clearpath_config.sensors.gps import SwiftNavDuro
+
 
 from typing import List
 
@@ -81,27 +84,13 @@ def get_xyz(self) -> List[float]:
         def get_rpy(self) -> List[float]:
             return self.sensor.get_rpy()
 
-    class HokuyoUST10Description(BaseDescription):
-        def __init__(self, sensor: HokuyoUST10) -> None:
-            super().__init__(sensor)
-
-    class SickLMS1XXDescription(BaseDescription):
-        def __init__(self, sensor: SickLMS1XX) -> None:
-            super().__init__(sensor)
-
-    class IntelRealsenseDescription(BaseDescription):
-        def __init__(self, sensor: IntelRealsense) -> None:
-            super().__init__(sensor)
-
-    class MicrostrainIMUDescription(BaseDescription):
-        def __init__(self, sensor: Microstrain) -> None:
-            super().__init__(sensor)
-
     MODEL = {
-        HokuyoUST10.SENSOR_MODEL: HokuyoUST10Description,
-        SickLMS1XX.SENSOR_MODEL: SickLMS1XXDescription,
-        IntelRealsense.SENSOR_MODEL: IntelRealsenseDescription,
-        Microstrain.SENSOR_MODEL: MicrostrainIMUDescription
+        HokuyoUST10.SENSOR_MODEL: BaseDescription,
+        SickLMS1XX.SENSOR_MODEL: BaseDescription,
+        IntelRealsense.SENSOR_MODEL: BaseDescription,
+        Microstrain.SENSOR_MODEL: BaseDescription,
+        VelodyneLidar.SENSOR_MODEL: BaseDescription,
+        SwiftNavDuro.SENSOR_MODEL: BaseDescription
     }
 
     def __new__(cls, sensor: BaseSensor) -> BaseDescription:

clearpath_description_generator/clearpath_description_generator/description_generator

@@ -70,8 +70,7 @@ class DescriptionGenerator():
         self.xacro_writer.write_include(
             package=self.pkg_clearpath_platform_description,
             file='common',
-            path='urdf/'
-        )
+            path='urdf/')
 
         # Platform
         self.generate_platform()
@@ -109,16 +108,32 @@ class DescriptionGenerator():
         decorations = self.clearpath_config.platform.decorations.get_all_decorations()
         for decoration in decorations:
             if decoration.get_enabled():
-                bumper_description = DecorationsDescription(self.platform, decoration)
+                decoration_description = DecorationsDescription(self.platform, decoration)
 
                 self.xacro_writer.write_include(
-                    package=bumper_description.get_package(),
-                    file=bumper_description.get_file(),
-                    path=bumper_description.get_path())
+                    package=decoration_description.get_package(),
+                    file=decoration_description.get_file(),
+                    path=decoration_description.get_path())
 
                 self.xacro_writer.write_macro(
-                    macro=bumper_description.get_file(),
-                    parameters=bumper_description.get_parameters())
+                    macro=decoration_description.get_file(),
+                    parameters=decoration_description.get_parameters())
+
+        self.xacro_writer.write_include(
+            package='clearpath_platform_description',
+            file='sensor_arch',
+            path='urdf/a200/decorations/')
+
+        self.xacro_writer.write_macro(
+            macro='sensor_arch',
+            parameters={
+              'parent_link': 'top_plate_link',
+              'size': '510'
+            },
+            blocks=XacroWriter.add_origin(
+              [0.0, 0.0, 0.0],
+              [0.0, 0.0, 0.0])
+            )
 
     def generate_mounts(self) -> None:
         self.xacro_writer.write_comment('Mounts')

clearpath_description_generator/package.xml

@@ -4,7 +4,12 @@
   <name>clearpath_description_generator</name>
   <version>0.0.0</version>
   <description>Clearpath robot description generator</description>
-  <maintainer email="[email protected]">rkreinin</maintainer>
+
+  <author email="[email protected]">Roni Kreinin</author>
+  <maintainer email="[email protected]">Luis Camero</maintainer>
+  <maintainer email="[email protected]">Roni Kreinin</maintainer>
+  <maintainer email="[email protected]">Tony Baltovski</maintainer>
+
   <license>BSD</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

clearpath_platform_description/urdf/a200/decorations/sensor_arch.urdf.xacro

@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="510_mm_sensor_arch">
+
+    <xacro:macro name="sensor_arch" params="parent_link size:=510 *origin">
+
+        <!-- Spawn the sensor arch link -->
+        <link name="sensor_arch_link">
+             <visual>
+                  <origin xyz="-0.0812 0 ${size / 1000}" rpy="0 0 0"/>
+                 <geometry>
+                     <mesh filename="package://clearpath_platform_description/meshes/a200/decorations/${size}_mm_sensor_arch.dae" />
+                 </geometry>
+             </visual>
+             <collision>
+                 <origin xyz="0 0 0" rpy="0 0 0"/>
+                 <geometry>
+                     <mesh filename="package://clearpath_platform_description/meshes/a200/decorations/${size}_mm_sensor_arch.dae" />
+                 </geometry>
+             </collision>
+        </link>
+
+        <link name="sensor_arch_mount"/>
+
+        <!-- Attach the sensor arch to the top plate -->
+        <joint name="sensor_arch_joint" type="fixed">
+             <xacro:insert_block name="origin"/>
+             <parent link="${parent_link}"/>
+             <child link="sensor_arch_link"/>
+        </joint>
+
+        <joint name="sensor_arch_mount_joint" type="fixed">
+             <origin xyz="0 0 ${size / 1000}" rpy="0 0 0"/>
+             <parent link="sensor_arch_link"/>
+             <child link="sensor_arch_mount"/>
+        </joint>
+
+    </xacro:macro>
+
+</robot>

clearpath_platform_description/urdf/a200/decorations/top_plate.urdf.xacro

@@ -3,7 +3,7 @@
     <!-- <xacro:include filename="$(find clearpath_platform_description)/urdf/common.urdf.xacro" /> -->
     <xacro:macro name="top_plate" params="model:=default">
         <!-- Spawn the top plate -->
-      <xacro:if value="${model == 'default'}">
+      <xacro:if value="${model == 'large'}">
         <link name="top_plate_link">
           <visual>
             <origin xyz="0 0 0" rpy="0 0 0" />
@@ -40,7 +40,7 @@
         <link name="top_plate_rear_link"/>
       </xacro:if>
 
-      <xacro:if value="${model == 'large'}">
+      <xacro:if value="${model == 'default'}">
         <link name="top_plate_link">
           <visual>
             <origin xyz="0 0 0" rpy="0 0 0" />

clearpath_platform_description/urdf/common.urdf.xacro

@@ -32,6 +32,14 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
     <material name="black"><color rgba="0.15 0.15 0.15 1.0" /></material>
     <material name="white"><color rgba="1.0 1.0 1.0 1.0" /></material>
     <material name="red"><color rgba="1.0 0.0 0.0 1.0" /></material>
+
+    <material name="aluminum">
+      <color rgba="0.5 0.5 0.5 1"/>
+    </material>
+    <material name="plastic">
+      <color rgba="0.1 0.1 0.1 1"/>
+    </material>
+
     <!-- 
         Standard Mount: adds link (w/o geometry) at given position
         - assumes orientation of parent