Improve nanosaur description and configurations

rnanosaur · Oct 9, 2024 · 49cb0bd · 49cb0bd
1 parent 686ae3e
commit 49cb0bd
Show file tree

Hide file tree

Showing 12 changed files with 193 additions and 37 deletions.
diff --git a/nanosaur_description/launch/description.launch.py b/nanosaur_description/launch/description.launch.py
@@ -41,7 +41,8 @@ def launch_setup(context: LaunchContext, support_package):
     namespace = context.perform_substitution(support_package)
 
     xacro_path = LaunchConfiguration('xacro_path')
-    cover_type = LaunchConfiguration('cover_type')
+    head_type = LaunchConfiguration('head_type')
+    flap_type = LaunchConfiguration('flap_type')
     diff_drive_emulation = LaunchConfiguration('diff_drive_emulation')
     use_nominal_extrinsics = LaunchConfiguration('use_nominal_extrinsics')
 
@@ -53,7 +54,8 @@ def launch_setup(context: LaunchContext, support_package):
             'robot_description': Command(
                 [
                     'xacro ', xacro_path, ' ',
-                    'cover_type:=', cover_type, ' ',
+                    'head_type:=', head_type, ' ',
+                    'flap_type:=', flap_type, ' ',
                     'diff_drive_emulation:=', diff_drive_emulation, ' ',
                     'use_nominal_extrinsics:=', use_nominal_extrinsics, ' ',
                 ])
@@ -83,10 +85,15 @@ def generate_launch_description():
         default_value='nanosaur',
         description='nanosaur namespace name. If you are working with multiple robot you can change this namespace.')
 
