Fix crash when using world_frame_id enu

CodexLabsLLC · Feb 2, 2022 · 765f0b8 · 765f0b8
1 parent 0ba43c8
commit 765f0b8
Showing 1 changed file with 18 additions and 16 deletions.
diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
@@ -760,28 +760,30 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr::
         const unsigned char* bytes = reinterpret_cast<const unsigned char*>(data_std.data());
         vector<unsigned char> lidar_msg_data(bytes, bytes + sizeof(float) * data_std.size());
         lidar_msg.data = std::move(lidar_msg_data);
-    }
-    else {
-        // msg = []
-    }
 
-    if (isENU_) {
-        try {
-            sensor_msgs::PointCloud2 lidar_msg_enu;
-            auto transformStampedENU = tf_buffer_.lookupTransform(AIRSIM_FRAME_ID, vehicle_name, ros::Time(0), ros::Duration(1));
-            tf2::doTransform(lidar_msg, lidar_msg_enu, transformStampedENU);
+        if (isENU_) {
+            try {
+                sensor_msgs::PointCloud2 lidar_msg_enu;
+                auto transformStampedENU = tf_buffer_.lookupTransform(AIRSIM_FRAME_ID, vehicle_name, ros::Time(0), ros::Duration(1));
+                tf2::doTransform(lidar_msg, lidar_msg_enu, transformStampedENU);
 
-            lidar_msg_enu.header.stamp = lidar_msg.header.stamp;
-            lidar_msg_enu.header.frame_id = lidar_msg.header.frame_id;
+                lidar_msg_enu.header.stamp = lidar_msg.header.stamp;
+                lidar_msg_enu.header.frame_id = lidar_msg.header.frame_id;
 
-            lidar_msg = std::move(lidar_msg_enu);
-        }
-        catch (tf2::TransformException& ex) {
-            ROS_WARN("%s", ex.what());
-            ros::Duration(1.0).sleep();
+                lidar_msg = std::move(lidar_msg_enu);
+            }
+            catch (tf2::TransformException& ex) {
+                ROS_WARN("%s", ex.what());
+                ros::Duration(1.0).sleep();
+            }
         }
+
+    }
+    else {
+        // msg = []
     }
 
+
     return lidar_msg;
 }