Add simulation support for RGB and stereo cameras (#78)

luxonis · Sep 30, 2024 · 52b73b2 · 52b73b2
1 parent 79e017e
commit 52b73b2
Show file tree

Hide file tree

Showing 6 changed files with 156 additions and 75 deletions.
diff --git a/rae_description/launch/rsp.launch.py b/rae_description/launch/rsp.launch.py
@@ -11,6 +11,7 @@
 
 def launch_setup(context, *args, **kwargs):
     use_sim_time = LaunchConfiguration('use_sim_time')
+    ns = LaunchConfiguration('namespace').perform(context)
     pkg_path = os.path.join(get_package_share_directory('rae_description'))
     xacro_path = os.path.join(pkg_path, 'urdf', 'rae.urdf.xacro')
 
@@ -21,8 +22,10 @@ def launch_setup(context, *args, **kwargs):
             name='robot_state_publisher',
             namespace=LaunchConfiguration('namespace'),
             parameters=[{
-                'robot_description': Command(['xacro ', xacro_path, ' sim_mode:=', use_sim_time]),
-                'use_sim_time': use_sim_time
+                'robot_description': Command(['xacro ', xacro_path,
+                                              ' sim_mode:=', use_sim_time,
+                                              ' namespace:=', ns]),
+                'use_sim_time:=': use_sim_time,
             }]
         )
     ]

diff --git a/rae_description/urdf/camera.urdf.xacro b/rae_description/urdf/camera.urdf.xacro
@@ -1,57 +1,94 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot">
-
-    <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
-        <render_engine>ogre2</render_engine>
-    </plugin>
-
-    <link name="rgb_camera_link">
-        <xacro:body mass="0.001" d="0.010" h="0.03" w="0.03">
-            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-        </xacro:body>
-        <collision>
-            <geometry>
-                <box size="0.010 0.03 0.03"/>
-            </geometry>
-        </collision>
+<robot name="rae"
+  xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="rae_mono_camera" params="camera_name ns enable_depth:='false'">
+
+    <link name="rae_${camera_name}_sim_camera_optical_frame">
+      <xacro:body mass="0.00001" d="0.00001" h="0.000003" w="0.00001">
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      </xacro:body>
     </link>
 
-    <joint name="rgb_camera_joint" type="fixed">
-        <parent link="base_link" />
-        <child link="rgb_camera_link" />
-        <origin xyz="0.09 0.0 0.025" rpy="0 0 0" />
+    <joint name="rae_${camera_name}_sim_camera_optical_frame_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.0" rpy="${pi/2.0} ${-pi/2.0} 0.0" />
+      <parent link="rae_${camera_name}_camera_optical_frame" />
+      <child link="rae_${camera_name}_sim_camera_optical_frame" />
     </joint>
 
-    <link name="rgb_camera_link_optical_frame">
-        <xacro:body mass="0.00001" d="0.0000010" h="0.000003" w="0.00003">
-            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-        </xacro:body>
+    <xacro:if value="${enable_depth}">
+      <xacro:property name="camera_type" value="rgbd_camera" />
+      <xacro:property name="topic_postfix" value="" />
+    </xacro:if>
+    <xacro:unless value="${enable_depth}">
+      <xacro:property name="camera_type" value="camera" />
+      <xacro:property name="topic_postfix" value="/image_raw" />
+    </xacro:unless>
+
+
+    <gazebo reference="rae_${camera_name}_sim_camera_optical_frame">
+      <sensor name="${camera_name}_mono_camera" type="${camera_type}">
+        <update_rate>15.0</update_rate>
+        <topic>${ns}/rae/${camera_name}${topic_postfix}</topic>
+        <ignition_frame_id>${ns}_rae_${camera_name}_sim_camera_optical_frame</ignition_frame_id>
+        <camera>
+          <horizontal_fov>${105.0/180.0*pi}</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>400</height>
+          </image>
+          <clip>
+            <near>0.2</near>
+            <far>6.0</far>
+          </clip>
+        </camera>
+      </sensor>
+
+      <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
+        <engine>ogre2</engine>
+      </plugin>
+    </gazebo>
+  </xacro:macro>
+
+  <xacro:macro name="rae_rgb_camera" params="camera_name ns">
+    <link name="rae_${camera_name}_sim_optical_frame">
+      <xacro:body mass="0.00001" d="0.00001" h="0.000003" w="0.00001">
+        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      </xacro:body>
     </link>
 