-    declare_cover_type_cmd = DeclareLaunchArgument(
-        name='cover_type',
-        default_value='fisheye',
-        description='Cover type to use. Options: pi, fisheye, realsense, zed.')
+    declare_head_type_cmd = DeclareLaunchArgument(
+        name='head_type',
+        default_value='realsense',
+        description='Head type to use. Options: empty, Realsense, zed.')
+
+    declare_flap_type_cmd = DeclareLaunchArgument(
+        name='flap_type',
+        default_value='empty',
+        description='Flap type to use. Options: empty, LD06.')
 
     declare_simulation_cmd = DeclareLaunchArgument(
         name='diff_drive_emulation',
@@ -103,7 +110,8 @@ def generate_launch_description():
     ld.add_action(nanosaur_cmd)
     ld.add_action(declare_model_path_cmd)
     ld.add_action(declare_simulation_cmd)
-    ld.add_action(declare_cover_type_cmd)
+    ld.add_action(declare_head_type_cmd)
+    ld.add_action(declare_flap_type_cmd)
     ld.add_action(declare_use_nominal_extrinsics_cmd)
     ld.add_action(OpaqueFunction(function=launch_setup, args=[namespace]))
 

diff --git a/nanosaur_description/launch/display.launch.py b/nanosaur_description/launch/display.launch.py
@@ -39,16 +39,22 @@ def generate_launch_description():
     nanosaur_description_path = get_package_share_directory('nanosaur_description')
 
     gui = LaunchConfiguration('gui')
-    cover_type = LaunchConfiguration('cover_type')
+    head_type = LaunchConfiguration('head_type')
+    flap_type = LaunchConfiguration('flap_type')
     diff_drive_emulation = LaunchConfiguration('diff_drive_emulation')
     rvizconfig = LaunchConfiguration('rvizconfig')
 
     default_rviz_config_path = os.path.join(nanosaur_description_path, 'rviz', 'urdf.rviz')
 
-    declare_cover_type_cmd = DeclareLaunchArgument(
-        name='cover_type',
-        default_value='fisheye',
-        description='Cover type to use. Options: pi, fisheye, realsense, zed.')
+    declare_head_type_cmd = DeclareLaunchArgument(
+        name='head_type',
+        default_value='realsense',
+        description='Head type to use. Options: empty, Realsense, zed.')
+
+    declare_flap_type_cmd = DeclareLaunchArgument(
+        name='flap_type',
+        default_value='empty',
+        description='Flap type to use. Options: empty, LD06.')
 
     declare_simulation_cmd = DeclareLaunchArgument(
         name='diff_drive_emulation',
@@ -91,13 +97,14 @@ def generate_launch_description():
     # https://answers.ros.org/question/306935/ros2-include-a-launch-file-from-a-launch-file/
     description_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([nanosaur_description_path, '/launch/description.launch.py']),
-        launch_arguments={'cover_type': cover_type, 'diff_drive_emulation': diff_drive_emulation}.items()
+        launch_arguments={'head_type': head_type, 'flap_type': flap_type, 'diff_drive_emulation': diff_drive_emulation}.items()
         )
 
     # Define LaunchDescription variable and return it
     ld = LaunchDescription()
 
-    ld.add_action(declare_cover_type_cmd)
+    ld.add_action(declare_head_type_cmd)
+    ld.add_action(declare_flap_type_cmd)
     ld.add_action(declare_simulation_cmd)
     ld.add_action(declare_gui_cmd)
     ld.add_action(declare_rvizconfig_cmd)

diff --git a/nanosaur_description/meshes/nanosaur_flap_LD06.stl b/nanosaur_description/meshes/nanosaur_flap_LD06.stl
diff --git a/nanosaur_description/meshes/nanosaur_head.stl b/nanosaur_description/meshes/nanosaur_head.stl
diff --git a/nanosaur_description/urdf/flap.urdf.xacro → ...aur_description/urdf/flap/flap.urdf.xacro b/nanosaur_description/urdf/flap.urdf.xacro → ...aur_description/urdf/flap/flap.urdf.xacro
@@ -2,22 +2,15 @@
 <robot name="nanosaur_flap"
     xmlns:xacro="http://ros.org/wiki/xacro">
 
-    <xacro:macro name="flap">
+    <xacro:macro name="nanosaur_flap" params="parent">
 
         <joint name="nanosaur_flap_joint" type="fixed">
             <origin xyz="0 0 0" rpy="0 0 0" />
-            <parent link="nanosaur_base"/>
+            <parent link="${parent}"/>
             <child link="nanosaur_flap"/>
         </joint>
 
         <link name="nanosaur_flap">
-            <inertial>
-                <mass value="0.018"/>
-                <origin rpy="0 0 0" xyz="-0.063 0 0.065"/>
-                <inertia ixx="7565.043" ixy="-0.00384" ixz="300.2704"
-                        iyy="2254.45646" iyz="0.00019"
-                        izz="9538.576"/>
-            </inertial>
             <visual>
                 <geometry><mesh filename="${path_meshes}/nanosaur_flap.stl" scale="0.001 0.001 0.001"/></geometry>
                 <material name="Green" />

diff --git a/nanosaur_description/urdf/flap/flap_LD06.urdf.xacro b/nanosaur_description/urdf/flap/flap_LD06.urdf.xacro
@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<robot name="nanosaur_flap_LD06"
+    xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:macro name="nanosaur_flap_LD06" params="parent">
+
+        <joint name="nanosaur_flap_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <parent link="${parent}"/>
+            <child link="nanosaur_flap"/>
+        </joint>
+
+        <link name="nanosaur_flap">
+            <visual>
+                <geometry><mesh filename="${path_meshes}/nanosaur_flap_LD06.stl" scale="0.001 0.001 0.001"/></geometry>
+                <material name="Green" />
+            </visual>
+        </link>
+
+        <joint name="LD06_joint" type="fixed">
+            <origin xyz="-0.057 0 0.065" rpy="0 0 -${M_PI/2}" />
+            <parent link="${parent}"/>
+            <child link="LD06"/>
+        </joint>
+
+        <link name="LD06">
+            <visual>
+                <geometry><mesh filename="${path_meshes}/parts/LD06.stl" /></geometry>
+                <material name="Black" />
+            </visual>
+            <collision>
+                <origin xyz="0 0 0.017" rpy="0 0 0" />
+                <geometry>
+                    <cylinder length="0.036" radius="0.02"/>
+                </geometry>
+                <material name="Black" />
+            </collision>
+        </link>
+
+    </xacro:macro>
+
+</robot>
diff --git a/nanosaur_description/urdf/head/d435.urdf.xacro b/nanosaur_description/urdf/head/d435.urdf.xacro
@@ -72,7 +72,7 @@ aluminum peripherial evaluation case.
         </xacro:unless>
       </visual>
       <collision>
-        <origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
+        <origin xyz="-0.008 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
         <geometry>
           <box size="${d435_cam_depth} ${d435_cam_width} ${d435_cam_height}"/>
         </geometry>

diff --git a/nanosaur_description/urdf/head/head.urdf.xacro b/nanosaur_description/urdf/head/head.urdf.xacro
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot name="nanosaur_head"
+    xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find nanosaur_description)/urdf/head/d435.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/head/d435i_imu_modules.urdf.xacro"/>
+
+    <xacro:macro name="nanosaur_head" params="parent">
+
+        <gazebo reference="head">
+            <material>Gazebo/Green</material>
+            <!-- Ignition color reference -->
+            <visual>
+                <material>
+                    <ambient>0.0 0.5 0.0 1.0</ambient>
+                    <diffuse>0.0 0.5 0.0 1.0</diffuse>
+                    <specular>0.0 0.5 0.0 1.0</specular>
+                </material>
+            </visual>
+        </gazebo>
+
+        <joint name="head_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <parent link="${parent}"/>
+            <child link="head"/>
+        </joint>
+
+        <link name="head">
+            <visual>
+                <geometry>
+                    <mesh filename="${path_meshes}/nanosaur_head.stl" scale="0.001 0.001 0.001"/>
+                </geometry>
+                <material name="Green" />
+            </visual>
+        </link>
+
+    </xacro:macro>
+</robot>
diff --git a/nanosaur_description/urdf/head/head_realsense.urdf.xacro b/nanosaur_description/urdf/head/head_realsense.urdf.xacro
@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<robot name="nanosaur_head_realsense"
+    xmlns:xacro="http://ros.org/wiki/xacro">
+
+    <xacro:include filename="$(find nanosaur_description)/urdf/head/d435.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/head/d435i_imu_modules.urdf.xacro"/>
+
+    <xacro:macro name="nanosaur_head_realsense" params="parent name:=camera use_nominal_extrinsics:=false">
+
+        <gazebo reference="head">
+            <material>Gazebo/Green</material>
+            <!-- Ignition color reference -->
+            <visual>
+                <material>
+                    <ambient>0.0 0.5 0.0 1.0</ambient>
+                    <diffuse>0.0 0.5 0.0 1.0</diffuse>
+                    <specular>0.0 0.5 0.0 1.0</specular>
+                </material>
+            </visual>
+        </gazebo>
+
+        <joint name="head_joint" type="fixed">
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <parent link="${parent}"/>
+            <child link="head"/>
+        </joint>
+
+        <link name="head">
+            <visual>
+                <geometry>
+                    <mesh filename="${path_meshes}/nanosaur_head_realsense_d435.stl" scale="0.001 0.001 0.001"/>
+                </geometry>
+                <material name="Green" />
+            </visual>
+        </link>
+
+        <xacro:sensor_d435 parent="${parent}" name="${name}" use_nominal_extrinsics="${use_nominal_extrinsics}">
+            <origin xyz="0.026 0 0.058" rpy="0 0 0" />
+        </xacro:sensor_d435>
+        <xacro:d435i_imu_modules name="${name}" use_nominal_extrinsics="${use_nominal_extrinsics}"/>
+
+    </xacro:macro>
+</robot>
diff --git a/nanosaur_description/urdf/nanosaur.urdf.xml b/nanosaur_description/urdf/nanosaur.urdf.xml
@@ -8,7 +8,8 @@
          Add extra wheel and center sprocket for differential drive simulation -->
     <xacro:arg name="diff_drive_emulation" default="false" />
     <!-- camera definitions -->
