robot description changes for vision

.gitmodules

@@ -13,3 +13,9 @@
 [submodule "hardware_drivers/teleop_tools"]
 	path = hardware_drivers/teleop_tools
 	url = https://github.com/f1tenth/teleop_tools.git
+[submodule "hardware_drivers/realsense-ros"]
+	path = hardware_drivers/realsense-ros
+	url = https://github.com/rl16432/realsense-ros.git
+[submodule "hardware_drivers/zed-ros2-wrapper"]
+	path = hardware_drivers/zed-ros2-wrapper
+	url = https://github.com/rl16432/zed-ros2-wrapper.git

f1tenth_description/gazebo/pose.gazebo.xacro

@@ -4,7 +4,10 @@
         <plugin
             filename="gz-sim-pose-publisher-system"
             name="gz::sim::systems::PosePublisher">
-
+            <publish_sensor_pose>true</publish_sensor_pose>
+            <use_pose_vector_msg>true</use_pose_vector_msg>
+            <publish_model_pose>true</publish_model_pose>
+            <publish_nested_model_pose>true</publish_nested_model_pose>
         </plugin>
     </gazebo>
 </robot>

f1tenth_description/sdf/aruco_marker/model.config

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>ArUco Marker</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>Nick Lamprianidis</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    <p>ArUco Marker</p>
+  </description>
+</model>

f1tenth_description/sdf/aruco_marker/model.urdf.xacro

@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:macro name="aruco_marker" params="xyz rpy">
+    <gazebo>
+      <link name="aruco_link">
+        <pose>${xyz} ${rpy}</pose>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <mass>0.001</mass>
+          <inertia>
+            <ixx>3.7499999999999997e-06</ixx>
+            <ixy>0.0</ixy>
+            <ixz>0.0</ixz>
+            <iyy>1.8750008333333333e-06</iyy>
+            <iyz>0.0</iyz>
+            <izz>1.8750008333333333e-06</izz>
+          </inertia>
+        </inertial>
+        <visual name="front_visual">
+          <pose>-0.00005 0 0 0 0 3.14</pose>
+          <geometry>
+            <box>
+              <size>0.0001 0.15 0.15</size>
+            </box>
+          </geometry>
+          <material>
+            <diffuse>1 1 1 1</diffuse>
+            <specular>0.4 0.4 0.4 1</specular>
+            <ambient>1 1 1 1</ambient>
+            <!-- <emissive>0.1 0.1 0.1 1</emissive> -->
+            <pbr>
+              <metal>
+                <albedo_map>$(find f1tenth_description)/sdf/aruco_marker/materials/textures/marker.png</albedo_map>
+                <!-- <roughness_map>$(find f1tenth_description)/sdf/aruco_marker/materials/textures/marker_padded.png</roughness_map> -->
+                <!-- <metalness_map>$(find f1tenth_description)/sdf/aruco_marker/materials/textures/marker_padded.png</metalness_map> -->
+                <!-- <environment_map>$(find f1tenth_description)/sdf/aruco_marker/materials/textures/marker_padded.png</environment_map> -->
+                <!-- <normal_map>$(find f1tenth_description)/sdf/aruco_marker/materials/textures/marker_padded.png</normal_map> -->
+                <!-- <ambient_occlusion_map>$(find f1tenth_description)/sdf/aruco_marker/materials/textures/marker_padded.png</ambient_occlusion_map> -->
+                <!-- <light_map>$(find f1tenth_description)/sdf/aruco_marker/materials/textures/marker_padded.png</light_map> -->
+                <!-- <emissive_map>$(find f1tenth_description)/sdf/aruco_marker/materials/textures/marker_padded.png</emissive_map> -->
+                <metalness>0</metalness>
+              </metal>
+            </pbr>
+          </material>
+        </visual>
+        <!-- Hide the marker from the back -->
+        <visual name="rear_visual">
+          <pose>0.00005 0 0 0 0 3.14</pose>
+          <geometry>
+            <box>
+              <size>0.0001 0.15 0.15</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name="collision">
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.0001 0.15 0.15</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </gazebo>
+  </xacro:macro>
+</robot>

f1tenth_description/setup.py

@@ -4,6 +4,17 @@
 
 package_name = 'f1tenth_description'
 
