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) {