Review fixes for #55

Signed-off-by: Adam Dąbrowski <[email protected]>

Gems/ROS2/Code/Source/Lidar/LidarRaycaster.cpp

@@ -19,6 +19,7 @@ namespace ROS2
     {
         AZStd::vector<AZ::Vector3> results;
         AzPhysics::SceneQueryRequests requests;
+        requests.reserve(directions.size());
         for (const AZ::Vector3& direction : directions)
         {   // NOTE - performance-wise, consider reusing requests
             AZStd::shared_ptr<AzPhysics::RayCastRequest> request = AZStd::make_shared<AzPhysics::RayCastRequest>();

Gems/ROS2/Code/Source/Lidar/LidarTemplateUtils.cpp

@@ -25,15 +25,15 @@ namespace ROS2
         {
             LidarTemplate generic3DLidar =
             {
-                .m_model = LidarTemplate::Generic3DLidar,
-                .m_name = "GenericLidar",
-                .m_minHAngle = -180.0f,
-                .m_maxHAngle = 180.0f,
-                .m_minVAngle = 35.0f,
-                .m_maxVAngle = -35.0f,
-                .m_layers = 24,
-                .m_numberOfIncrements = 924,
-                .m_maxRange = 100.0f
+                /*.m_model = */LidarTemplate::Generic3DLidar,
+                /*.m_name = */"GenericLidar",
+                /*.m_minHAngle = */-180.0f,
+                /*.m_maxHAngle = */180.0f,
+                /*.m_minVAngle = */35.0f,
+                /*.m_maxVAngle = */-35.0f,
+                /*.m_layers = */24,
+                /*.m_numberOfIncrements = */924,
+                /*.m_maxRange = */100.0f
             };
             templates[LidarTemplate::Generic3DLidar] = generic3DLidar;
         }

Gems/ROS2/Code/Source/Lidar/ROS2LidarSensorComponent.cpp

@@ -112,7 +112,7 @@ namespace ROS2
         m_lastScanResults = m_lidarRaycaster.PerformRaycast(start, directions, distance);
         if (m_lastScanResults.empty())
         {
-            AZ_TracePrintf("Lidar Sensor Component", "No results from raycast");
+            AZ_TracePrintf("Lidar Sensor Component", "No results from raycast\n");
             return;
         }
 
@@ -142,8 +142,6 @@ namespace ROS2
             message.fields.push_back(pf);
         }
 
-        message.data.resize(message.row_step);
-
         auto lidarTM = entityTransform->GetWorldTM();
         lidarTM.Invert();
 
@@ -153,7 +151,10 @@ namespace ROS2
             point = lidarTM.TransformPoint(point);
         }
 
-        memcpy(message.data.data(), m_lastScanResults.data(), message.data.size());
+        size_t sizeInBytes = m_lastScanResults.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(), m_lastScanResults.data(), sizeInBytes);
         m_pointCloudPublisher->publish(message);
     }
 } // namespace ROS2