Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
0662b69
CpuLidarSensor skeleton + creation test
apojomovsky Feb 11, 2026
34f245b
CpuLidarSensor SDF lidar config parsing
apojomovsky Feb 11, 2026
2923aac
CpuLidarSensor ray generation from SDF config
apojomovsky Feb 11, 2026
d113796
CpuLidarSensor SetRaycastResults + range computation
apojomovsky Feb 11, 2026
39d341c
CpuLidarSensor LaserScan publishing over gz-transport
apojomovsky Feb 11, 2026
4788e51
gz-sensors: CpuLidarSensor Gaussian noise model
apojomovsky Feb 12, 2026
f0cefdc
gz-sensors: CpuLidarSensor PointCloud publishing
apojomovsky Feb 12, 2026
c1c9fc6
copyright: update to 2026
apojomovsky Feb 13, 2026
666e4db
fix: extract 2D slice for LaserScan in 3D CPU Lidar mode
apojomovsky Feb 13, 2026
cdc7d55
fix: synchronize LaserScan metadata when extracting 2D slice from 3D …
apojomovsky Feb 16, 2026
a05719a
perf: optimize CPU Lidar with pre-computed unit vectors and vectorize…
apojomovsky Feb 16, 2026
269f4d9
feat: add post-noise range clamping to CPU Lidar
apojomovsky Feb 16, 2026
5b18c39
test: add failing intensity publication checks for CPU lidar
apojomovsky Feb 16, 2026
d921003
feat: publish CPU lidar intensities in scan and point cloud
apojomovsky Feb 16, 2026
db098d9
docs: clarify that RayResult intensity is always 0.0 from physics pip…
apojomovsky Feb 18, 2026
00c7d8f
test: add DefaultTopic and HasConnectionsFalse tests for CPU lidar
apojomovsky Feb 18, 2026
3ddc347
fix: guard Update against empty ranges to prevent segfault
apojomovsky Feb 18, 2026
e330dd2
bazel: add cpu_lidar sensor library target
apojomovsky Feb 19, 2026
05c780d
style: tighten comments to match codebase conventions
apojomovsky Feb 19, 2026
3fd5a8c
style: remove redundant unitVectors member from header
apojomovsky Feb 19, 2026
534564c
fix: resolve cpplint line length violations
apojomovsky Feb 19, 2026
0c40b53
nit: add explicit includes
apojomovsky Feb 20, 2026
538cb09
Merge branch 'main' into feature/cpu-lidar
apojomovsky Mar 23, 2026
d9db0e5
fix: address review comments — sdf includes, dedup, NaN sentinel, tes…
apojomovsky Apr 8, 2026
6fc8626
fix: align with gz-physics REP-117 convention — +INF for ray miss
apojomovsky Apr 8, 2026
6a5369e
nit: trim verbose comments to match codebase conventions
apojomovsky Apr 9, 2026
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 19 additions & 0 deletions BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -239,6 +239,24 @@ cc_test(
],
)

gz_sensor_library(
name = "cpu_lidar",
srcs = ["src/CpuLidarSensor.cc"],
hdrs = ["include/gz/sensors/CpuLidarSensor.hh"],
includes = ["include"],
visibility = ["//visibility:public"],
deps = [
":gz-sensors",
"@gz-common//profiler",
"@gz-math",
"@gz-msgs",
"@gz-msgs//:gzmsgs_cc_proto",
"@gz-transport",
"@gz-utils//:SuppressWarning",
"@sdformat",
],
)

