diff --git a/BUILD.bazel b/BUILD.bazel index 9c3ccf0591..c6093a9184 100644 --- a/BUILD.bazel +++ b/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( [ diff --git a/CMakeLists.txt b/CMakeLists.txt index 5711ddcaf6..becda7d518 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -156,6 +156,7 @@ gz_find_package(gz-sensors REQUIRED air_pressure air_speed altimeter + cpu_lidar imu force_torque logical_camera diff --git a/examples/worlds/cpu_lidar_sensor.sdf b/examples/worlds/cpu_lidar_sensor.sdf new file mode 100644 index 0000000000..063b31fa1b --- /dev/null +++ b/examples/worlds/cpu_lidar_sensor.sdf @@ -0,0 +1,233 @@ + + + + + + + + 0.001 + 1.0 + + bullet + + + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + true + + + + + 20 20 0.1 + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + true + 3 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + + true + 0 3 0.5 0 0 0 + + + + + 0.5 + 1.0 + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + + true + -2 -2 0.75 0 0 0.785 + + + + + 1.5 0.5 1.5 + + + + + + + 1.5 0.5 1.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + + true + 0 0 0.5 0 0 0 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + /cpu_lidar + 10 + 1 + false + + + + 640 + 1 + -3.14159 + 3.14159 + + + 1 + 1 + 0 + 0 + + + + 0.08 + 10.0 + 0.01 + + + + + + + + diff --git a/include/gz/sim/components/CpuLidar.hh b/include/gz/sim/components/CpuLidar.hh new file mode 100644 index 0000000000..d2449015fc --- /dev/null +++ b/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 +#include +#include +#include +#include + +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; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.CpuLidar", CpuLidar) +} +} +} +} +#endif diff --git a/include/gz/sim/components/RaycastData.hh b/include/gz/sim/components/RaycastData.hh index ab776695a8..94f0a22726 100644 --- a/include/gz/sim/components/RaycastData.hh +++ b/include/gz/sim/components/RaycastData.hh @@ -67,6 +67,9 @@ struct RaycastDataInfo /// @brief The results of the raycasting. std::vector results; + + /// @brief Whether the sensor needs fresh raycast results this step. + bool needsRaycast = true; }; } diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d267cea5e9..4e5fe9110d 100644 --- a/src/SdfEntityCreator.cc +++ b/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) { diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 84255b30ff..9424b2514f 100644 --- a/src/systems/CMakeLists.txt +++ b/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) diff --git a/src/systems/cpu_lidar/CMakeLists.txt b/src/systems/cpu_lidar/CMakeLists.txt new file mode 100644 index 0000000000..3b6bdd7262 --- /dev/null +++ b/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 +) diff --git a/src/systems/cpu_lidar/CpuLidar.cc b/src/systems/cpu_lidar/CpuLidar.cc new file mode 100644 index 0000000000..17019c6ae9 --- /dev/null +++ b/src/systems/cpu_lidar/CpuLidar.cc @@ -0,0 +1,334 @@ +/* + * 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. + * +*/ + +#include "CpuLidar.hh" + +#include +#include +#include +#include +#include + +#include + +#include + +#include + +#include +#include + +#include "gz/sim/components/CpuLidar.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/RaycastData.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +/// \brief Private CpuLidar data class. +class gz::sim::systems::CpuLidarPrivate +{ + /// \brief A map of CPU lidar entity to its sensor. + public: std::unordered_map> entitySensorMap; + + /// \brief gz-sensors sensor factory for creating sensors + public: sensors::SensorFactory sensorFactory; + + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + + /// \brief Keep track of world ID. + /// Defaults to kNullEntity. + public: Entity worldEntity = kNullEntity; + + /// True if the system has been initialized + public: bool initialized = false; + + /// \brief Create CPU lidar sensors in gz-sensors + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); + + /// \brief Update CPU lidar sensor data based on physics raycast data + /// \param[in] _ecm Immutable reference to ECM. + public: void Update(const EntityComponentManager &_ecm); + + /// \brief Create sensor + /// \param[in] _ecm Immutable reference to ECM. + /// \param[in] _entity Entity of the CPU lidar + /// \param[in] _cpuLidar CpuLidar component. + /// \param[in] _parent Parent entity component. + public: void AddSensor( + const EntityComponentManager &_ecm, + const Entity _entity, + const components::CpuLidar *_cpuLidar, + const components::ParentEntity *_parent); + + /// \brief Remove CPU lidar sensors if their entities have been removed from + /// simulation. + /// \param[in] _ecm Immutable reference to ECM. + public: void RemoveSensorEntities(const EntityComponentManager &_ecm); +}; + +////////////////////////////////////////////////// +CpuLidar::CpuLidar() : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +CpuLidar::~CpuLidar() = default; + +////////////////////////////////////////////////// +void CpuLidar::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("CpuLidar::PreUpdate"); + + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + gzerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + + auto rays = it->second->GenerateRays(); + + components::RaycastDataInfo raycastData; + raycastData.rays.reserve(rays.size()); + for (const auto &ray : rays) + { + raycastData.rays.push_back({ray.first, ray.second}); + } + + _ecm.CreateComponent(entity, components::RaycastData(raycastData)); + } + this->dataPtr->newSensors.clear(); + + // Only raycast when the sensor needs fresh data. + if (!_info.paused) + { + for (auto &it : this->dataPtr->entitySensorMap) + { + auto *comp = _ecm.Component(it.first); + if (comp) + { + comp->Data().needsRaycast = + (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()); + } + } + } +} + +////////////////////////////////////////////////// +void CpuLidar::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + GZ_PROFILE("CpuLidar::PostUpdate"); + + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + gzwarn << "Detected jump back in time [" + << std::chrono::duration(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + this->dataPtr->CreateSensors(_ecm); + + if (!_info.paused) + { + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + + this->dataPtr->Update(_ecm); + + for (auto &it : this->dataPtr->entitySensorMap) + { + it.second->Update(_info.simTime, false); + } + } + + this->dataPtr->RemoveSensorEntities(_ecm); +} + +////////////////////////////////////////////////// +void CpuLidarPrivate::AddSensor( + const EntityComponentManager &_ecm, + const Entity _entity, + const components::CpuLidar *_cpuLidar, + const components::ParentEntity *_parent) +{ + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _cpuLidar->Data(); + data.SetName(sensorScopedName); + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/cpu_lidar"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::CpuLidarSensor>(data); + if (nullptr == sensor) + { + gzerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); +} + +////////////////////////////////////////////////// +void CpuLidarPrivate::CreateSensors(const EntityComponentManager &_ecm) +{ + GZ_PROFILE("CpuLidarPrivate::CreateSensors"); + if (kNullEntity == this->worldEntity) + this->worldEntity = _ecm.EntityByComponents(components::World()); + if (kNullEntity == this->worldEntity) + { + gzerr << "Missing world entity." << std::endl; + return; + } + + if (!this->initialized) + { + _ecm.Each( + [&](const Entity &_entity, + const components::CpuLidar *_cpuLidar, + const components::ParentEntity *_parent)->bool + { + this->AddSensor(_ecm, _entity, _cpuLidar, _parent); + return true; + }); + this->initialized = true; + } + else + { + _ecm.EachNew( + [&](const Entity &_entity, + const components::CpuLidar *_cpuLidar, + const components::ParentEntity *_parent)->bool + { + this->AddSensor(_ecm, _entity, _cpuLidar, _parent); + return true; + }); + } +} + +////////////////////////////////////////////////// +void CpuLidarPrivate::Update(const EntityComponentManager &_ecm) +{ + GZ_PROFILE("CpuLidarPrivate::Update"); + _ecm.Each( + [&](const Entity &_entity, + const components::CpuLidar * /*_cpuLidar*/, + const components::RaycastData *_raycastData, + const components::WorldPose *_worldPose)->bool + { + auto it = this->entitySensorMap.find(_entity); + if (it != this->entitySensorMap.end()) + { + it->second->SetPose(_worldPose->Data()); + + const auto &results = _raycastData->Data().results; + if (!results.empty()) + { + std::vector rayResults; + rayResults.reserve(results.size()); + for (const auto &result : results) + { + sensors::CpuLidarSensor::RayResult r; + r.point = result.point; + r.fraction = result.fraction; + r.normal = result.normal; + rayResults.push_back(r); + } + it->second->SetRaycastResults(rayResults); + } + } + else + { + gzerr << "Failed to update CpuLidar: " << _entity << ". " + << "Entity not found." << std::endl; + } + + return true; + }); +} + +////////////////////////////////////////////////// +void CpuLidarPrivate::RemoveSensorEntities( + const EntityComponentManager &_ecm) +{ + GZ_PROFILE("CpuLidarPrivate::RemoveSensorEntities"); + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::CpuLidar *)->bool + { + auto sensorId = this->entitySensorMap.find(_entity); + if (sensorId == this->entitySensorMap.end()) + { + gzerr << "Internal error, missing CpuLidar sensor for entity [" + << _entity << "]" << std::endl; + return true; + } + + this->entitySensorMap.erase(sensorId); + + return true; + }); +} + +GZ_ADD_PLUGIN(CpuLidar, System, + CpuLidar::ISystemPreUpdate, + CpuLidar::ISystemPostUpdate +) + +GZ_ADD_PLUGIN_ALIAS(CpuLidar, "gz::sim::systems::CpuLidar") diff --git a/src/systems/cpu_lidar/CpuLidar.hh b/src/systems/cpu_lidar/CpuLidar.hh new file mode 100644 index 0000000000..807349fa29 --- /dev/null +++ b/src/systems/cpu_lidar/CpuLidar.hh @@ -0,0 +1,64 @@ +/* + * 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_SYSTEMS_CPU_LIDAR_HH_ +#define GZ_SIM_SYSTEMS_CPU_LIDAR_HH_ + +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class CpuLidarPrivate; + + /// \class CpuLidar CpuLidar.hh gz/sim/systems/CpuLidar.hh + /// \brief This system manages all CPU-based lidar sensors in simulation. + /// Each sensor publishes lidar scan data over Gazebo Transport. + class CpuLidar: + public System, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: explicit CpuLidar(); + + /// \brief Destructor + public: ~CpuLidar() override; + + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) final; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 79664aac2d..189bba9c4c 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -557,6 +558,11 @@ class gz::sim::systems::PhysicsPrivate MinimumFeatureList, physics::GetRayIntersectionFromLastStepFeature>{}; + /// \brief Feature list for batched ray intersection queries. + public: struct BatchRayIntersectionFeatureList : physics::FeatureList< + MinimumFeatureList, + physics::GetBatchRayIntersectionFromLastStepFeature>{}; + /// \brief Feature list to change contacts before they are applied to physics. public: struct SetContactPropertiesCallbackFeatureList : physics::FeatureList< @@ -702,6 +708,7 @@ class gz::sim::systems::PhysicsPrivate ContactFeatureList, GravityFeatureList, RayIntersectionFeatureList, + BatchRayIntersectionFeatureList, SetContactPropertiesCallbackFeatureList, NestedModelFeatureList, CollisionDetectorFeatureList, @@ -4307,6 +4314,72 @@ void PhysicsPrivate::UpdateRayIntersections(EntityComponentManager &_ecm) return; } + // Prefer the batched ray intersection API when available. + auto worldBatchRayFeature = + this->entityWorldMap + .EntityCast(worldEntity); + + if (worldBatchRayFeature) + { + using BatchWorld = physics::World3d; + using RayQuery = BatchWorld::RayQuery; + + _ecm.Each( + [&](const Entity &_entity, + components::RaycastData *_raycastData, + components::Pose */*_pose*/) -> bool + { + if (!_raycastData->Data().needsRaycast) + return true; + _raycastData->Data().needsRaycast = false; + + const auto &rays = _raycastData->Data().rays; + auto &results = _raycastData->Data().results; + results.clear(); + results.reserve(rays.size()); + + const auto &entityWorldPose = worldPose(_entity, _ecm); + + std::vector batchInput; + batchInput.reserve(rays.size()); + for (const auto &ray : rays) + { + RayQuery q; + q.origin = math::eigen3::convert( + entityWorldPose.Pos() + + entityWorldPose.Rot().RotateVector(ray.start)); + q.target = math::eigen3::convert( + entityWorldPose.Pos() + + entityWorldPose.Rot().RotateVector(ray.end)); + batchInput.push_back(q); + } + + const auto batchOutput = + worldBatchRayFeature->GetBatchRayIntersectionFromLastStep( + batchInput); + + for (const auto &hit : batchOutput) + { + results.emplace_back(); + auto &result = results.back(); + + const math::Vector3d intersectionPoint = + math::eigen3::convert(hit.point); + result.point = entityWorldPose.Rot().RotateVectorReverse( + intersectionPoint - entityWorldPose.Pos()); + + result.fraction = hit.fraction; + + const math::Vector3d normal = math::eigen3::convert(hit.normal); + result.normal = entityWorldPose.Rot().RotateVectorReverse(normal); + } + return true; + }); + return; + } + + // Fallback: single-ray intersection auto worldRayIntersectionFeature = this->entityWorldMap.EntityCast(worldEntity); @@ -4333,6 +4406,10 @@ void PhysicsPrivate::UpdateRayIntersections(EntityComponentManager &_ecm) components::Pose */*_pose*/) -> bool { // Retrieve the rays from the RaycastData component + if (!_raycastData->Data().needsRaycast) + return true; + _raycastData->Data().needsRaycast = false; + const auto &rays = _raycastData->Data().rays; // Clear the previous results diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index c81149bdee..64c0a780b6 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -17,6 +17,7 @@ set(tests collada_world_exporter.cc components.cc contact_system.cc + cpu_lidar_system.cc detachable_joint.cc diff_drive_system.cc drive_to_pose_controller_system.cc diff --git a/test/integration/cpu_lidar_system.cc b/test/integration/cpu_lidar_system.cc new file mode 100644 index 0000000000..2d7b584f93 --- /dev/null +++ b/test/integration/cpu_lidar_system.cc @@ -0,0 +1,355 @@ +/* + * 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. + * +*/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include "gz/sim/components/CpuLidar.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/RaycastData.hh" +#include "gz/sim/components/Sensor.hh" + +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "test_config.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +// Named constants for iteration counts used in test scenarios. +// These values control how many simulation iterations each test runs +// to ensure sufficient time for sensor discovery and data generation. +namespace kIterations +{ +/// \brief Iterations for basic sensor discovery test +static constexpr int kDiscoveryIterations = 10; +/// \brief Iterations for raycast data validity test +static constexpr int kRaycastDataIterations = 10; +/// \brief Iterations for raycast results processing test +static constexpr int kRaycastResultsIterations = 200; +/// \brief Iterations for laser scan publication test +static constexpr int kLaserScanIterations = 200; +/// \brief Iteration threshold for sensor topic verification +static constexpr int kTopicVerificationIteration = 2; +/// \brief Iteration threshold for raycast data checking +static constexpr int kRaycastCheckIteration = 3; +/// \brief Iteration threshold for results processing +static constexpr int kResultsCheckIteration = 5; +/// \brief Timeout duration in milliseconds for message waiting +static constexpr int kMessageTimeoutMs = 5000; +/// \brief Iterations for point cloud publication test +static constexpr int kPointCloudIterations = 200; +} + +class CpuLidarTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +// Test for sensor discovery - verifies that the CPU lidar sensor +// is properly discovered and configured with the correct name and topic. +TEST_F(CpuLidarTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(Discovery)) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/cpu_lidar.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + bool cpuLidarFound = false; + + test::Relay testSystem; + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &_entity, + const components::CpuLidar *, + const components::Name *_name) -> bool + { + cpuLidarFound = true; + EXPECT_EQ("cpu_lidar", _name->Data()); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + // Verify sensor topic is set after initial discovery + if (_info.iterations > kIterations::kTopicVerificationIteration) + { + auto topicComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ("/test/cpu_lidar", topicComp->Data()); + } + } + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, kIterations::kDiscoveryIterations, false); + EXPECT_TRUE(cpuLidarFound); +} + +///////////////////////////////////////////////// +// Test for raycast data validity - verifies that the CPU lidar +// sensor generates valid raycast data with proper structure. +TEST_F(CpuLidarTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(RaycastDataValidity)) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/cpu_lidar.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + bool raycastDataFound = false; + + test::Relay testSystem; + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + // Check for raycast data after sufficient iterations + if (_info.iterations >= kIterations::kRaycastCheckIteration) + { + _ecm.Each( + [&](const Entity &, + const components::CpuLidar *, + const components::RaycastData *_rayData) -> bool + { + raycastDataFound = true; + EXPECT_EQ(640u, _rayData->Data().rays.size()); + + // Verify each ray has valid start and end points + for (const auto &ray : _rayData->Data().rays) + { + EXPECT_GT((ray.end - ray.start).Length(), 0.0); + } + return true; + }); + } + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, kIterations::kRaycastDataIterations, false); + EXPECT_TRUE(raycastDataFound); +} + +///////////////////////////////////////////////// +// Test for raycast results processing - verifies that the CPU lidar +// sensor properly processes and stores raycast results. +TEST_F(CpuLidarTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(RaycastResultsProcessed)) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/cpu_lidar.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + bool resultsFound = false; + + // Subscribe to the sensor topic so HasConnections() returns true + // and the system requests raycasts from Physics. + transport::Node node; + node.Subscribe("/test/cpu_lidar", [](const msgs::LaserScan &){}); + + test::Relay testSystem; + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + // Wait for sufficient iterations before checking results + if (_info.iterations < kIterations::kResultsCheckIteration) return; + + _ecm.Each( + [&](const Entity &, + const components::CpuLidar *, + const components::RaycastData *_rayData) -> bool + { + if (!_rayData->Data().results.empty()) + { + resultsFound = true; + EXPECT_EQ(_rayData->Data().rays.size(), + _rayData->Data().results.size()); + } + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, kIterations::kRaycastResultsIterations, false); + EXPECT_TRUE(resultsFound); +} + +///////////////////////////////////////////////// +// Test for laser scan publication - verifies that the CPU lidar +// sensor publishes laser scan messages correctly with valid range data. +// Uses condition_variable and mutex to wait for messages with timeout. +TEST_F(CpuLidarTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(LaserScanPublished)) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/cpu_lidar.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Synchronization primitives for thread-safe message waiting + std::mutex mutex; + std::condition_variable cv; + std::vector received; + bool messageReceived = false; + + transport::Node node; + node.Subscribe("/test/cpu_lidar", + [&](const msgs::LaserScan &_msg) + { + std::lock_guard lock(mutex); + received.push_back(_msg); + messageReceived = true; + cv.notify_one(); + }); + + // Run server in background and wait for message with timeout + std::thread serverThread([&]() + { + server.Run(true, kIterations::kLaserScanIterations, false); + }); + + // Wait for message with timeout using condition_variable + { + std::unique_lock lock(mutex); + auto timeout = std::chrono::milliseconds(kIterations::kMessageTimeoutMs); + if (!cv.wait_for(lock, timeout, [&]{ return messageReceived; })) + { + // Timeout reached without receiving message + } + } + + // Ensure server thread completes + serverThread.join(); + + std::lock_guard lock(mutex); + ASSERT_GT(received.size(), 0u); + + auto &scan = received.back(); + EXPECT_EQ(640, scan.count()); + EXPECT_DOUBLE_EQ(0.08, scan.range_min()); + EXPECT_DOUBLE_EQ(10.0, scan.range_max()); + + // Verify all range values are valid (not inf or nan and within bounds) + bool anyHit = false; + for (int i = 0; i < scan.ranges_size(); ++i) + { + if (!std::isinf(scan.ranges(i)) && !std::isnan(scan.ranges(i))) + { + anyHit = true; + EXPECT_GT(scan.ranges(i), 0.08); + EXPECT_LT(scan.ranges(i), 10.0); + } + } + EXPECT_TRUE(anyHit); +} + +///////////////////////////////////////////////// +// Test for point cloud publication - verifies that the CPU lidar +// sensor publishes PointCloudPacked messages with valid data. +TEST_F(CpuLidarTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(PointCloudPublished)) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/cpu_lidar.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + std::mutex mutex; + std::condition_variable cv; + std::vector received; + bool messageReceived = false; + + transport::Node node; + node.Subscribe("/test/cpu_lidar/points", + [&](const msgs::PointCloudPacked &_msg) + { + std::lock_guard lock(mutex); + received.push_back(_msg); + messageReceived = true; + cv.notify_one(); + }); + + std::thread serverThread([&]() + { + server.Run(true, kIterations::kPointCloudIterations, false); + }); + + { + std::unique_lock lock(mutex); + auto timeout = std::chrono::milliseconds(kIterations::kMessageTimeoutMs); + static_cast( + cv.wait_for(lock, timeout, [&]{ return messageReceived; })); + } + + serverThread.join(); + + std::lock_guard lock(mutex); + ASSERT_GT(received.size(), 0u); + + auto &msg = received.back(); + EXPECT_EQ(640u, msg.width()); + EXPECT_EQ(1u, msg.height()); + EXPECT_GT(msg.data().size(), 0u); + EXPECT_GT(msg.point_step(), 0u); +} diff --git a/test/worlds/cpu_lidar.sdf b/test/worlds/cpu_lidar.sdf new file mode 100644 index 0000000000..0ebf9f3f37 --- /dev/null +++ b/test/worlds/cpu_lidar.sdf @@ -0,0 +1,95 @@ + + + + + 0.001 + 0 + + bullet + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + + + true + 2 0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + + + true + 0 0 0.5 0 0 0 + + 0.05 0.05 0.05 0 0 0 + + + + + 0.1 0.1 0.1 + + + + + /test/cpu_lidar + 10 + 1 + false + + + + 640 + 1 + -1.396263 + 1.396263 + + + 1 + 1 + 0 + 0 + + + + 0.08 + 10.0 + 0.01 + + + + + + + +