diff --git a/AirLib/include/sensors/lidar/LidarBase.hpp b/AirLib/include/sensors/lidar/LidarBase.hpp index 74316f91fb..7c8d5bd5f0 100644 --- a/AirLib/include/sensors/lidar/LidarBase.hpp +++ b/AirLib/include/sensors/lidar/LidarBase.hpp @@ -26,7 +26,7 @@ namespace airlib UpdatableObject::reportState(reporter); reporter.writeValue("Lidar-Timestamp", output_.time_stamp); - reporter.writeValue("Lidar-NumPoints", static_cast(output_.point_cloud.size() / 3)); + reporter.writeValue("Lidar-NumPoints", static_cast(output_.point_cloud.size() / 4)); } const LidarData& getOutput() const diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp index 5480349475..ddb38b8295 100644 --- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp +++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp @@ -91,6 +91,7 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const point_cloud.emplace_back(point.x()); point_cloud.emplace_back(point.y()); point_cloud.emplace_back(point.z()); + point_cloud.emplace_back(laser); segmentation_cloud.emplace_back(segmentationID); } } diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index e17a0f7381..a5101ddbce 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -733,14 +733,15 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr:: lidar_msg.header.stamp = ros::Time::now(); lidar_msg.header.frame_id = vehicle_name + "/" + sensor_name; - if (lidar_data.point_cloud.size() > 3) { + if (lidar_data.point_cloud.size() > 4) { lidar_msg.height = 1; - lidar_msg.width = lidar_data.point_cloud.size() / 3; + lidar_msg.width = lidar_data.point_cloud.size() / 4; - lidar_msg.fields.resize(3); + lidar_msg.fields.resize(4); lidar_msg.fields[0].name = "x"; lidar_msg.fields[1].name = "y"; lidar_msg.fields[2].name = "z"; + lidar_msg.fields[3].name = "ring"; int offset = 0; diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index ce3f3390df..45724fcd0b 100755 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -651,14 +651,15 @@ sensor_msgs::msg::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const lidar_msg.header.stamp = rclcpp::Time(lidar_data.time_stamp); lidar_msg.header.frame_id = vehicle_name + "/" + sensor_name; - if (lidar_data.point_cloud.size() > 3) { + if (lidar_data.point_cloud.size() > 4) { lidar_msg.height = 1; - lidar_msg.width = lidar_data.point_cloud.size() / 3; + lidar_msg.width = lidar_data.point_cloud.size() / 4; - lidar_msg.fields.resize(3); + lidar_msg.fields.resize(4); lidar_msg.fields[0].name = "x"; lidar_msg.fields[1].name = "y"; lidar_msg.fields[2].name = "z"; + lidar_msg.fields[3].name = "ring"; int offset = 0;