Improve simulator real-time performance

Controller/src/so3_quadrotor_simulator/launch/simulator_attitude_control.launch

@@ -4,7 +4,7 @@
     <arg name="init_z" value="2.0"/> 
 
     <node pkg="so3_quadrotor_simulator" type="quadrotor_simulator_so3" name="quadrotor_simulator_so3" output="screen">
-        <param name="rate/odom" value="50.0"/>
+        <param name="rate/odom" value="100.0"/>
         <param name="simulator/init_state_x" value="$(arg init_x)"/>
         <param name="simulator/init_state_y" value="$(arg init_y)"/>
         <param name="simulator/init_state_z" value="$(arg init_z)"/>

Controller/src/so3_quadrotor_simulator/launch/simulator_position_control.launch

@@ -4,7 +4,7 @@
     <arg name="init_z" value="2.0"/> 
 
     <node pkg="so3_quadrotor_simulator" type="quadrotor_simulator_so3" name="quadrotor_simulator_so3" output="screen">
-        <param name="rate/odom" value="50.0"/>
+        <param name="rate/odom" value="100.0"/>
         <param name="simulator/init_state_x" value="$(arg init_x)"/>
         <param name="simulator/init_state_y" value="$(arg init_y)"/>
         <param name="simulator/init_state_z" value="$(arg init_z)"/>

Simulator/src/config/config.yaml

@@ -4,7 +4,7 @@ depth_topic: "/depth_image"
 lidar_topic: "/lidar_points"
 
 render_depth: true
-depth_fps: 30
+depth_fps: 33          # odom 100hz, depth 33hz, 每3帧odom渲染一次
 
 render_lidar: true
 lidar_fps: 10

Simulator/src/src/test_simulator_cuda.cpp

@@ -50,6 +50,8 @@ public:
         render_depth = config["render_depth"].as<bool>();
         float depth_fps = config["depth_fps"].as<float>();
         float lidar_fps = config["lidar_fps"].as<float>();
+        depth_pub_duration = ros::Duration(1 / depth_fps);
+        lidar_pub_duration = ros::Duration(1 / lidar_fps);
         
         std::string ply_file = config["ply_file"].as<std::string>();
         std::string odom_topic = config["odom_topic"].as<std::string>();
@@ -99,13 +101,14 @@ public:
         std::cout<<"Pointloud size:"<<cloud->points.size()<<std::endl;
         printf("2.Mapping... \n");
         grid_map = new GridMap(cloud, resolution, occupy_threshold);
+        
+        ros::Time next_depth_pub_time = ros::Time::now();
+        ros::Time next_lidar_pub_time = ros::Time::now();
 
         // ROS
         image_pub_ = nh_.advertise<sensor_msgs::Image>(depth_topic, 1);
         point_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(lidar_topic, 1);
         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);
 
         printf("3.Simulation Ready! \n");
@@ -114,16 +117,15 @@ public:
 
     void odomCallback(const nav_msgs::Odometry::ConstPtr &msg);
 
-    void timerDepthCallback(const ros::TimerEvent &);
+    void renderDepthCallback(const ros::Time stamp);
 
-    void timerLidarCallback(const ros::TimerEvent &);
+    void renderLidarCallback(const ros::Time stamp);
 
     void timerMapCallback(const ros::TimerEvent &);
 
 private:
     bool render_depth{false};
     bool render_lidar{false};
-    bool odom_init{false};
     Eigen::Quaternionf quat;
     Eigen::Quaternionf quat_bc, quat_wc;
     Eigen::Vector3f pos;
@@ -139,13 +141,17 @@ private:
     ros::Subscriber odom_sub_;
     ros::Timer timer_depth_, timer_lidar_, timer_map_;
 
+    ros::Time next_depth_pub_time, next_lidar_pub_time;
+    ros::Duration depth_pub_duration, lidar_pub_duration;
+    double depth_time{0.0}, lidar_time{0.0};
+    int depth_count{0}, lidar_count{0};
     // mocka::Maps map;
 };
 
 
 
