0909:修复图像延迟问题，修复imu频率受控制命令影响的问题;非稳定版本

RoboMaster · Sep 9, 2022 · f14cc9c · f14cc9c
1 parent fd8b636
commit f14cc9c
Show file tree

Hide file tree

Showing 5 changed files with 499 additions and 484 deletions.
diff --git a/README.md b/README.md
@@ -1,7 +1,8 @@
 # __自主无人机竞速模拟器使用说明__  
 1. ## 更新说明
-### 0907: 
-   1. 修复imu帧率下降，未经稳定性测试，非正式版；
+### 0909-未经稳定性测试，非正式版: 
+   1. 修复imu帧率下降问题；
+   2. 修复图像时间戳延迟问题；
    ---
 ### 0830: 
    1. 相机配置文件中的x, y, z 数值 乘以二， 纠正因提及缩放导致的误差；
@@ -64,7 +65,7 @@
 
    5. ## WIN 端下载并启动模拟器
     >在浏览器中输入此网址即可开始下载:    
-    `https://stg-robomasters-hz-q0o2.oss-cn-hangzhou.aliyuncs.com/simulator/simulator_WIN.zip`
+    `https://stg-robomasters-hz-q0o2.oss-cn-hangzhou.aliyuncs.com/simulator/simulator_WIN_0909.zip`
     ---
     >解压后进入文件夹中，参考simulator_WIN.md 启动模拟器  
     ![pic](doc/2022-08-18%2015-26-03%20的屏幕截图.png)

diff --git a/roswrapper/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/roswrapper/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h
@@ -172,11 +172,11 @@ class AirsimROSWrapper
     ros::AsyncSpinner img_RGBD_async_spinner_;
     ros::AsyncSpinner img_stereo_async_spinner_;
     ros::AsyncSpinner img_bottom_async_spinner_;
-    ros::AsyncSpinner lidar_async_spinner_;
+    //ros::AsyncSpinner lidar_async_spinner_;
     ros::AsyncSpinner drone_state_async_spinner_;
     ros::AsyncSpinner command_listener_async_spinner_;
-    ros::AsyncSpinner update_commands_async_spinner_;
-    bool is_used_lidar_timer_cb_queue_;
+    // ros::AsyncSpinner update_commands_async_spinner_;
+    // bool is_used_lidar_timer_cb_queue_;
     bool is_used_img_timer_cb_queue_;
 
 private:
@@ -265,28 +265,28 @@ class AirsimROSWrapper
     void img_response_RGBD_timer_cb(const ros::TimerEvent& event); // update images from airsim_client_ every nth sec
     void img_response_stereo_timer_cb(const ros::TimerEvent& event); // update images from airsim_client_ every nth sec
     void drone_state_timer_cb(const ros::TimerEvent& event); // update drone state from airsim_client_ every nth sec
-    void lidar_timer_cb(const ros::TimerEvent& event);
+    // void lidar_timer_cb(const ros::TimerEvent& event);
 
     /// ROS subscriber callbacks
     void pose_cmd_body_frame_cb(const airsim_ros_pkgs::PoseCmd::ConstPtr& msg, const std::string& vehicle_name);
-    void vel_cmd_world_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name);
+    // void vel_cmd_world_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name);
     void vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name);
 
     void angle_rate_throttle_frame_cb(const airsim_ros_pkgs::AngleRateThrottle::ConstPtr& msg, const std::string& vehicle_name);
 
-    void vel_cmd_group_body_frame_cb(const airsim_ros_pkgs::VelCmdGroup& msg);
-    void vel_cmd_group_world_frame_cb(const airsim_ros_pkgs::VelCmdGroup& msg);
+    // void vel_cmd_group_body_frame_cb(const airsim_ros_pkgs::VelCmdGroup& msg);
+    // void vel_cmd_group_world_frame_cb(const airsim_ros_pkgs::VelCmdGroup& msg);
 
-    void vel_cmd_all_world_frame_cb(const airsim_ros_pkgs::VelCmd& msg);
-    void vel_cmd_all_body_frame_cb(const airsim_ros_pkgs::VelCmd& msg);
+    // void vel_cmd_all_world_frame_cb(const airsim_ros_pkgs::VelCmd& msg);
+    // void vel_cmd_all_body_frame_cb(const airsim_ros_pkgs::VelCmd& msg);
 
     // void vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd& msg, const std::string& vehicle_name);
     void gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAngleQuatCmd& gimbal_angle_quat_cmd_msg);
     void gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAngleEulerCmd& gimbal_angle_euler_cmd_msg);
 
     // commands
     void car_cmd_cb(const airsim_ros_pkgs::CarControls::ConstPtr& msg, const std::string& vehicle_name);
