From 026e740b6275feb1e0768dd10b932307cf22d0d7 Mon Sep 17 00:00:00 2001 From: TJU-Lu Date: Mon, 27 Oct 2025 23:20:33 +0800 Subject: [PATCH] Fix critical lag on some systems caused by CvBridge Error --- Simulator/src/include/sensor_simulator.h | 2 +- Simulator/src/src/test_simulator_cuda.cpp | 2 +- YOPO/test_yopo_ros.py | 64 ++++++++++++++--------- 3 files changed, 41 insertions(+), 27 deletions(-) diff --git a/Simulator/src/include/sensor_simulator.h b/Simulator/src/include/sensor_simulator.h index 0c1696e..e7b74fb 100644 --- a/Simulator/src/include/sensor_simulator.h +++ b/Simulator/src/include/sensor_simulator.h @@ -112,7 +112,7 @@ public: image_pub_ = nh_.advertise(depth_topic, 1); point_cloud_pub_ = nh_.advertise(lidar_topic, 1); - odom_sub_ = nh_.subscribe(odom_topic, 1, &SensorSimulator::odomCallback, this); + odom_sub_ = nh_.subscribe(odom_topic, 1, &SensorSimulator::odomCallback, this, ros::TransportHints().tcpNoDelay()); timer_depth_ = nh_.createTimer(ros::Duration(1 / depth_fps), &SensorSimulator::timerDepthCallback, this); timer_lidar_ = nh_.createTimer(ros::Duration(1 / lidar_fps), &SensorSimulator::timerLidarCallback, this); printf("3.Simulation Ready! \n"); diff --git a/Simulator/src/src/test_simulator_cuda.cpp b/Simulator/src/src/test_simulator_cuda.cpp index 17d4592..4cdc057 100644 --- a/Simulator/src/src/test_simulator_cuda.cpp +++ b/Simulator/src/src/test_simulator_cuda.cpp @@ -103,7 +103,7 @@ public: // ROS image_pub_ = nh_.advertise(depth_topic, 1); point_cloud_pub_ = nh_.advertise(lidar_topic, 1); - odom_sub_ = nh_.subscribe(odom_topic, 1, &SensorSimulator::odomCallback, this); + odom_sub_ = nh_.subscribe(odom_topic, 1, &SensorSimulator::odomCallback, this, ros::TransportHints().tcpNoDelay()); timer_depth_ = nh_.createTimer(ros::Duration(1 / depth_fps), &SensorSimulator::timerDepthCallback, this); timer_lidar_ = nh_.createTimer(ros::Duration(1 / lidar_fps), &SensorSimulator::timerLidarCallback, this); timer_map_ = nh_.createTimer(ros::Duration(1), &SensorSimulator::timerMapCallback, this); diff --git a/YOPO/test_yopo_ros.py b/YOPO/test_yopo_ros.py index 11694fe..f5368ab 100644 --- a/YOPO/test_yopo_ros.py +++ b/YOPO/test_yopo_ros.py @@ -46,7 +46,7 @@ class YopoNet: self.device = "cuda" if torch.cuda.is_available() else "cpu" # variables - self.bridge = CvBridge() + # self.bridge = CvBridge() self.odom = Odometry() self.odom_init = False self.last_yaw = 0.0 @@ -149,18 +149,11 @@ class YopoNet: if not self.odom_init: return # 1. Depth Image Process - try: - depth = self.bridge.imgmsg_to_cv2(data, "32FC1") - except Exception as e1: - try: - assert data.encoding == "32FC1", f"Expected encoding '32FC1', got {data.encoding}" - depth = np.frombuffer(data.data, dtype=np.float32).reshape(data.height, data.width) - except Exception as e2: - print(f"\033[91mBoth cv_bridge and numpy fallback failed:\ncv_bridge error: {e1}\nnumpy error: {e2}\033[0m") - print("Possible solutions may be found at https://github.com/TJU-Aerial-Robotics/YOPO/issues/2") - return - time0 = time.time() + # depth = self.bridge.imgmsg_to_cv2(data, "32FC1") + assert data.encoding == "32FC1", f"Expected encoding '32FC1', got {data.encoding}" + depth = np.frombuffer(data.data, dtype=np.float32).reshape(data.height, data.width) + if depth.shape[0] != self.height or depth.shape[1] != self.width: depth = cv2.resize(depth, (self.width, self.height), interpolation=cv2.INTER_NEAREST) depth = np.minimum(depth * self.scale, self.max_dis) / self.max_dis @@ -210,19 +203,7 @@ class YopoNet: self.visualize_trajectory(score_pred, endstate_w) time5 = time.time() - if self.verbose: - self.time_interpolation = self.time_interpolation + (time1 - time0) - self.time_prepare = self.time_prepare + (time2 - time1) - self.time_forward = self.time_forward + (time3 - time2) - self.time_process = self.time_process + (time4 - time3) - self.time_visualize = self.time_visualize + (time5 - time4) - self.count = self.count + 1 - print(f"Time Consuming:" - f"depth-interpolation: {1000 * self.time_interpolation / self.count:.2f}ms;" - f"data-prepare: {1000 * self.time_prepare / self.count:.2f}ms; " - f"network-inference: {1000 * self.time_forward / self.count:.2f}ms; " - f"post-process: {1000 * self.time_process / self.count:.2f}ms;" - f"visualize-trajectory: {1000 * self.time_visualize / self.count:.2f}ms") + self.print_time(time0, time1, time2, time3, time4, time5) def control_pub(self, _timer): if self.ctrl_time is None or self.ctrl_time > self.traj_time: @@ -337,6 +318,39 @@ class YopoNet: point_cloud_msg = point_cloud2.create_cloud(header, fields, points_array) self.all_trajs_pub.publish(point_cloud_msg) + def print_time(self, time0, time1, time2, time3, time4, time5): + """ + Performance reference: PyTorch model should take < 5 ms; TensorRT model should take < 1 ms + + Notes: + - Running program and enabling RViz under WSL greatly increase processing time, and Ubuntu does not have these issues + - Even with queue_size=1, it may cause message accumulation and lag when processing time exceeds the image frequency + """ + self.time_interpolation = self.time_interpolation + (time1 - time0) + self.time_prepare = self.time_prepare + (time2 - time1) + self.time_forward = self.time_forward + (time3 - time2) + self.time_process = self.time_process + (time4 - time3) + self.time_visualize = self.time_visualize + (time5 - time4) + self.count = self.count + 1 + + total_time = (time5 - time0) * 1000 + tolerance = 30.0 + if total_time > tolerance: + rospy.logwarn(f"Warn: Processing time {(time5 - time0) * 1000:.2f} ms exceeds {tolerance} ms, may cause message lag!") + print(f"Current Time Consuming:" + f"depth-interpolation: {1000 * (time1 - time0):.2f}ms;" + f"data-prepare: {1000 * (time2 - time1):.2f}ms; " + f"network-inference: {1000 * (time3 - time2):.2f}ms; " + f"post-process: {1000 * (time4 - time3):.2f}ms;" + f"visualize-trajectory: {1000 * (time5 - time4):.2f}ms") + if self.verbose or (total_time > tolerance): + print(f"Average Time Consuming:" + f"depth-interpolation: {1000 * self.time_interpolation / self.count:.2f}ms;" + f"data-prepare: {1000 * self.time_prepare / self.count:.2f}ms; " + f"network-inference: {1000 * self.time_forward / self.count:.2f}ms; " + f"post-process: {1000 * self.time_process / self.count:.2f}ms;" + f"visualize-trajectory: {1000 * self.time_visualize / self.count:.2f}ms") + def warm_up(self): depth = torch.zeros((1, 1, self.height, self.width), dtype=torch.float32, device=self.device) obs = torch.zeros((1, 9), dtype=torch.float32, device=self.device)