-    <joint name="rgb_camera_link_optical_frame_joint" type="fixed">
-        <origin xyz="0.0051 0 0" rpy="0.0 0.0 0.0" />
-        <parent link="rgb_camera_link" />
-        <child link="rgb_camera_link_optical_frame" />
+    <joint name="rae_${camera_name}_sim_optical_frame_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.0" rpy="${pi/2} ${-pi/2} 0.0" />
+      <parent link="rae_${camera_name}_camera_optical_frame" />
+      <child link="rae_${camera_name}_sim_optical_frame" />
     </joint>
 
-    <gazebo reference="rgb_camera_link_optical_frame">
-        <sensor name="rgbd_camera" type="camera">
-            <camera>
-                <horizontal_fov>1.047</horizontal_fov>
-                <image>
-                    <width>320</width>
-                    <height>240</height>
-                </image>
-                <clip>
-                    <near>0.1</near>
-                    <far>100</far>
-                </clip>
-            </camera>
-            <ignition_frame_id>rgb_camera_link_optical_frame</ignition_frame_id>
-            <always_on>1</always_on>
-            <update_rate>30</update_rate>
-            <visualize>true</visualize>
-            <topic>/camera/image</topic>
-        </sensor>
+    <gazebo reference="rae_${camera_name}_sim_optical_frame">
+      <sensor name="rae_${camera_name}_camera" type="camera">
+        <always_on>true</always_on>
+        <update_rate>30.0</update_rate>
+        <topic>${ns}/rae/rgb/image_raw</topic>
+        <ignition_frame_id>${ns}/rae_${camera_name}_sim_optical_frame</ignition_frame_id>
+        <camera>
+          <horizontal_fov>${127.0/180.0*pi}</horizontal_fov>
+          <image>
+            <width>720</width>
+            <height>480</height>
+          </image>
+          <clip>
+            <near>0.1</near>
+            <far>100</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+      </sensor>
+
+      <plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
+        <engine>ogre2</engine>
+      </plugin>
     </gazebo>
-</robot>
+  </xacro:macro>
+</robot>
diff --git a/rae_description/urdf/rae.urdf.xacro b/rae_description/urdf/rae.urdf.xacro
@@ -2,8 +2,10 @@
 <robot name="rae"
   xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:arg name="sim_mode" default="false" />
+  <xacro:arg name="namespace" default="" />
 
   <xacro:include filename="inertial_moment_calc.xacro"/>
+  <xacro:include filename="camera.urdf.xacro"/>
   <xacro:include filename="rae_ros2_control.urdf.xacro"/>
   <xacro:property name="deg_to_rad" value="0.01745329251994329577" />
 
@@ -24,7 +26,7 @@
         <always_on>1</always_on>
         <update_rate>10.0</update_rate>
         <visualize>true</visualize>
-        <topic>rae/imu/data</topic>
+        <topic>/rae/imu/data</topic>
         <frame_id>rae_imu_frame</frame_id>
         <ignition_frame_id>rae_imu_frame</ignition_frame_id>
       </sensor>
@@ -238,7 +240,7 @@
   <joint name="rgb_camera_joint" type="fixed">
     <parent link="chassis"/>
     <child link="rae_rgb_camera_frame"/>
-    <origin xyz="-0.025 0.0101195 0.0517014" rpy="-0.523599 0 0"/>
+    <origin xyz="-0.025 0.0101195 0.0517014" rpy="${-30.0/180*pi} 0 0"/>
   </joint>
 
   <link name="rae_left_camera_frame">
@@ -251,7 +253,7 @@
   <joint name="rae_left_camera_joint" type="fixed">
     <parent link="chassis"/>
     <child link="rae_left_camera_frame"/>
-    <origin xyz="0.0375 0.0100195 0.0515282" rpy="-0.523599 0 0"/>
+    <origin xyz="0.0375 0.0100195 0.0515282" rpy="${-30.0/180*pi} 0 0"/>
   </joint>
 
   <link name="rae_left_camera_optical_frame">
@@ -277,7 +279,7 @@
   <joint name="rae_right_camera_joint" type="fixed">
     <parent link="chassis"/>
     <child link="rae_right_camera_frame"/>
-    <origin xyz="-0.0375 0.0100195 0.0515282" rpy="-0.523599 0 0"/>
+    <origin xyz="-0.0375 0.0100195 0.0515282" rpy="${-30.0/180*pi} 0 0"/>
   </joint>
 
   <link name="rae_right_camera_optical_frame">
@@ -303,7 +305,7 @@
   <joint name="rae_right_back_camera_joint" type="fixed">
     <parent link="chassis"/>
     <child link="rae_right_back_camera_frame"/>