+def package_files(directory):
+    """
+    Package all data files recursively in a directory
+    """
+    paths = []
+    for (path, _, filenames) in os.walk(directory):
+        if len(filenames) > 0:
+            paths.append((os.path.join('share', package_name, path), 
+                          [os.path.join(path, filename) for filename in filenames]))
+    return paths
+
 setup(
     name=package_name,
     version='0.0.0',
@@ -19,8 +30,7 @@
         (os.path.join('share', package_name, 'gazebo'), glob('gazebo/*')),
         (os.path.join('share', package_name, 'worlds'), glob('worlds/*')),
         (os.path.join('share', package_name, 'config'), glob('config/*')),
-        (os.path.join('share', package_name, 'sdf'), glob('sdf/*')),
-
+        *package_files('sdf'),
     ],
     install_requires=['setuptools'],
     zip_safe=True,

f1tenth_description/urdf/const.xacro

@@ -4,7 +4,16 @@
     <!-- Global -->
     <xacro:arg name="robot_name" default="f1tenth"/>
     <xacro:property name="robot_name" value="$(arg robot_name)" />
-    <!-- <xacro:property name="params_path" value="$(arg params_path)"/> -->
+    <xacro:arg name="camera_name" default="d435"/>
+    <xacro:property name="camera_name" value="$(arg camera_name)" />
+    <xacro:arg name="stereo_camera_name" default="zed2"/>
+    <xacro:property name="stereo_camera_name" value="$(arg stereo_camera_name)" />
+    <xacro:arg name="add_camera" default="false"/>
+    <xacro:property name="add_camera" value="$(arg add_camera)" />
+    <xacro:arg name="add_aruco" default="false"/>
+    <xacro:property name="add_aruco" value="$(arg add_aruco)" />
+    <xacro:arg name="use_stereo" default="false"/>
+    <xacro:property name="use_stereo" value="$(arg use_stereo)" />
 
     <!-- Wheels -->
     <xacro:property name="wheel_diameter" value="0.11" />

f1tenth_description/urdf/d435.xacro

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- RealSense dependency -->
+  <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+  <xacro:if value="${add_camera}">
+    <xacro:sensor_d435 gazebo="true" robot_name="${robot_name}" 
+      name="${camera_name}" parent="base_link" use_nominal_extrinsics="true" 
+      enable_infra1="false" enable_infra2="false" enable_color="true" enable_depth="true" 
+      color_width="1920" color_height="1080" color_fps="30"
+      depth_width="1280" depth_height="720" depth_fps="30">
+      <origin xyz="0 0 0.3" rpy="0 0 0" />
+    </xacro:sensor_d435>
+  </xacro:if>
+</robot>

f1tenth_description/urdf/joints.xacro

@@ -8,8 +8,8 @@
 
         <axis xyz="0 0 1" />
         <limit lower="-0.6" upper="+0.6"
-               velocity="1.0" effort="25" />
-        <origin xyz="0.1575 0.1025 0.055" rpy="0 0 0"/>
+            velocity="1.0" effort="25" />
+        <origin xyz="0.1575 0.1025 0.055" rpy="0 0 0" />
     </joint>
 
     <joint name="front_right_wheel_steering_joint" type="revolute">
@@ -18,8 +18,8 @@
 
         <axis xyz="0 0 1" />
         <limit lower="-0.6" upper="+0.6"
-               velocity="1.0" effort="25" />
-        <origin xyz="0.1575 -0.1025 0.055" rpy="0 0 0"/>
+            velocity="1.0" effort="25" />
+        <origin xyz="0.1575 -0.1025 0.055" rpy="0 0 0" />
     </joint>
 
     <!-- Drive joints -->
@@ -43,39 +43,48 @@
         <parent link="base_link" />
 
         <axis xyz="0 1 0" />
-        <origin xyz="-0.1575 0.1025 0.055" rpy="0 0 0"/>
+        <origin xyz="-0.1575 0.1025 0.055" rpy="0 0 0" />
     </joint>
 
     <joint name="rear_right_wheel_joint" type="continuous">
         <child link="rear_right_wheel" />
         <parent link="base_link" />
 
         <axis xyz="0 1 0" />
-        <origin xyz="-0.1575 -0.1025 0.055" rpy="0 0 0"/>
+        <origin xyz="-0.1575 -0.1025 0.055" rpy="0 0 0" />
     </joint>
 
     <!-- Chassis Joints-->
     <joint name="base_link_to_box" type="fixed">
         <child link="box_visual" />
         <parent link="base_link" />
 