gz_sensor_library(
name = "depth_camera",
srcs = ["src/DepthCameraSensor.cc"],
Expand Down Expand Up @@ -595,6 +613,7 @@ cc_library(
":altimeter",
":boundingbox_camera",
":camera",
":cpu_lidar",
":depth_camera",
":dvl",
":force_torque",
Expand Down
139 changes: 139 additions & 0 deletions include/gz/sensors/CpuLidarSensor.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,139 @@
/*
* 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_SENSORS_CPULIDARSENSOR_HH_
#define GZ_SENSORS_CPULIDARSENSOR_HH_

#include <memory>
#include <utility>
#include <vector>

#include <sdf/Element.hh>
#include <sdf/Sensor.hh>

#include <gz/math/Angle.hh>
#include <gz/math/Vector3.hh>
#include <gz/utils/SuppressWarning.hh>

#include <gz/sensors/config.hh>
#include <gz/sensors/cpu_lidar/Export.hh>

#include "gz/sensors/Sensor.hh"

namespace gz
{
namespace sensors
{
inline namespace GZ_SENSORS_VERSION_NAMESPACE {

class CpuLidarSensorPrivate;

/// \brief CPU-based lidar sensor that performs ray casting without
/// depending on gz-rendering.
class GZ_SENSORS_CPU_LIDAR_VISIBLE CpuLidarSensor : public Sensor
{
/// \brief Result of a single ray cast
public: struct RayResult
{
/// \brief Hit point in entity frame
gz::math::Vector3d point;

/// \brief Fraction along the ray [0,1]; +INF if no object in range; NaN on error.
double fraction;

/// \brief Normal at hit point in entity frame
gz::math::Vector3d normal;

/// \brief Intensity value
double intensity = 0.0;
};
/// \brief constructor
public: CpuLidarSensor();

/// \brief destructor
public: virtual ~CpuLidarSensor();

/// \brief Load the sensor based on data from an sdf::Sensor object.
/// \param[in] _sdf SDF Sensor parameters.
/// \return true if loading was successful
public: virtual bool Load(const sdf::Sensor &_sdf) override;

/// \brief Load the sensor with SDF parameters.
/// \param[in] _sdf SDF Sensor parameters.
/// \return true if loading was successful
public: virtual bool Load(sdf::ElementPtr _sdf) override;

using Sensor::Update;

/// \brief Update the sensor and generate data
/// \param[in] _now The current time
/// \return true if the update was successful
public: virtual bool Update(
const std::chrono::steady_clock::duration &_now) override;

/// \brief Check if there are any subscribers
/// \return True if there are subscribers, false otherwise
public: virtual bool HasConnections() const override;

/// \brief Get the minimum horizontal angle
public: gz::math::Angle AngleMin() const;

/// \brief Get the maximum horizontal angle
public: gz::math::Angle AngleMax() const;

/// \brief Get the minimum vertical angle
public: gz::math::Angle VerticalAngleMin() const;

/// \brief Get the maximum vertical angle
public: gz::math::Angle VerticalAngleMax() const;

/// \brief Get the minimum range
public: double RangeMin() const;

/// \brief Get the maximum range
public: double RangeMax() const;

/// \brief Get the horizontal ray count
public: unsigned int RayCount() const;

/// \brief Get the vertical ray count
public: unsigned int VerticalRayCount() const;

/// \brief Generate rays from the lidar configuration.
/// \return Vector of (start, end) pairs in entity frame
public: std::vector<std::pair<gz::math::Vector3d, gz::math::Vector3d>>
GenerateRays() const;

/// \brief Set the raycast results from the physics engine.
/// \param[in] _results Vector of results, one per ray.
public: void SetRaycastResults(
const std::vector<RayResult> &_results);

/// \brief Get all range values.
/// \param[out] _ranges Vector to fill with range data.
public: void Ranges(std::vector<double> &_ranges) const;

GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
/// \brief Data pointer for private data
/// \internal
private: std::unique_ptr<CpuLidarSensorPrivate> dataPtr;
GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
};
}
}
}

#endif
8 changes: 8 additions & 0 deletions src/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,14 @@ gz_add_component(magnetometer SOURCES ${magnetometer_sources} GET_TARGET_NAME ma
set(imu_sources ImuSensor.cc)
gz_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target)

set(cpu_lidar_sources CpuLidarSensor.cc)
gz_add_component(cpu_lidar SOURCES ${cpu_lidar_sources} GET_TARGET_NAME cpu_lidar_target)
target_link_libraries(${cpu_lidar_target}
PRIVATE
gz-msgs::gz-msgs
gz-transport::gz-transport
)

set(altimeter_sources AltimeterSensor.cc)
gz_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target)

Expand Down
Loading