-    <origin xyz="-0.0375 0.0100195 -0.0515282" rpy="0.523599 0 0"/>
+    <origin xyz="-0.0375 0.0100195 -0.0515282" rpy="${30.0/180*pi} 0 0"/>
   </joint>
 
   <link name="rae_right_back_camera_optical_frame">
@@ -329,7 +331,7 @@
   <joint name="rae_left_back_camera_joint" type="fixed">
     <parent link="chassis"/>
     <child link="rae_left_back_camera_frame"/>
-    <origin xyz="0.0375 0.0100195 -0.0515282" rpy="0.523599 0 0"/>
+    <origin xyz="0.0375 0.0100195 -0.0515282" rpy="${30.0/180*pi} 0 0"/>
   </joint>
 
   <link name="rae_left_back_camera_optical_frame">
@@ -353,6 +355,14 @@
     <child link="rae_imu_frame"/>
   </joint>
 
-  <xacro:rae_ros2_control sim_mode="$(arg sim_mode)"/>
+  <xacro:rae_ros2_control sim_mode="$(arg sim_mode)" namespace="$(arg namespace)" />
+
+  <xacro:if value="$(arg sim_mode)">
+    <xacro:rae_mono_camera camera_name="right" ns="$(arg namespace)" enable_depth="true" />
+    <xacro:rae_mono_camera camera_name="left_back" ns="$(arg namespace)" enable_depth="true" />
+
+    <!-- Uncomment when RGB camera is available on real robot -->
+    <!-- <xacro:rae_rgb_camera camera_name="rgb" ns="$(arg namespace)" /> -->
+  </xacro:if>
 
 </robot>
diff --git a/rae_description/urdf/rae_ros2_control.urdf.xacro b/rae_description/urdf/rae_ros2_control.urdf.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot name="rae"
   xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="rae_ros2_control" params="sim_mode">
+  <xacro:macro name="rae_ros2_control" params="sim_mode namespace">
     <ros2_control name="RAE" type="system">
       <xacro:unless value="${sim_mode}">
         <hardware>
@@ -68,7 +68,8 @@
         <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
           <parameters>$(find rae_hw)/config/controller.yaml</parameters>
           <ros>
-            <remapping>/diff_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
+            <namespace>${namespace}</namespace>
+            <remapping>diff_controller/cmd_vel_unstamped:=cmd_vel</remapping>
           </ros>
         </plugin>
       </gazebo>

diff --git a/rae_gazebo/config/gz_bridge.yaml b/rae_gazebo/config/gz_bridge.yaml
@@ -3,34 +3,50 @@
   gz_type_name: "ignition.msgs.Clock"
   direction: GZ_TO_ROS
 
-- topic_name: "/camera/image"
+- ros_topic_name: "/rae/right/image_raw"
+  gz_topic_name: "/rae/right/image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "ignition.msgs.Image"
   direction: GZ_TO_ROS
 
-- topic_name: "/camera/depth_image"
+- topic_name: "/rae/right/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "ignition.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/rae/stereo_front/image_raw"
+  gz_topic_name: "/rae/right/depth_image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "ignition.msgs.Image"
   direction: GZ_TO_ROS
 
-- topic_name: "/camera/camera_info"
+- ros_topic_name: "/rae/stereo_front/camera_info"
+  gz_topic_name: "/rae/right/camera_info"
   ros_type_name: "sensor_msgs/msg/CameraInfo"
   gz_type_name: "ignition.msgs.CameraInfo"
   direction: GZ_TO_ROS
 
-- topic_name: "/image_out"
+- ros_topic_name: "/rae/left_back/image_raw"
+  gz_topic_name: "/rae/left_back/image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "ignition.msgs.Image"
   direction: GZ_TO_ROS
 
-- topic_name: "/image_tuning"
+- topic_name: "/rae/left_back/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "ignition.msgs.CameraInfo"
+  direction: GZ_TO_ROS
+
+- ros_topic_name: "/rae/stereo_back/image_raw"
+  gz_topic_name: "/rae/left_back/depth_image"
   ros_type_name: "sensor_msgs/msg/Image"
   gz_type_name: "ignition.msgs.Image"
   direction: GZ_TO_ROS
 
-- topic_name: "/scan"
-  ros_type_name: "sensor_msgs/msg/LaserScan"
-  gz_type_name: "ignition.msgs.LaserScan"
+- ros_topic_name: "/rae/stereo_back/camera_info"
+  gz_topic_name: "/rae/left_back/camera_info"
+  ros_type_name: "sensor_msgs/msg/CameraInfo"
+  gz_type_name: "ignition.msgs.CameraInfo"
   direction: GZ_TO_ROS
 
 - topic_name: "/rae/imu/data"