-    <xacro:arg name="cover_type" default="fisheye" /> <!--options: pi, fisheye, realsense, zedmini -->
+    <xacro:arg name="head_type" default="empty" /> <!--options: empty, realsense, zedmini -->
+    <xacro:arg name="flap_type" default="empty" /> <!--options: empty, LD06 -->
     <xacro:arg name="use_nominal_extrinsics" default="false" /> <!-- ONLY FOR REALSENSE -->
 
      <!-- nanosaur definition -->
@@ -21,7 +22,8 @@
      </joint>
 
     <!-- nanosaur robot -->
-    <xacro:nanosaur cover_type="$(arg cover_type)"
+    <xacro:nanosaur head_type="$(arg head_type)"
+                    flap_type="$(arg flap_type)"
                     use_nominal_extrinsics="$(arg use_nominal_extrinsics)"
                     diff_drive_emulation="$(arg diff_drive_emulation)" />
 

diff --git a/nanosaur_description/urdf/omniwheel.urdf.xacro b/nanosaur_description/urdf/omniwheel.urdf.xacro
@@ -61,7 +61,7 @@
             </visual>
             <!-- Collision cylinder -->
             <collision>
-                <origin xyz="0 0 0" rpy="0 0 0" />
+                <origin xyz="0 0 ${wheel_length / 2}" rpy="0 0 0" />
                 <geometry>
                     <cylinder length="${wheel_length}" radius="${wheel_diameter / 2}"/>
                 </geometry>