-    void update_commands(const ros::TimerEvent& event);
+    // void update_commands(const ros::TimerEvent& event);
 
     // state, returns the simulation timestamp best guess based on drone state timestamp, airsim needs to return timestap for environment
     ros::Time update_state();
@@ -295,16 +295,16 @@ class AirsimROSWrapper
 
     /// ROS service callbacks
     bool takeoff_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response, const std::string& vehicle_name);
-    bool takeoff_group_srv_cb(airsim_ros_pkgs::TakeoffGroup::Request& request, airsim_ros_pkgs::TakeoffGroup::Response& response);
-    bool takeoff_all_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response);
+    // bool takeoff_group_srv_cb(airsim_ros_pkgs::TakeoffGroup::Request& request, airsim_ros_pkgs::TakeoffGroup::Response& response);
+    // bool takeoff_all_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response);
     bool land_srv_cb(airsim_ros_pkgs::Land::Request& request, airsim_ros_pkgs::Land::Response& response, const std::string& vehicle_name);
-    bool land_group_srv_cb(airsim_ros_pkgs::LandGroup::Request& request, airsim_ros_pkgs::LandGroup::Response& response);
-    bool land_all_srv_cb(airsim_ros_pkgs::Land::Request& request, airsim_ros_pkgs::Land::Response& response);
-    bool reset_srv_cb(airsim_ros_pkgs::Reset::Request& request, airsim_ros_pkgs::Reset::Response& response);
+    // bool land_group_srv_cb(airsim_ros_pkgs::LandGroup::Request& request, airsim_ros_pkgs::LandGroup::Response& response);
+    // bool land_all_srv_cb(airsim_ros_pkgs::Land::Request& request, airsim_ros_pkgs::Land::Response& response);
+    // bool reset_srv_cb(airsim_ros_pkgs::Reset::Request& request, airsim_ros_pkgs::Reset::Response& response);
 
     /// ROS tf broadcasters
-    void publish_camera_tf(const ImageResponse& img_response, const ros::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id);
-    void publish_odom_tf(const nav_msgs::Odometry& odom_msg);
+    // void publish_camera_tf(const ImageResponse& img_response, const ros::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id);
+    // void publish_odom_tf(const nav_msgs::Odometry& odom_msg);
 
     /// camera helper methods
     sensor_msgs::CameraInfo generate_cam_info(const std::string& camera_name, const CameraSetting& camera_setting, const CaptureSetting& capture_setting) const;
@@ -361,16 +361,16 @@ class AirsimROSWrapper
     std::string host_ip_;
 
     // subscriber / services for ALL robots
-    ros::Subscriber vel_cmd_all_body_frame_sub_;
-    ros::Subscriber vel_cmd_all_world_frame_sub_;
-    ros::ServiceServer takeoff_all_srvr_;
-    ros::ServiceServer land_all_srvr_;
+    // ros::Subscriber vel_cmd_all_body_frame_sub_;
+    // ros::Subscriber vel_cmd_all_world_frame_sub_;
+    // ros::ServiceServer takeoff_all_srvr_;
+    // ros::ServiceServer land_all_srvr_;
 
     // todo - subscriber / services for a GROUP of robots, which is defined by a list of `vehicle_name`s passed in the ros msg / srv request
-    ros::Subscriber vel_cmd_group_body_frame_sub_;
-    ros::Subscriber vel_cmd_group_world_frame_sub_;
-    ros::ServiceServer takeoff_group_srvr_;
-    ros::ServiceServer land_group_srvr_;
+    // ros::Subscriber vel_cmd_group_body_frame_sub_;
+    // ros::Subscriber vel_cmd_group_world_frame_sub_;
+    // ros::ServiceServer takeoff_group_srvr_;
+    // ros::ServiceServer land_group_srvr_;
 
     AIRSIM_MODE airsim_mode_ = AIRSIM_MODE::DRONE;
 
@@ -389,19 +389,19 @@ class AirsimROSWrapper
     // seperate busy connections to airsim, update in their own thread
     msr::airlib::RpcLibClientBase airsim_client_images_;
     msr::airlib::MultirotorRpcLibClient airsim_client_states_;
