Add CpuLidar system plugin with optimized physics raycasting

BUILD.bazel

@@ -598,6 +598,27 @@ gz_sim_system_libraries(
     ],
 )
 
+gz_sim_system_libraries(
+    srcs = glob(
+        [
+            "src/systems/cpu_lidar/**/*.cc",
+            "src/systems/cpu_lidar/**/*.hh",
+        ],
+    ),
+    so_lib_name = "gz-sim-cpu-lidar-system.so",
+    static_lib_name = "gz-sim-cpu-lidar-system-static",
+    visibility = ["//visibility:public"],
+    deps = [
+        ":gz-sim",
+        "@gz-common//profiler",
+        "@gz-physics",
+        "@gz-plugin//:register",
+        "@gz-sensors",
+        "@gz-sensors//:cpu_lidar",
+        "@sdformat",
+    ],
+)
+
 gz_sim_system_libraries(
     srcs = glob(
         [

CMakeLists.txt

@@ -156,6 +156,7 @@ gz_find_package(gz-sensors REQUIRED
     air_pressure
     air_speed
     altimeter
+    cpu_lidar
     imu
     force_torque
     logical_camera

examples/worlds/cpu_lidar_sensor.sdf

@@ -0,0 +1,233 @@
+<?xml version="1.0" ?>
+<!--
+  CPU Lidar Sensor Demo
+
+  This world demonstrates a physics-based lidar sensor that uses the collision
+  engine (DART + Bullet) for raycasting instead of the GPU rendering pipeline.
+  It does not require a GPU or rendering engine.
+
+  Requirements:
+    - Physics engine must be DART with the Bullet collision detector.
+      Other collision detectors (ODE, FCL) do not support raycasting.
+    - The cpu-lidar system plugin must be loaded (not the sensors system).
+    - The sensor detects collision geometry, not visual meshes.
+
+  Echo lidar messages:
+    gz topic -e -t /cpu_lidar
+  Echo point cloud messages:
+    gz topic -e -t /cpu_lidar/points
+-->
+<sdf version="1.6">
+  <world name="cpu_lidar_demo">
+
+    <!-- DART physics with Bullet collision detector (required for raycasting) -->
+    <physics name="1ms" type="dart">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+      <dart>
+        <collision_detector>bullet</collision_detector>
+      </dart>
+    </physics>
+
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <!-- CPU lidar system — processes lidar sensors via physics raycasting -->
+    <plugin
+      filename="gz-sim-cpu-lidar-system"
+      name="gz::sim::systems::CpuLidar">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <!-- Ground plane -->
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>20 20 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>20 20 0.1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Obstacle: box in front -->
+    <model name="box_front">
+      <static>true</static>
+      <pose>3 0 0.5 0 0 0</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 1</ambient>
+            <diffuse>1 0 0 1</diffuse>
+            <specular>1 0 0 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Obstacle: cylinder to the left -->
+    <model name="cylinder_left">
+      <static>true</static>
+      <pose>0 3 0.5 0 0 0</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <cylinder>
+              <radius>0.5</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <cylinder>
+              <radius>0.5</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0 1 0 1</ambient>
+            <diffuse>0 1 0 1</diffuse>
+            <specular>0 1 0 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Obstacle: box behind and to the right -->
+    <model name="box_behind_right">
+      <static>true</static>
+      <pose>-2 -2 0.75 0 0 0.785</pose>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>1.5 0.5 1.5</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>1.5 0.5 1.5</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0 0 1 1</ambient>
+            <diffuse>0 0 1 1</diffuse>
+            <specular>0 0 1 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Model with CPU lidar sensor -->
+    <model name="lidar_platform">
+      <static>true</static>
+      <pose>0 0 0.5 0 0 0</pose>
+      <link name="link">
+        <pose>0.05 0.05 0.05 0 0 0</pose>
+        <inertial>
+          <mass>0.1</mass>
+          <inertia>
+            <ixx>0.000166667</ixx>
+            <iyy>0.000166667</iyy>
+            <izz>0.000166667</izz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+
+        <sensor name="cpu_lidar" type="lidar">
+          <topic>/cpu_lidar</topic>
+          <update_rate>10</update_rate>
+          <always_on>1</always_on>
+          <visualize>false</visualize>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>640</samples>
+                <resolution>1</resolution>
+                <min_angle>-3.14159</min_angle>
+                <max_angle>3.14159</max_angle>
+              </horizontal>
+              <vertical>
+                <samples>1</samples>
+                <resolution>1</resolution>
+                <min_angle>0</min_angle>
+                <max_angle>0</max_angle>
+              </vertical>
+            </scan>
+            <range>
+              <min>0.08</min>
+              <max>10.0</max>
+              <resolution>0.01</resolution>
+            </range>
+          </ray>
+        </sensor>
+      </link>
+    </model>
+
+  </world>
+</sdf>

include/gz/sim/components/CpuLidar.hh

@@ -0,0 +1,43 @@
+/*
+ * Copyright (C) 2026 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef GZ_SIM_COMPONENTS_CPU_LIDAR_HH_
+#define GZ_SIM_COMPONENTS_CPU_LIDAR_HH_
+
+#include <sdf/Sensor.hh>
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/components/Serialization.hh>
+#include <gz/sim/config.hh>
+
+namespace gz
+{
+namespace sim
+{
+// Inline bracket to help doxygen filtering.
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief A component type that contains a CPU lidar sensor,
+  /// sdf::Sensor, information.
+  using CpuLidar = Component<sdf::Sensor, class CpuLidarTag,
+      serializers::SensorSerializer>;
+  GZ_SIM_REGISTER_COMPONENT("gz_sim_components.CpuLidar", CpuLidar)
+}
+}
+}
+}
+#endif

include/gz/sim/components/RaycastData.hh

@@ -67,6 +67,9 @@ struct RaycastDataInfo
 
   /// @brief The results of the raycasting.
   std::vector<RaycastResultInfo> results;
+
+  /// @brief Whether the sensor needs fresh raycast results this step.
+  bool needsRaycast = true;
 };
 }
 

src/SdfEntityCreator.cc

@@ -46,6 +46,7 @@
 #include "gz/sim/components/DepthCamera.hh"
 #include "gz/sim/components/ForceTorque.hh"
 #include "gz/sim/components/Geometry.hh"
+#include "gz/sim/components/CpuLidar.hh"
 #include "gz/sim/components/GpuLidar.hh"
 #include "gz/sim/components/Gravity.hh"
 #include "gz/sim/components/Imu.hh"
@@ -1135,11 +1136,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor)
   }
   else if (_sensor->Type() == sdf::SensorType::LIDAR)
   {
-    // \todo(anyone) Implement CPU-based lidar
-    // this->dataPtr->ecm->CreateComponent(sensorEntity,
-    //     components::Lidar(*_sensor));
-    gzwarn << "Sensor type LIDAR not supported yet. Try using"
-      << "a GPU LIDAR instead." << std::endl;
+    this->dataPtr->ecm->CreateComponent(sensorEntity,
+        components::CpuLidar(*_sensor));
+
+    this->dataPtr->ecm->CreateComponent(sensorEntity,
+        components::WorldPose(math::Pose3d::Zero));
   }
   else if (_sensor->Type() == sdf::SensorType::DEPTH_CAMERA)
   {

src/systems/CMakeLists.txt

@@ -90,6 +90,7 @@ add_subdirectory(buoyancy_engine)
 add_subdirectory(collada_world_exporter)
 add_subdirectory(comms_endpoint)
 add_subdirectory(contact)
+add_subdirectory(cpu_lidar)
 add_subdirectory(camera_video_recorder)
 add_subdirectory(detachable_joint)
 add_subdirectory(diff_drive)

src/systems/cpu_lidar/CMakeLists.txt

@@ -0,0 +1,8 @@
+gz_add_system(cpu-lidar
+  SOURCES
+    CpuLidar.cc
+  PUBLIC_LINK_LIBS
+    gz-common::gz-common
+  PRIVATE_LINK_LIBS
+    gz-sensors::cpu_lidar
+)