-
Notifications
You must be signed in to change notification settings - Fork 88
Add CpuLidarSensor for physics-based lidar without rendering #593
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
apojomovsky
wants to merge
26
commits into
gazebosim:main
Choose a base branch
from
apojomovsky:feature/cpu-lidar
base: main
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
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 34f245b
CpuLidarSensor SDF lidar config parsing
apojomovsky 2923aac
CpuLidarSensor ray generation from SDF config
apojomovsky d113796
CpuLidarSensor SetRaycastResults + range computation
apojomovsky 39d341c
CpuLidarSensor LaserScan publishing over gz-transport
apojomovsky 4788e51
gz-sensors: CpuLidarSensor Gaussian noise model
apojomovsky f0cefdc
gz-sensors: CpuLidarSensor PointCloud publishing
apojomovsky c1c9fc6
copyright: update to 2026
apojomovsky 666e4db
fix: extract 2D slice for LaserScan in 3D CPU Lidar mode
apojomovsky cdc7d55
fix: synchronize LaserScan metadata when extracting 2D slice from 3D …
apojomovsky a05719a
perf: optimize CPU Lidar with pre-computed unit vectors and vectorize…
apojomovsky 269f4d9
feat: add post-noise range clamping to CPU Lidar
apojomovsky 5b18c39
test: add failing intensity publication checks for CPU lidar
apojomovsky d921003
feat: publish CPU lidar intensities in scan and point cloud
apojomovsky db098d9
docs: clarify that RayResult intensity is always 0.0 from physics pip…
apojomovsky 00c7d8f
test: add DefaultTopic and HasConnectionsFalse tests for CPU lidar
apojomovsky 3ddc347
fix: guard Update against empty ranges to prevent segfault
apojomovsky e330dd2
bazel: add cpu_lidar sensor library target
apojomovsky 05c780d
style: tighten comments to match codebase conventions
apojomovsky 3fd5a8c
style: remove redundant unitVectors member from header
apojomovsky 534564c
fix: resolve cpplint line length violations
apojomovsky 0c40b53
nit: add explicit includes
apojomovsky 538cb09
Merge branch 'main' into feature/cpu-lidar
apojomovsky d9db0e5
fix: address review comments — sdf includes, dedup, NaN sentinel, tes…
apojomovsky 6fc8626
fix: align with gz-physics REP-117 convention — +INF for ray miss
apojomovsky 6a5369e
nit: trim verbose comments to match codebase conventions
apojomovsky File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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; | ||
apojomovsky marked this conversation as resolved.
Show resolved
Hide resolved
|
||
|
|
||
| /// \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 | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.