Sensor Simulation (LiDAR, Depth, IMU)

Why Simulate Sensors?

Virtual sensors enable:

• Unlimited Data: Generate millions of labeled samples for ML training
• Perfect Ground Truth: Know exact object poses, velocities
• Controlled Testing: Vary lighting, weather, occlusions systematically
• Cost Savings: Test with $500 virtual LiDAR instead of $5,000 physical unit

---

LiDAR Simulation

2D Planar LiDAR (RPLiDAR A1)

lidar_plugin.urdf

<gazebo reference="lidar_link">
  <sensor type="ray" name="lidar">
    <pose>0 0 0 0 0 0</pose>
    <visualize>true</visualize>
    <update_rate>10</update_rate>

    <ray>
      <scan>
        <horizontal>
          <samples>360</samples>
          <resolution>1</resolution>
          <min_angle>-3.14159</min_angle>
          <max_angle>3.14159</max_angle>
        </horizontal>
      </scan>
      <range>
        <min>0.15</min>
        <max>12.0</max>
        <resolution>0.01</resolution>
      </range>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.01</stddev>
      </noise>
    </ray>

    <plugin name="gazebo_ros_lidar" filename="libgazebo_ros_ray_sensor.so">
      <ros>
        <namespace>/robot</namespace>
        <remapping>~/out:=scan</remapping>
      </ros>
      <output_type>sensor_msgs/LaserScan</output_type>
      <frame_name>lidar_link</frame_name>
    </plugin>
  </sensor>
</gazebo>

Resulting ROS 2 Topic:

ros2 topic echo /robot/scan

# Output: sensor_msgs/LaserScan
# ranges: [1.2, 1.1, 0.9, ..., 1.3]  (360 measurements)
# angle_min: -3.14159
# angle_max: 3.14159
# range_min: 0.15
# range_max: 12.0

3D LiDAR (Velodyne VLP-16)

<sensor type="ray" name="velodyne">
  <ray>
    <scan>
      <horizontal>
        <samples>1800</samples>  <!-- 0.2° resolution -->
        <min_angle>-3.14159</min_angle>
        <max_angle>3.14159</max_angle>
      </horizontal>
      <vertical>
        <samples>16</samples>  <!-- 16 laser beams -->
        <min_angle>-0.2618</min_angle>  <!-- -15° -->
        <max_angle>0.2618</max_angle>   <!-- +15° -->
      </vertical>
    </scan>
    <range>
      <min>0.3</min>
      <max>100.0</max>
    </range>
  </ray>
  <plugin name="gazebo_ros_velodyne" filename="libgazebo_ros_ray_sensor.so">
    <output_type>sensor_msgs/PointCloud2</output_type>
  </plugin>
</sensor>

Visualize in RViz2:

rviz2
# Add → PointCloud2 → Topic: /robot/velodyne/points

---

Depth Camera Simulation (RealSense D435i)

depth_camera.urdf

<gazebo reference="camera_link">
  <sensor name="depth_camera" type="depth">
    <update_rate>30</update_rate>
    <camera>
      <horizontal_fov>1.5708</horizontal_fov>  <!-- 90° FOV -->
      <image>
        <width>640</width>
        <height>480</height>
        <format>R8G8B8</format>
      </image>
      <clip>
        <near>0.1</near>
        <far>10.0</far>
      </clip>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.007</stddev>
      </noise>
    </camera>

    <plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
      <ros>
        <namespace>/robot</namespace>
        <remapping>~/image_raw:=rgb/image_raw</remapping>
        <remapping>~/depth/image_raw:=depth/image_raw</remapping>
        <remapping>~/points:=depth/points</remapping>
      </ros>
      <camera_name>realsense</camera_name>
      <frame_name>camera_link</frame_name>
      <hack_baseline>0.07</hack_baseline>  <!-- Stereo baseline -->
    </plugin>
  </sensor>
</gazebo>

Processing Depth Images:

depth_processor.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
import numpy as np

