Merge pull request #21 from RoboMaster/develop

Develop
RoboMaster · Sep 17, 2022 · 42df687 · 42df687
2 parents bc3f06f + 1c5bcdb
commit 42df687
Show file tree

Hide file tree

Showing 5 changed files with 633 additions and 557 deletions.
diff --git a/README.md b/README.md
@@ -1,5 +1,13 @@
 # __自主无人机竞速模拟器使用说明__  
 1. ## 更新说明
+### v1.1.1 
+   1. 模拟器版本更新 -> v1.1.1; 赛道一， 赛道二添加了视觉标定板， 具体参数参考模拟器根目录README文件; 赛道二动态圈发生碰撞时会停止，防止卡主无人机；
+   2. roswrapper限制了控制命令的发布频率为100hz；
+
+### v1.1.0-未经稳定性测试，非正式版: 
+   1. 修复imu帧率下降问题；
+   2. 修复图像时间戳延迟问题；
+   ---
 ### 0830: 
    1. 相机配置文件中的x, y, z 数值 乘以二， 纠正因提及缩放导致的误差；
    2. FOV由100度改为90度；
@@ -61,7 +69,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_V1_1_1.zip`
     ---
     >解压后进入文件夹中，参考simulator_WIN.md 启动模拟器  
     ![pic](doc/2022-08-18%2015-26-03%20的屏幕截图.png)
@@ -75,7 +83,7 @@
 
 3. ## LINUX + LINUX 双机开发模式
     ### 简介
-    双击模式分离后免除了ros 端程序对模拟器的影响，使得开发更便利。但是 Linux端运行模拟器对帧率有较大的影响。使用 Linux 端 clone此仓库。
+    双机模式分离后免除了ros 端程序对模拟器的影响，使得开发更便利。但是 Linux端运行模拟器对帧率有较大的影响。使用 Linux 端 clone此仓库。
     ### 使用说明
    1. ## A LINUX 端安装ROS noetic
     >设置软件库   
@@ -112,7 +120,7 @@
 
    5. ## B LINUX 端下载并启动模拟器
     >在浏览器中输入此网址即可开始下载:    
-    `https://stg-robomasters-hz-q0o2.oss-cn-hangzhou.aliyuncs.com/simulator/simulator_LINUX.zip`
+    `https://stg-robomasters-hz-q0o2.oss-cn-hangzhou.aliyuncs.com/simulator/simulator_LINUX_V1_1_1.zip`
     ---
     >解压后进入文件夹中，参考simulator_LINUX.md 启动模拟器  
     ![pic](doc/2022-08-18%2015-35-08%20%E7%9A%84%E5%B1%8F%E5%B9%95%E6%88%AA%E5%9B%BE.png)
@@ -163,7 +171,7 @@
 
    5. ## 下载模拟器
     >在浏览器中输入此网址即可开始下载:    
-    `https://stg-robomasters-hz-q0o2.oss-cn-hangzhou.aliyuncs.com/simulator/simulator_LINUX.zip`
+    `https://stg-robomasters-hz-q0o2.oss-cn-hangzhou.aliyuncs.com/simulator/simulator_LINUX_V1_1_1.zip`
     ---
     >解压后进入文件夹中，参考simulator_LINUX.md 启动模拟器  
     ![pic](doc/2022-08-18%2015-35-08%20%E7%9A%84%E5%B1%8F%E5%B9%95%E6%88%AA%E5%9B%BE.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
@@ -135,7 +135,7 @@ struct GimbalCmd
     // GimbalCmd(const std::string& vehicle_name,
     //         const std::string& camera_name,
     //         const msr::airlib::Quaternionr& target_quat) :
-    //         vehicle_name(vehicle_name), camera_name(camera_name), target_quat(target_quat) {};AirsimROSWrapper
+    //         vehicle_name(vehicle_name), camera_name(camera_name), target_quat(target_quat) {};
 };
 
 class AirsimROSWrapper
@@ -172,8 +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_;
-    bool is_used_lidar_timer_cb_queue_;
+    //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_;
     bool is_used_img_timer_cb_queue_;
 
 private:
@@ -262,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();
+    // 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();
@@ -292,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;
@@ -358,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;
 
@@ -385,15 +388,20 @@ class AirsimROSWrapper
     std::unique_ptr<msr::airlib::RpcLibClientBase> airsim_client_ = nullptr;
     // seperate busy connections to airsim, update in their own thread
     msr::airlib::RpcLibClientBase airsim_client_images_;
-    msr::airlib::RpcLibClientBase airsim_client_lidar_;
+    msr::airlib::MultirotorRpcLibClient airsim_client_states_;
+    // 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_;
 
     bool img_cb_flag = 0;
 
@@ -418,14 +426,17 @@ class AirsimROSWrapper
 
     /// ROS params
     double vel_cmd_duration_;
-
+
+    long long last_cmd_time;
+
     /// ROS Timers.
     ros::Timer airsim_img_response_timer_;
     ros::Timer airsim_img_response_bottom_timer_;
     ros::Timer airsim_img_response_RGBD_timer_;
     ros::Timer airsim_img_response_stereo_timer_;
     ros::Timer airsim_control_update_timer_;
-    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
@@ -13,7 +13,8 @@ int main(int argc, char** argv)
     std::string host_ip = "localhost";
     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.command_listener_async_spinner_.start();
     if (airsim_ros_wrapper.is_used_img_timer_cb_queue_) {
         if(airsim_ros_wrapper.is_RGBD_)
         {
@@ -28,11 +29,7 @@ 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::spin();
 
+    ros::waitForShutdown();
     return 0;
 }