Skip to content

Commit

Permalink
ROS2 changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@wouter-heerwegh
wouter-heerwegh committed Oct 1, 2022
1 parent e3f3cc5 commit bf3b413
Show file tree
Hide file tree
Showing 2 changed files with 121 additions and 25 deletions.
37 changes: 37 additions & 0 deletions ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -113,6 +148,8 @@ class AirsimROSWrapper
// create std::vector<Statistics*> which I can use to iterate over all these options
// and apply common operations such as print, reset
// std::vector<ros_bridge::Statistics*> statistics_obj_ptr;

enabledSensors enabled_sensors;

// Print all statistics
void PrintStatistics();
Expand Down
109 changes: 84 additions & 25 deletions ros2/src/fsds_ros2_bridge/src/airsim_ros_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@ AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr<rclcpp::Node>& 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!");
}
Expand All @@ -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};
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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.");
Expand All @@ -193,9 +220,19 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()

vehicle_name = curr_vehicle_name;
clock_pub = nh_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 10);
global_gps_pub = nh_->create_publisher<sensor_msgs::msg::NavSatFix>("gps", 10);
imu_pub = nh_->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
gss_pub = nh_->create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>("gss", 10);

if(enabled_sensors.gps){
global_gps_pub = nh_->create_publisher<sensor_msgs::msg::NavSatFix>("gps", 10);
}

if(enabled_sensors.imu){
imu_pub = nh_->create_publisher<sensor_msgs::msg::Imu>("imu", 10);
}

if(enabled_sensors.gss){
gss_pub = nh_->create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>("gss", 10);
}

wheel_states_pub = nh_->create_publisher<fs_msgs::msg::WheelStates>("wheel_states", 10);

bool UDP_control;
Expand Down Expand Up @@ -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)
{
Expand All @@ -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)
{
Expand Down

0 comments on commit bf3b413

Please sign in to comment.