-        <origin xyz="-0.1554 0.09 0.2" rpy="0 0 0"/>
+        <origin xyz="-0.1554 0.09 0.2" rpy="0 0 0" />
     </joint>
 
     <joint name="base_link_to_lower_chassis" type="fixed">
         <child link="lower_chassis" />
         <parent link="base_link" />
 
-        <origin xyz="-0.155 0.09 0.055" rpy="0 0 0"/>
+        <origin xyz="-0.155 0.09 0.055" rpy="0 0 0" />
     </joint>
 
     <!-- Sensor Joints -->
     <joint name="hokuyo_10lx_lidar_joint" type="fixed">
         <child link="hokuyo_10lx_lidar_link" />
         <parent link="base_link" />
 
-        <origin xyz="0.1 0 0.15" rpy="0 0 0"/>
+        <origin xyz="0.1 0 0.15" rpy="0 0 0" />
     </joint>
 
-
+    <!-- Add AruCo marker  -->
+    <xacro:if value="${add_aruco}">
+        <gazebo>
+            <joint name="aruco_marker_joint" type="fixed">
+                <pose relative_to="box_visual">0 0 0 0 0 0</pose> <!-- This doesn't do anything -->
+                <child>aruco_link</child>
+                <parent>box_visual</parent>
+            </joint>
+        </gazebo>
+    </xacro:if>
 </robot>

f1tenth_description/urdf/links.xacro

@@ -45,11 +45,28 @@
         <origin xyz="0 -0.0915 0" rpy="0 0 0"/>
     </xacro:box_link_nomesh >
 
-    <!-- Sensor links -->
+    <!-- Give box visual white color -->
+    <gazebo reference="box_visual">
+        <visual>
+            <material>
+                <emissive>1 1 1 1</emissive>
+                <diffuse>1 1 1 1</diffuse>
+                <specular>1 1 1 1</specular>
+                <ambient>1 1 1 1</ambient>
+            </material>
+        </visual>
+    </gazebo>
 
+    <!-- Sensor links -->
     <xacro:cylinder_link_nomesh name="hokuyo_10lx_lidar_link"
                     radius="0.05" width="0.08" mass="0.05">
       <origin xyz="0 0 0" rpy="0 0 0"/>
     </xacro:cylinder_link_nomesh>
 
+    <!-- AruCo marker -->
+    <xacro:if value="${add_aruco}">
+        <xacro:include filename="$(find f1tenth_description)/sdf/aruco_marker/model.urdf.xacro" />
+        <xacro:aruco_marker xyz="-0.1705 0 0.183" rpy="0 0 0"/>
+    </xacro:if>
+
 </robot>

f1tenth_description/urdf/robot.urdf.xacro

@@ -9,7 +9,13 @@
     <!-- links and joints -->
     <xacro:include filename="$(find f1tenth_description)/urdf/links.xacro" />
     <xacro:include filename="$(find f1tenth_description)/urdf/joints.xacro" />
-
+    <xacro:if value="$(arg use_stereo)">
+        <xacro:include filename="$(find f1tenth_description)/urdf/zed2.xacro" />
+    </xacro:if>
+    <xacro:unless value="$(arg use_stereo)">
+        <xacro:include filename="$(find f1tenth_description)/urdf/d435.xacro" />
+    </xacro:unless>
+
     <!-- Gazebo stuff -->
     <xacro:include filename="$(find f1tenth_description)/gazebo/ackermann.gazebo.xacro" />
     <xacro:include filename="$(find f1tenth_description)/gazebo/odometry.gazebo.xacro" />

f1tenth_description/urdf/zed2.xacro

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <!-- RealSense dependency -->
+  <xacro:include filename="$(find zed_wrapper)/urdf/zed_macro.urdf.xacro" />
+  <xacro:if value="$(arg add_camera)">
+    <xacro:zed_camera parent="base_link"
+      gazebo="true" robot_name="$(arg robot_name)"
+      image_width="1280" image_height="720" fps="30"
+      name="$(arg stereo_camera_name)" model="zed2"
+      custom_baseline="false" enable_gnss="false">
+      <origin xyz="0 0 0.4" rpy="0 0 0" />
+    </xacro:zed_camera>
+  </xacro:if>
+</robot>