diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h index 40ef759b7..48a723a73 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h +++ b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRaycasterBus.h @@ -88,6 +88,7 @@ namespace ROS2 { Points = (1 << 0), //!< return 3D point coordinates Ranges = (1 << 1), //!< return array of distances + Intensities = (1 << 2), //!< return intensity data }; //! Bitwise operators for RaycastResultFlags @@ -97,6 +98,7 @@ namespace ROS2 { AZStd::vector m_points; AZStd::vector m_ranges; + AZStd::vector m_intensities; }; //! Interface class that allows for communication with a single Lidar instance. diff --git a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h index 4ad411d69..423e8bb4f 100644 --- a/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h +++ b/Gems/ROS2/Code/Include/ROS2/Lidar/LidarRegistrarBus.h @@ -22,6 +22,7 @@ namespace ROS2 EntityExclusion = 1 << 2, MaxRangePoints = 1 << 3, PointcloudPublishing = 1 << 4, + Intensity = 1 << 5, All = 0b1111111111111111, }; diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp index 3b0828c60..07504b8f8 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.cpp @@ -75,6 +75,10 @@ namespace ROS2 } RaycastResultFlags requestedFlags = RaycastResultFlags::Ranges | RaycastResultFlags::Points; + if (m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Intensity) + { + requestedFlags |= RaycastResultFlags::Intensities; + } LidarRaycasterRequestBus::Event(m_lidarRaycasterId, &LidarRaycasterRequestBus::Events::ConfigureRaycastResultFlags, requestedFlags); @@ -161,8 +165,10 @@ namespace ROS2 return m_lidarRaycasterId; } - RaycastResult LidarCore::PerformRaycast() + const RaycastResult& LidarCore::PerformRaycast() { + static const RaycastResult EmptyResults{}; + AZ::Entity* entity = nullptr; AZ::ComponentApplicationBus::BroadcastResult(entity, &AZ::ComponentApplicationRequests::FindEntity, m_entityId); const auto entityTransform = entity->FindComponent(); @@ -172,7 +178,7 @@ namespace ROS2 if (m_lastScanResults.m_points.empty()) { AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n"); - return RaycastResult(); + return EmptyResults; } return m_lastScanResults; } diff --git a/Gems/ROS2/Code/Source/Lidar/LidarCore.h b/Gems/ROS2/Code/Source/Lidar/LidarCore.h index 5b78f41f3..445441214 100644 --- a/Gems/ROS2/Code/Source/Lidar/LidarCore.h +++ b/Gems/ROS2/Code/Source/Lidar/LidarCore.h @@ -37,7 +37,7 @@ namespace ROS2 //! Perform a raycast. //! @return Results of the raycast. - RaycastResult PerformRaycast(); + const RaycastResult& PerformRaycast(); //! Visualize the results of the last performed raycast. void VisualizeResults() const; diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp index 468f11eda..cf182a09d 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2Lidar2DSensorComponent.cpp @@ -108,7 +108,7 @@ namespace ROS2 void ROS2Lidar2DSensorComponent::FrequencyTick() { - RaycastResult lastScanResults = m_lidarCore.PerformRaycast(); + const RaycastResult& lastScanResults = m_lidarCore.PerformRaycast(); auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); auto message = sensor_msgs::msg::LaserScan(); diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp index 208bf6dfb..26bd74795 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp @@ -12,6 +12,7 @@ #include #include #include +#include namespace ROS2 { @@ -131,9 +132,7 @@ namespace ROS2 void ROS2LidarSensorComponent::FrequencyTick() { - auto entityTransform = GetEntity()->FindComponent(); - - if (m_canRaycasterPublish) + if (m_canRaycasterPublish && m_sensorConfiguration.m_publishingEnabled) { const builtin_interfaces::msg::Time timestamp = ROS2Interface::Get()->GetROSTimestamp(); LidarRaycasterRequestBus::Event( @@ -142,43 +141,91 @@ namespace ROS2 aznumeric_cast(timestamp.sec) * aznumeric_cast(1.0e9f) + timestamp.nanosec); } - auto lastScanResults = m_lidarCore.PerformRaycast(); + const RaycastResult& lastScanResults = m_lidarCore.PerformRaycast(); - if (m_canRaycasterPublish) + if (m_canRaycasterPublish || !m_sensorConfiguration.m_publishingEnabled) { // Skip publishing when it can be handled by the raycaster. return; } - const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse(); - for (auto& point : lastScanResults.m_points) - { - point = inverseLidarTM.TransformPoint(point); - } + PublishRaycastResults(lastScanResults); + } + + void ROS2LidarSensorComponent::PublishRaycastResults(const RaycastResult& results) + { + const bool isIntensityEnabled = m_lidarCore.m_lidarConfiguration.m_lidarSystemFeatures & LidarSystemFeatures::Intensity; auto* ros2Frame = Utils::GetGameOrEditorComponent(GetEntity()); auto message = sensor_msgs::msg::PointCloud2(); message.header.frame_id = ros2Frame->GetFrameID().data(); message.header.stamp = ROS2Interface::Get()->GetROSTimestamp(); message.height = 1; - message.width = lastScanResults.m_points.size(); - message.point_step = sizeof(AZ::Vector3); - message.row_step = message.width * message.point_step; + message.width = results.m_points.size(); + + sensor_msgs::PointCloud2Modifier modifier(message); + if (isIntensityEnabled) + { + modifier.setPointCloud2Fields( + 4, + "x", + 1, + sensor_msgs::msg::PointField::FLOAT32, + "y", + 1, + sensor_msgs::msg::PointField::FLOAT32, + "z", + 1, + sensor_msgs::msg::PointField::FLOAT32, + "intensity", + 1, + sensor_msgs::msg::PointField::FLOAT32); + } + else + { + modifier.setPointCloud2Fields( + 3, + "x", + 1, + sensor_msgs::msg::PointField::FLOAT32, + "y", + 1, + sensor_msgs::msg::PointField::FLOAT32, + "z", + 1, + sensor_msgs::msg::PointField::FLOAT32); + } + modifier.resize(results.m_points.size()); + + sensor_msgs::PointCloud2Iterator xIt(message, "x"); + sensor_msgs::PointCloud2Iterator yIt(message, "y"); + sensor_msgs::PointCloud2Iterator zIt(message, "z"); + + AZStd::optional> intensityIt; + if (isIntensityEnabled) + { + intensityIt = sensor_msgs::PointCloud2Iterator(message, "intensity"); + } - AZStd::array point_field_names = { "x", "y", "z" }; - for (int i = 0; i < point_field_names.size(); i++) + const auto entityTransform = GetEntity()->FindComponent(); + const auto inverseLidarTM = entityTransform->GetWorldTM().GetInverse(); + for (size_t i = 0; i < results.m_points.size(); ++i) { - sensor_msgs::msg::PointField pf; - pf.name = point_field_names[i]; - pf.offset = i * 4; - pf.datatype = sensor_msgs::msg::PointField::FLOAT32; - pf.count = 1; - message.fields.push_back(pf); + const AZ::Vector3 globalPoint = inverseLidarTM.TransformPoint(results.m_points[i]); + *xIt = globalPoint.GetX(); + *yIt = globalPoint.GetY(); + *zIt = globalPoint.GetZ(); + + if (isIntensityEnabled) + { + *intensityIt.value() = results.m_intensities[i]; + ++intensityIt.value(); + } + + ++xIt; + ++yIt; + ++zIt; } - size_t sizeInBytes = lastScanResults.m_points.size() * sizeof(AZ::Vector3); - message.data.resize(sizeInBytes); - AZ_Assert(message.row_step * message.height == sizeInBytes, "Inconsistency in the size of point cloud data"); - memcpy(message.data.data(), lastScanResults.m_points.data(), sizeInBytes); m_pointCloudPublisher->publish(message); } } // namespace ROS2 diff --git a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h index d958c45cc..ceaac63b4 100644 --- a/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h +++ b/Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.h @@ -44,6 +44,7 @@ namespace ROS2 private: ////////////////////////////////////////////////////////////////////////// void FrequencyTick(); + void PublishRaycastResults(const RaycastResult& results); bool m_canRaycasterPublish = false; std::shared_ptr> m_pointCloudPublisher;