-     msr::airlib::RpcLibClientBase airsim_client_lidar_;
+    // msr::airlib::RpcLibClientBase airsim_client_lidar_;
 
     // todo not sure if async spinners shuold be inside this class, or should be instantiated in airsim_node.cpp, and cb queues should be public
     // todo for multiple drones with multiple sensors, this won't scale. make it a part of VehicleROS?
     ros::CallbackQueue img_timer_cb_queue_;
     ros::CallbackQueue img_timer_cb_queue_RGBD_;
     ros::CallbackQueue img_timer_cb_queue_stereo_;
     ros::CallbackQueue img_timer_cb_queue_bottom_;
-    ros::CallbackQueue lidar_timer_cb_queue_;
+    // ros::CallbackQueue lidar_timer_cb_queue_;
     ros::CallbackQueue drone_state_timer_cb_queue_;
     ros::CallbackQueue command_listener_queue_;
     // ros::CallbackQueue default_queue_;
-    ros::CallbackQueue update_commands_queue_;
+    // ros::CallbackQueue update_commands_queue_;
 
     bool img_cb_flag = 0;
 
@@ -433,8 +433,8 @@ class AirsimROSWrapper
     ros::Timer airsim_img_response_RGBD_timer_;
     ros::Timer airsim_img_response_stereo_timer_;
     ros::Timer airsim_control_update_timer_;
-    ros::Timer airsim_control_update_timer2_;
-    ros::Timer airsim_lidar_update_timer_;
+    // ros::Timer airsim_control_update_timer2_;
+    // ros::Timer airsim_lidar_update_timer_;
 
     typedef std::pair<std::vector<ImageRequest>, std::string> airsim_img_request_vehicle_name_pair;
     std::vector<airsim_img_request_vehicle_name_pair> airsim_img_request_vehicle_name_pair_vec_;

diff --git a/roswrapper/ros/src/airsim_ros_pkgs/launch/airsim_node.launch b/roswrapper/ros/src/airsim_ros_pkgs/launch/airsim_node.launch
@@ -47,13 +47,13 @@
 	<node name="airsim_node" pkg="airsim_ros_pkgs" type="airsim_node" output="$(arg output)">
 		<param name="is_vulkan" type="bool" value="false" /> 
 		<!-- ROS timer rates. Note that timer callback will be processed at maximum possible rate, upperbounded by the following ROS params -->
-		<param name="update_airsim_img_response_every_n_sec" type="double" value="0.05" /> 
+		<param name="update_airsim_img_response_every_n_sec" type="double" value="0.050" /> 
 		<param name="update_airsim_control_every_n_sec" type="double" value="0.01" />
-		<param name="update_lidar_every_n_sec" type="double" value="0.01" />
+		<param name="update_lidar_every_n_sec" type="double" value="1" />
 		<param name="publish_clock" type="bool" value="$(arg publish_clock)" />
 		<param name="host_ip" type="string" value="$(arg host_ip)" />
 	</node>
 
 	<!-- Static transforms -->
-	<!-- include file="$(find airsim_ros_pkgs)/launch/static_transforms.launch"/-->
+	<!--include file="$(find airsim_ros_pkgs)/launch/static_transforms.launch"/-->
 </launch>
diff --git a/roswrapper/ros/src/airsim_ros_pkgs/src/airsim_node.cpp b/roswrapper/ros/src/airsim_ros_pkgs/src/airsim_node.cpp
@@ -14,7 +14,6 @@ int main(int argc, char** argv)
     nh_private.getParam("host_ip", host_ip);
     AirsimROSWrapper airsim_ros_wrapper(nh, nh_private, host_ip);
     airsim_ros_wrapper.drone_state_async_spinner_.start();
-    airsim_ros_wrapper.update_commands_async_spinner_.start();
     airsim_ros_wrapper.command_listener_async_spinner_.start();
     if (airsim_ros_wrapper.is_used_img_timer_cb_queue_) {
         if(airsim_ros_wrapper.is_RGBD_)
@@ -30,9 +29,6 @@ int main(int argc, char** argv)
         airsim_ros_wrapper.img_bottom_async_spinner_.start();
     }
 
-    if (airsim_ros_wrapper.is_used_lidar_timer_cb_queue_) {
-        airsim_ros_wrapper.lidar_async_spinner_.start();
-    }
 
     ros::waitForShutdown();
     return 0;