From e3f3cc5c56f9053649b19e7bc6cbfaebd8c77470 Mon Sep 17 00:00:00 2001 From: Wouter Heerwegh Date: Thu, 29 Sep 2022 20:30:18 +0200 Subject: [PATCH 1/2] Make it possible to disable gss, imu or gps --- .../include/airsim_ros_wrapper.h | 37 +++++++ .../src/airsim_ros_wrapper.cpp | 104 +++++++++++++----- 2 files changed, 113 insertions(+), 28 deletions(-) diff --git a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h index a3cf7efb..acfceefe 100644 --- a/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h +++ b/ros/src/fsds_ros_bridge/include/airsim_ros_wrapper.h @@ -71,6 +71,41 @@ struct SimpleMatrix } }; +struct enabledSensors { + bool lidar = false; + bool camera = false; + bool gps = false; + bool gss = false; + bool imu = false; + + void print() { + std::stringstream ss; + std::cout << "Printing enabled sensors:" << std::endl; + + if(lidar){ + ss << "Lidar enabled\n"; + } + + if(camera){ + ss << "Camera enabled\n"; + } + + if(gps){ + ss << "GPS enabled\n"; + } + + if(gss){ + ss << "GSS enabled\n"; + } + + if(imu){ + ss << "IMU enabled\n"; + } + + std::cout << ss.str(); + } +}; + class AirsimROSWrapper { public: @@ -109,6 +144,8 @@ class AirsimROSWrapper ros_bridge::Statistics imu_pub_statistics; ros_bridge::Statistics gss_pub_statistics; + enabledSensors enabled_sensors; + // create std::vector which I can use to iterate over all these options // and apply common operations such as print, reset // std::vector statistics_obj_ptr; diff --git a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp index 0ad18511..c235939a 100644 --- a/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp +++ b/ros/src/fsds_ros_bridge/src/airsim_ros_wrapper.cpp @@ -12,9 +12,7 @@ AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHan { initialize_airsim(timeout_sec); try { - ROS_INFO_STREAM("READING SETTINGS"); std::string settings_text = airsim_client_.getSettingsString(); - ROS_INFO_STREAM("SETTINGS READ"); msr::airlib::AirSimSettings::initializeSettings(settings_text); msr::airlib::AirSimSettings::singleton().load(); @@ -31,8 +29,8 @@ AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHan throw std::invalid_argument(std::string("Failed loading settings.json.") + ex.what()); } - initialize_statistics(); initialize_ros(); + initialize_statistics(); ROS_INFO_STREAM("AirsimROSWrapper Initialized!"); } @@ -58,15 +56,26 @@ void AirsimROSWrapper::initialize_airsim(double timeout) void AirsimROSWrapper::initialize_statistics() { setCarControlsStatistics = ros_bridge::Statistics("setCarControls"); - getGpsDataStatistics = ros_bridge::Statistics("getGpsData"); + + if(enabled_sensors.gps){ + getGpsDataStatistics = ros_bridge::Statistics("getGpsData"); + global_gps_pub_statistics = ros_bridge::Statistics("global_gps_pub"); + } + getCarStateStatistics = ros_bridge::Statistics("getCarState"); - getImuStatistics = ros_bridge::Statistics("getImu"); - getGSSStatistics = ros_bridge::Statistics("getGSS"); + + if(enabled_sensors.imu){ + getImuStatistics = ros_bridge::Statistics("getImu"); + imu_pub_statistics = ros_bridge::Statistics("imu_pub"); + } + + if(enabled_sensors.gss){ + getGSSStatistics = ros_bridge::Statistics("getGSS"); + gss_pub_statistics = ros_bridge::Statistics("gss_pub"); + } + control_cmd_sub_statistics = ros_bridge::Statistics("control_cmd_sub"); - global_gps_pub_statistics = ros_bridge::Statistics("global_gps_pub"); odom_pub_statistics = ros_bridge::Statistics("odom_pub"); - imu_pub_statistics = ros_bridge::Statistics("imu_pub"); - gss_pub_statistics = ros_bridge::Statistics("gss_pub"); // Populate statistics obj vector // statistics_obj_ptr = {&setCarControlsStatistics, &getGpsDataStatistics, &getCarStateStatistics, &control_cmd_sub_statistics, &global_gps_pub_statistics, &odom_local_ned_pub_statistics}; @@ -138,10 +147,16 @@ void AirsimROSWrapper::initialize_ros() odom_update_timer_ = nh_private_.createTimer(ros::Duration(update_odom_every_n_sec), &AirsimROSWrapper::odom_cb, this); extra_info_timer_ = nh_private_.createTimer(ros::Duration(1), &AirsimROSWrapper::extra_info_cb, this); } + + if(enabled_sensors.gps) + gps_update_timer_ = nh_private_.createTimer(ros::Duration(update_gps_every_n_sec), &AirsimROSWrapper::gps_timer_cb, this); + + if(enabled_sensors.imu) + imu_update_timer_ = nh_private_.createTimer(ros::Duration(update_imu_every_n_sec), &AirsimROSWrapper::imu_timer_cb, this); + + if(enabled_sensors.gss) + gss_update_timer_ = nh_private_.createTimer(ros::Duration(update_gss_every_n_sec), &AirsimROSWrapper::gss_timer_cb, this); - gps_update_timer_ = nh_private_.createTimer(ros::Duration(update_gps_every_n_sec), &AirsimROSWrapper::gps_timer_cb, this); - imu_update_timer_ = nh_private_.createTimer(ros::Duration(update_imu_every_n_sec), &AirsimROSWrapper::imu_timer_cb, this); - gss_update_timer_ = nh_private_.createTimer(ros::Duration(update_gss_every_n_sec), &AirsimROSWrapper::gss_timer_cb, this); statictf_timer_ = nh_private_.createTimer(ros::Duration(publish_static_tf_every_n_sec), &AirsimROSWrapper::statictf_cb, this); statistics_timer_ = nh_private_.createTimer(ros::Duration(1), &AirsimROSWrapper::statistics_timer_cb, this); @@ -179,10 +194,22 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() set_nans_to_zeros_in_pose(*vehicle_setting); + enabled_sensors.gss = (vehicle_setting->sensors.find("GSS") != vehicle_setting->sensors.end()); + enabled_sensors.gps = (vehicle_setting->sensors.find("Gps") != vehicle_setting->sensors.end()); + enabled_sensors.imu = (vehicle_setting->sensors.find("Imu") != vehicle_setting->sensors.end()); + + enabled_sensors.print(); + vehicle_name = curr_vehicle_name; - global_gps_pub = nh_.advertise("gps", 10); - imu_pub = nh_.advertise("imu", 10); - gss_pub = nh_.advertise("gss", 10); + + if(enabled_sensors.gps) + global_gps_pub = nh_.advertise("gps", 10); + + if(enabled_sensors.imu) + imu_pub = nh_.advertise("imu", 10); + + if(enabled_sensors.gss) + gss_pub = nh_.advertise("gss", 10); bool UDP_control; nh_private_.getParam("UDP_control", UDP_control); @@ -712,16 +739,27 @@ void AirsimROSWrapper::PrintStatistics() std::stringstream dbg_msg; dbg_msg << "--------- ros_wrapper statistics ---------" << std::endl; dbg_msg << setCarControlsStatistics.getSummaryAsString() << std::endl; - dbg_msg << getGpsDataStatistics.getSummaryAsString() << std::endl; + + if(enabled_sensors.gps){ + dbg_msg << getGpsDataStatistics.getSummaryAsString() << std::endl; + dbg_msg << global_gps_pub_statistics.getSummaryAsString() << std::endl; + } + dbg_msg << getCarStateStatistics.getSummaryAsString() << std::endl; - dbg_msg << getImuStatistics.getSummaryAsString() << std::endl; - dbg_msg << getGSSStatistics.getSummaryAsString() << std::endl; + + if(enabled_sensors.imu){ + dbg_msg << getImuStatistics.getSummaryAsString() << std::endl; + dbg_msg << imu_pub_statistics.getSummaryAsString() << std::endl; + } + + if(enabled_sensors.gss){ + dbg_msg << getGSSStatistics.getSummaryAsString() << std::endl; + dbg_msg << gss_pub_statistics.getSummaryAsString() << std::endl; + } + dbg_msg << control_cmd_sub_statistics.getSummaryAsString() << std::endl; - dbg_msg << global_gps_pub_statistics.getSummaryAsString() << std::endl; dbg_msg << odom_pub_statistics.getSummaryAsString() << std::endl; - dbg_msg << imu_pub_statistics.getSummaryAsString() << std::endl; - dbg_msg << gss_pub_statistics.getSummaryAsString() << std::endl; - + for (auto &getLidarDataStatistics : getLidarDataVecStatistics) { dbg_msg << getLidarDataStatistics.getSummaryAsString() << std::endl; @@ -744,15 +782,25 @@ void AirsimROSWrapper::ResetStatistics() // statistics_obj->Reset(); // } setCarControlsStatistics.Reset(); - getGpsDataStatistics.Reset(); + + if(enabled_sensors.gps){ + getGpsDataStatistics.Reset(); + global_gps_pub_statistics.Reset(); + } getCarStateStatistics.Reset(); - getImuStatistics.Reset(); - getGSSStatistics.Reset(); + + if(enabled_sensors.imu){ + getImuStatistics.Reset(); + imu_pub_statistics.Reset(); + } + + if(enabled_sensors.gss){ + getGSSStatistics.Reset(); + gss_pub_statistics.Reset(); + } + control_cmd_sub_statistics.Reset(); - global_gps_pub_statistics.Reset(); odom_pub_statistics.Reset(); - imu_pub_statistics.Reset(); - gss_pub_statistics.Reset(); for (auto &getLidarDataStatistics : getLidarDataVecStatistics) { From bf3b413ef8e476a68df939e8c3799620af9a727d Mon Sep 17 00:00:00 2001 From: Wouter Heerwegh Date: Sat, 1 Oct 2022 11:34:16 +0200 Subject: [PATCH 2/2] ROS2 changes --- .../include/airsim_ros_wrapper.h | 37 ++++++ .../src/airsim_ros_wrapper.cpp | 109 ++++++++++++++---- 2 files changed, 121 insertions(+), 25 deletions(-) diff --git a/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h b/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h index f7273a85..e7c620d1 100644 --- a/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h +++ b/ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h @@ -60,6 +60,41 @@ typedef msr::airlib::AirSimSettings::CameraSetting CameraSetting; typedef msr::airlib::AirSimSettings::LidarSetting LidarSetting; typedef msr::airlib::CarApiBase CarApiBase; +struct enabledSensors { + bool lidar = false; + bool camera = false; + bool gps = false; + bool gss = false; + bool imu = false; + + void print() { + std::stringstream ss; + std::cout << "Printing enabled sensors:" << std::endl; + + if(lidar){ + ss << "Lidar enabled\n"; + } + + if(camera){ + ss << "Camera enabled\n"; + } + + if(gps){ + ss << "GPS enabled\n"; + } + + if(gss){ + ss << "GSS enabled\n"; + } + + if(imu){ + ss << "IMU enabled\n"; + } + + std::cout << ss.str(); + } +}; + struct SimpleMatrix { int rows; @@ -113,6 +148,8 @@ class AirsimROSWrapper // create std::vector which I can use to iterate over all these options // and apply common operations such as print, reset // std::vector statistics_obj_ptr; + + enabledSensors enabled_sensors; // Print all statistics void PrintStatistics(); diff --git a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp index fd33f20e..de78a576 100644 --- a/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp +++ b/ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp @@ -31,8 +31,8 @@ AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr& nh, cons throw std::invalid_argument(std::string("Failed loading settings.json.") + ex.what()); } - initialize_statistics(); initialize_ros(); + initialize_statistics(); RCLCPP_INFO(nh_->get_logger(), "AirsimROSWrapper Initialized!"); } @@ -59,15 +59,26 @@ void AirsimROSWrapper::initialize_airsim(double timeout) void AirsimROSWrapper::initialize_statistics() { setCarControlsStatistics = ros_bridge::Statistics("setCarControls"); - getGpsDataStatistics = ros_bridge::Statistics("getGpsData"); + + if(enabled_sensors.gps){ + getGpsDataStatistics = ros_bridge::Statistics("getGpsData"); + global_gps_pub_statistics = ros_bridge::Statistics("global_gps_pub"); + } + getCarStateStatistics = ros_bridge::Statistics("getCarState"); - getImuStatistics = ros_bridge::Statistics("getImu"); - getGSSStatistics = ros_bridge::Statistics("getGSS"); + + if(enabled_sensors.imu){ + getImuStatistics = ros_bridge::Statistics("getImu"); + imu_pub_statistics = ros_bridge::Statistics("imu_pub"); + } + + if(enabled_sensors.gss){ + getGSSStatistics = ros_bridge::Statistics("getGSS"); + gss_pub_statistics = ros_bridge::Statistics("gss_pub"); + } + control_cmd_sub_statistics = ros_bridge::Statistics("control_cmd_sub"); - global_gps_pub_statistics = ros_bridge::Statistics("global_gps_pub"); odom_pub_statistics = ros_bridge::Statistics("odom_pub"); - imu_pub_statistics = ros_bridge::Statistics("imu_pub"); - gss_pub_statistics = ros_bridge::Statistics("gss_pub"); // Populate statistics obj vector // statistics_obj_ptr = {&setCarControlsStatistics, &getGpsDataStatistics, &getCarStateStatistics, &control_cmd_sub_statistics, &global_gps_pub_statistics, &odom_local_ned_pub_statistics}; @@ -149,10 +160,20 @@ void AirsimROSWrapper::initialize_ros() } clock_timer_ = nh_->create_wall_timer(dseconds{0.01}, std::bind(&AirsimROSWrapper::clock_timer_cb, this)); - gps_update_timer_ = nh_->create_wall_timer(dseconds{update_gps_every_n_sec}, std::bind(&AirsimROSWrapper::gps_timer_cb, this)); + if(enabled_sensors.gps){ + gps_update_timer_ = nh_->create_wall_timer(dseconds{update_gps_every_n_sec}, std::bind(&AirsimROSWrapper::gps_timer_cb, this)); + } + wheel_states_update_timer_ = nh_->create_wall_timer(dseconds{update_wheel_states_every_n_sec}, std::bind(&AirsimROSWrapper::wheel_states_timer_cb, this)); - imu_update_timer_ = nh_->create_wall_timer(dseconds{update_imu_every_n_sec}, std::bind(&AirsimROSWrapper::imu_timer_cb, this)); - gss_update_timer_ = nh_->create_wall_timer(dseconds{update_gss_every_n_sec}, std::bind(&AirsimROSWrapper::gss_timer_cb, this)); + + if(enabled_sensors.imu){ + imu_update_timer_ = nh_->create_wall_timer(dseconds{update_imu_every_n_sec}, std::bind(&AirsimROSWrapper::imu_timer_cb, this)); + } + + if(enabled_sensors.gss){ + gss_update_timer_ = nh_->create_wall_timer(dseconds{update_gss_every_n_sec}, std::bind(&AirsimROSWrapper::gss_timer_cb, this)); + } + statictf_timer_ = nh_->create_wall_timer(dseconds{publish_static_tf_every_n_sec}, std::bind(&AirsimROSWrapper::statictf_cb, this)); statistics_timer_ = nh_->create_wall_timer(dseconds{1}, std::bind(&AirsimROSWrapper::statistics_timer_cb, this)); @@ -184,6 +205,12 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() { auto& vehicle_setting = curr_vehicle_elem.second; auto curr_vehicle_name = curr_vehicle_elem.first; + + enabled_sensors.gss = (vehicle_setting->sensors.find("GSS") != vehicle_setting->sensors.end()); + enabled_sensors.gps = (vehicle_setting->sensors.find("Gps") != vehicle_setting->sensors.end()); + enabled_sensors.imu = (vehicle_setting->sensors.find("Imu") != vehicle_setting->sensors.end()); + + enabled_sensors.print(); if(curr_vehicle_name != "FSCar") { throw std::invalid_argument("Only the FSCar vehicle_name is allowed."); @@ -193,9 +220,19 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() vehicle_name = curr_vehicle_name; clock_pub = nh_->create_publisher("/clock", 10); - global_gps_pub = nh_->create_publisher("gps", 10); - imu_pub = nh_->create_publisher("imu", 10); - gss_pub = nh_->create_publisher("gss", 10); + + if(enabled_sensors.gps){ + global_gps_pub = nh_->create_publisher("gps", 10); + } + + if(enabled_sensors.imu){ + imu_pub = nh_->create_publisher("imu", 10); + } + + if(enabled_sensors.gss){ + gss_pub = nh_->create_publisher("gss", 10); + } + wheel_states_pub = nh_->create_publisher("wheel_states", 10); bool UDP_control; @@ -783,15 +820,26 @@ void AirsimROSWrapper::PrintStatistics() std::stringstream dbg_msg; dbg_msg << "--------- ros_wrapper statistics ---------" << std::endl; dbg_msg << setCarControlsStatistics.getSummaryAsString() << std::endl; - dbg_msg << getGpsDataStatistics.getSummaryAsString() << std::endl; + + if(enabled_sensors.gps){ + dbg_msg << getGpsDataStatistics.getSummaryAsString() << std::endl; + dbg_msg << global_gps_pub_statistics.getSummaryAsString() << std::endl; + } + dbg_msg << getCarStateStatistics.getSummaryAsString() << std::endl; - dbg_msg << getImuStatistics.getSummaryAsString() << std::endl; - dbg_msg << getGSSStatistics.getSummaryAsString() << std::endl; + + if(enabled_sensors.imu){ + dbg_msg << getImuStatistics.getSummaryAsString() << std::endl; + dbg_msg << imu_pub_statistics.getSummaryAsString() << std::endl; + } + + if(enabled_sensors.gss){ + dbg_msg << getGSSStatistics.getSummaryAsString() << std::endl; + dbg_msg << gss_pub_statistics.getSummaryAsString() << std::endl; + } + dbg_msg << control_cmd_sub_statistics.getSummaryAsString() << std::endl; - dbg_msg << global_gps_pub_statistics.getSummaryAsString() << std::endl; dbg_msg << odom_pub_statistics.getSummaryAsString() << std::endl; - dbg_msg << imu_pub_statistics.getSummaryAsString() << std::endl; - dbg_msg << gss_pub_statistics.getSummaryAsString() << std::endl; for (auto &getLidarDataStatistics : getLidarDataVecStatistics) { @@ -815,15 +863,26 @@ void AirsimROSWrapper::ResetStatistics() // statistics_obj->Reset(); // } setCarControlsStatistics.Reset(); - getGpsDataStatistics.Reset(); + + if(enabled_sensors.gps){ + getGpsDataStatistics.Reset(); + global_gps_pub_statistics.Reset(); + } + getCarStateStatistics.Reset(); - getImuStatistics.Reset(); - getGSSStatistics.Reset(); + + if(enabled_sensors.gps){ + getImuStatistics.Reset(); + imu_pub_statistics.Reset(); + } + + if(enabled_sensors.gps){ + getGSSStatistics.Reset(); + gss_pub_statistics.Reset(); + } + control_cmd_sub_statistics.Reset(); - global_gps_pub_statistics.Reset(); odom_pub_statistics.Reset(); - imu_pub_statistics.Reset(); - gss_pub_statistics.Reset(); for (auto &getLidarDataStatistics : getLidarDataVecStatistics) {