class DepthProcessor(Node):
    def __init__(self):
        super().__init__('depth_processor')
        self.bridge = CvBridge()

        self.subscription = self.create_subscription(
            Image,
            '/robot/depth/image_raw',
            self.depth_callback,
            10
        )

    def depth_callback(self, msg):
        # Convert ROS Image to OpenCV
        depth_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='32FC1')

        # Find closest obstacle
        min_depth = np.nanmin(depth_image)
        self.get_logger().info(f'Closest obstacle: {min_depth:.2f}m')

        # Visualize
        depth_colormap = cv2.applyColorMap(
            cv2.convertScaleAbs(depth_image, alpha=0.03),
            cv2.COLORMAP_JET
        )
        cv2.imshow('Depth', depth_colormap)
        cv2.waitKey(1)

def main(args=None):
    rclpy.init(args=args)
    node = DepthProcessor()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

---

IMU Simulation

imu.urdf

<gazebo reference="imu_link">
  <sensor name="imu_sensor" type="imu">
    <always_on>true</always_on>
    <update_rate>100</update_rate>
    <imu>
      <angular_velocity>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
          </noise>
        </z>
      </angular_velocity>
      <linear_acceleration>
        <x>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
          </noise>
        </x>
        <y>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
          </noise>
        </y>
        <z>
          <noise type="gaussian">
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
          </noise>
        </z>
      </linear_acceleration>
    </imu>
    <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
      <ros>
        <namespace>/robot</namespace>
        <remapping>~/out:=imu</remapping>
      </ros>
      <frame_name>imu_link</frame_name>
    </plugin>
  </sensor>
</gazebo>

IMU Data Structure:

# sensor_msgs/Imu
msg.orientation          # Quaternion (x, y, z, w)
msg.angular_velocity     # rad/s (x, y, z)
msg.linear_acceleration  # m/s² (x, y, z)

---

Sensor Fusion Example

sensor_fusion.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, Imu
from geometry_msgs.msg import Twist

class FusionNode(Node):
    def __init__(self):
        super().__init__('fusion_node')

        # Subscribers
        self.create_subscription(LaserScan, '/robot/scan', self.lidar_callback, 10)
        self.create_subscription(Imu, '/robot/imu', self.imu_callback, 10)

        # Publisher
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)

        # State
        self.obstacle_distance = float('inf')
        self.pitch = 0.0  # From IMU

    def lidar_callback(self, msg):
        self.obstacle_distance = min(msg.ranges)

    def imu_callback(self, msg):
        # Extract pitch from quaternion (simplified)
        import math
        q = msg.orientation
        self.pitch = math.asin(2.0 * (q.w * q.y - q.z * q.x))

        # Obstacle avoidance with stability check
        cmd = Twist()
        if self.obstacle_distance < 0.5:
            cmd.linear.x = 0.0
            cmd.angular.z = 0.5
        elif abs(self.pitch) > 0.3:  # >17° tilt
            cmd.linear.x = 0.0  # Stop if unstable
            self.get_logger().warn(f'Unstable pitch: {self.pitch:.2f} rad')
        else:
            cmd.linear.x = 0.3

        self.cmd_pub.publish(cmd)

def main(args=None):
    rclpy.init(args=args)
    rclpy.spin(FusionNode())
    rclpy.shutdown()

---

Realistic Noise Models

Gaussian Noise

<noise>
  <type>gaussian</type>
  <mean>0.0</mean>
  <stddev>0.01</stddev>  <!-- 1cm standard deviation -->
</noise>

Quantization (Sensor Bit Depth)

<noise>
  <type>gaussian</type>
  <mean>0.0</mean>
  <stddev>0.005</stddev>
  <precision>0.01</precision>  <!-- Round to nearest 1cm -->
</noise>

---

Key Takeaways

✅ LiDAR: Ray casting for 2D/3D obstacle detection

✅ Depth Camera: RGB-D for object segmentation and SLAM

✅ IMU: Angular velocity + linear acceleration for orientation

✅ Noise Models: Gaussian noise mimics real sensor uncertainty

---

Next Steps

Learn how to create photorealistic visualizations with Unity.

Continue to Unity Visualization →