-void SensorSimulator::timerDepthCallback(const ros::TimerEvent&) {
+void SensorSimulator::renderDepthCallback(const ros::Time stamp) {
-    if (!odom_init || !render_depth)
+    if (!render_depth)
         return;
 
     auto start = std::chrono::high_resolution_clock::now();
@@ -156,11 +162,13 @@ void SensorSimulator::timerDepthCallback(const ros::TimerEvent&) {
     
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> elapsed = end - start;
+    depth_time += elapsed.count();
+    depth_count++;
     // std::cout << "生成图像耗时: " << elapsed.count() << " 秒" << std::endl;
 
     sensor_msgs::Image ros_image;
     cv_bridge::CvImage cv_image;
-    cv_image.header.stamp = ros::Time::now();
+    cv_image.header.stamp = stamp;
     cv_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     cv_image.image = depth_image;
     cv_image.toImageMsg(ros_image);
@@ -172,9 +180,8 @@ void SensorSimulator::timerMapCallback(const ros::TimerEvent&) {
         pcl_pub.publish(output);    
 }
 
-
+void SensorSimulator::renderLidarCallback(const ros::Time stamp) {
-void SensorSimulator::timerLidarCallback(const ros::TimerEvent&) {
+    if (!render_lidar)
-    if (!odom_init || !render_lidar)
         return;
 
     auto start = std::chrono::high_resolution_clock::now();
@@ -185,11 +192,13 @@ void SensorSimulator::timerLidarCallback(const ros::TimerEvent&) {
     
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> elapsed = end - start;
+    lidar_time += elapsed.count();
+    lidar_count++;
     // std::cout << "生成雷达耗时: " << elapsed.count() << " 秒" << std::endl;
 
     sensor_msgs::PointCloud2 output;
     pcl::toROSMsg(lidar_points, output);
-    output.header.stamp = ros::Time::now();
+    output.header.stamp = stamp;
     output.header.frame_id = "odom";
     point_cloud_pub_.publish(output);
 }
@@ -205,7 +214,29 @@ void SensorSimulator::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
     pos.y() = msg->pose.pose.position.y;
     pos.z() = msg->pose.pose.position.z;
 
-    odom_init = true;
+    ros::Time tnow = ros::Time::now();
+
+    // 避免仿真odom消息中断，导致时间差太大
+    if (fabs((tnow - next_depth_pub_time).toSec()) > 10 * depth_pub_duration.toSec())
+        next_depth_pub_time = tnow;
+    if (fabs((tnow - next_lidar_pub_time).toSec()) > 10 * lidar_pub_duration.toSec())
+        next_lidar_pub_time = tnow;
+
+    if (tnow >= next_depth_pub_time){
+        next_depth_pub_time += depth_pub_duration;
+        renderDepthCallback(msg->header.stamp);
+    }
+    if (tnow >= next_lidar_pub_time){
+        next_lidar_pub_time += lidar_pub_duration;
+        renderLidarCallback(msg->header.stamp);
+    }
+    ros::Duration render_duration = ros::Time::now() - tnow;
+    if (render_duration > depth_pub_duration || render_duration > lidar_pub_duration){
+        // Performance reference: should take < 1 ms on 3060 GPU & Ubuntu 20.04
+        ROS_WARN("Current Rendering time: %.2f ms, delay too much!", 1000 * render_duration.toSec());
+        std::cout << "Average Depth Rendering time: " << (depth_time / depth_count) * 1000 << " ms" << std::endl;
+        std::cout << "Average Lidar Rendering time: " << (lidar_time / lidar_count) * 1000 << " ms" << std::endl;
+    }
 }
 
 int main(int argc, char** argv) {

YOPO/test_yopo_ros.py

@@ -92,8 +92,8 @@ class YopoNet:
         self.all_trajs_pub = rospy.Publisher("/yopo_net/trajs_visual", PointCloud2, queue_size=1)
         self.ctrl_pub = rospy.Publisher(self.config["ctrl_topic"], PositionCommand, queue_size=1)
         # ros subscriber
-        self.odom_sub = rospy.Subscriber(self.config['odom_topic'], Odometry, self.callback_odometry, queue_size=1)
+        self.odom_sub = rospy.Subscriber(self.config['odom_topic'], Odometry, self.callback_odometry, queue_size=1, tcp_nodelay=True)
-        self.depth_sub = rospy.Subscriber(self.config['depth_topic'], Image, self.callback_depth, queue_size=1)
+        self.depth_sub = rospy.Subscriber(self.config['depth_topic'], Image, self.callback_depth, queue_size=1, tcp_nodelay=True)
         self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.callback_set_goal, queue_size=1)
         # ros timer
         rospy.sleep(1.0)  # wait connection...