diff --git a/nanosaur_description/urdf/robot.urdf.xacro b/nanosaur_description/urdf/robot.urdf.xacro
@@ -5,17 +5,19 @@
     <xacro:include filename="$(find nanosaur_description)/urdf/materials.urdf" />
     <xacro:include filename="$(find nanosaur_description)/urdf/materials.gazebo.xacro" />
     <!-- nanosaur parts -->
-    <xacro:include filename="$(find nanosaur_description)/urdf/flap.urdf.xacro" />
     <xacro:include filename="$(find nanosaur_description)/urdf/omniwheel.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/flap/flap.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/flap/flap_LD06.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/head/head.urdf.xacro" />
+    <xacro:include filename="$(find nanosaur_description)/urdf/head/head_realsense.urdf.xacro" />
     <!-- Global properties -->
     <xacro:property name="M_PI" value="3.14159"/>
     <!--https://github.com/ros-simulation/gazebo_ros_pkgs/pull/1284 -->
     <!--I cannot use the solution below for remote machines-->
     <!--<xacro:property name="path_meshes" value="file://$(find nanosaur_description)/meshes"/>-->
     <!--<xacro:property name="path_meshes" value="package://nanosaur_description/meshes"/>-->
-    <!-- nanosaur properties -->
 
-    <xacro:macro name="nanosaur" params="cover_type use_nominal_extrinsics diff_drive_emulation mimic_enable:=true path_meshes:=package://nanosaur_description/meshes">
+    <xacro:macro name="nanosaur" params="head_type flap_type use_nominal_extrinsics diff_drive_emulation mimic_enable:=true path_meshes:=package://nanosaur_description/meshes">
 
         <link name="nanosaur_base">
             <inertial>
@@ -29,6 +31,13 @@
                 <geometry><mesh filename="${path_meshes}/nanosaur_base.stl" scale="0.001 0.001 0.001"/></geometry>
                 <material name="Green" />
             </visual>
+            <collision>
+                <origin xyz="0.01 0 0.005" rpy="0 0 0"/>
+                <geometry>
+                    <box size="0.17 0.075 0.09"/>
+                </geometry>
+                <material name="Green" />
+            </collision>
         </link>
 
         <joint name="nanosaur_cover_batteries_joint" type="fixed">
@@ -57,13 +66,6 @@
             </visual>
         </link>
 
-        <xacro:omniwheel parent="nanosaur_base" type="left" position="front" xyz="0.05 0.041 -0.035"/>
-        <xacro:omniwheel parent="nanosaur_base" type="left" position="rear" xyz="-0.05 0.041 -0.035"/>
-        <xacro:omniwheel parent="nanosaur_base" type="right" position="front" xyz="0.05 -0.041 -0.035"/>
-        <xacro:omniwheel parent="nanosaur_base" type="right" position="rear" xyz="-0.05 -0.041 -0.035"/>
-
-        <xacro:flap />
-
         <joint name="NVIDIA_jetson_joint" type="fixed">
             <origin xyz="0 0 0" rpy="0 0 0" />
             <parent link="nanosaur_base"/>
@@ -162,6 +164,27 @@
             </visual>
         </link>
 
+        <xacro:omniwheel parent="nanosaur_base" type="left" position="front" xyz="0.05 0.041 -0.035"/>
+        <xacro:omniwheel parent="nanosaur_base" type="left" position="rear" xyz="-0.05 0.041 -0.035"/>
+        <xacro:omniwheel parent="nanosaur_base" type="right" position="front" xyz="0.05 -0.041 -0.035"/>
+        <xacro:omniwheel parent="nanosaur_base" type="right" position="rear" xyz="-0.05 -0.041 -0.035"/>
+
+        <!-- Nanosaur heads -->
+        <xacro:if value="${head_type == 'empty'}">
+            <xacro:nanosaur_head parent="nanosaur_base" />
+        </xacro:if>
+        <xacro:if value="${head_type == 'realsense'}">
+            <xacro:nanosaur_head_realsense parent="nanosaur_base" name="camera" use_nominal_extrinsics="${use_nominal_extrinsics}" />
+        </xacro:if>
+
+        <!-- Nanosaur flap -->
+        <xacro:if value="${flap_type == 'empty'}">
+            <xacro:nanosaur_flap parent="nanosaur_base" />
+        </xacro:if>
+        <xacro:if value="${flap_type == 'LD06'}">
+            <xacro:nanosaur_flap_LD06 parent="nanosaur_base" />
+        </xacro:if>
+
     </xacro:macro>
 
 </robot>