Add CI file and fix ros2 directory

.github/workflows/main.yml

@@ -11,31 +11,30 @@ on:
 
 # A workflow run is made up of one or more jobs that can run sequentially or in parallel
 jobs:
-  # This workflow contains a single job called "build"
-  build:
+  build_ros:
+    name: Build ROS1 workspace
+    strategy:
+      matrix:
+        os: [ubuntu-18.04]
+        include:
+          - os: ubuntu-18.04
+            distro: melodic
     # The type of runner that the job will run on
-    runs-on: ubuntu-18.04
-
+    runs-on: ${{ matrix.os }}
+    container:
+      image:  osrf/ros:${{ matrix.distro }}-desktop
     # Steps represent a sequence of tasks that will be executed as part of the job
     steps:
     # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it
     - uses: actions/checkout@v2
       with:
         submodules: recursive
 
-    # Runs a set of commands using the runners shell
-    - name: Setup ROS
-      uses: betwo/github-setup-catkin@v1.3.0
-      with:
-        # Version range or exact version of ROS version to use, using SemVer's version range syntax.
-        ros-version: 'melodic'
-        build-tool: 'catkin_tools'
-        # Root directory of the catkin workspace
-        workspace: ./ros
-
     - name: Compilation settings
       run: |
-        sudo apt-get -y install gcc-8 g++-8 rsync libyaml-cpp-dev libcurl4-openssl-dev
+        sudo apt-get update -y
+        sudo apt-get -y install gcc-8 g++-8 rsync libyaml-cpp-dev libcurl4-openssl-dev lsb-release wget software-properties-common gnupg 
+        sudo apt-get -y install python3-catkin-tools ros-${{ matrix.distro }}-joy ros-${{ matrix.distro }}-tf2-geometry-msgs
         sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 700 --slave /usr/bin/g++ g++ /usr/bin/g++-7
         sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8
 
@@ -47,8 +46,50 @@ jobs:
 
     - name: Build catkin ws
       run: |
+        source /opt/ros/${{ matrix.distro }}/setup.bash
         set -ex o pipefail
         catkin build 
       working-directory: ./ros 
+      shell: bash
       env:
-       ACTIONS_ALLOW_UNSECURE_COMMANDS: 'true'
+        ACTIONS_ALLOW_UNSECURE_COMMANDS: 'true'
+
+  build_ros2:
+    name: Build ROS2 workspace
+    strategy:
+      matrix:
+        compiler: [ g++, clang++ ]
+        os: [ubuntu-20.04, ubuntu-22.04]
+        include:
+          - os: ubuntu-22.04
+            distro: humble
+          - os: ubuntu-20.04
+            distro: foxy
+    # The type of runner that the job will run on
+    runs-on: ${{ matrix.os }}
+    container:      
+      image:  osrf/ros:${{ matrix.distro }}-desktop
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          submodules: recursive
+
+      - name: Compilation Settings
+        run: |
+          apt-get update -y
+          apt-get install -y rsync wget libyaml-cpp-dev clang lsb-release wget software-properties-common gnupg
+          wget -O /tmp/cmake.sh https://apt.kitware.com/kitware-archive.sh
+          chmod u+x /tmp/cmake.sh
+          /tmp/cmake.sh && rm /tmp/cmake.sh
+          apt-get update -y && apt-get install -y --no-install-recommends cmake
+      - name: Setup AirSim
+        run : |
+          ./setup.sh
+          ./build.sh
+        working-directory: AirSim
+      - name: Build ros2 workspace
+        shell: bash
+        run: |
+          source /opt/ros/${{ matrix.distro }}/setup.bash
+          colcon build --cmake-args -DCMAKE_CXX_COMPILER=${{ matrix.compiler }}
+        working-directory: ros2

AirSim/cmake/cmake-modules/CommonSetup.cmake

@@ -60,7 +60,7 @@ macro(CommonSetup)
                 ${RPC_LIB_DEFINES} ${CMAKE_CXX_FLAGS}")
 
             if (${CMAKE_CXX_COMPILER_ID} MATCHES "Clang")
-                set(CMAKE_CXX_FLAGS "-stdlib=libc++ -Wno-documentation ${CMAKE_CXX_FLAGS}")
+                set(CMAKE_CXX_FLAGS "-Wno-documentation ${CMAKE_CXX_FLAGS}")
                 set(CXX_EXP_LIB "-lc++fs -ferror-limit=10")
             else()
                 set(CXX_EXP_LIB "-lstdc++fs -fmax-errors=10 -Wnoexcept -Wstrict-null-sentinel")

ros2/src/fsds_ros2_bridge/include/airsim_ros_wrapper.h

@@ -28,7 +28,7 @@ STRICT_MODE_OFF //todo what does this do?
 #include <geometry_msgs/msg/twist.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
 #include <iostream>
-#include <math.h>
+#include <cmath>
 #include <math_common.h>
 #include <nav_msgs/msg/odometry.hpp>
 #include <opencv2/opencv.hpp>
@@ -66,31 +66,31 @@ struct enabledSensors {
     bool gps = false;
     bool gss = false;
     bool imu = false;
-    
-    void print() {
+
+    void print() const {
         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();
     }
 };
@@ -111,16 +111,16 @@ class AirsimROSWrapper
 {
 public:
     AirsimROSWrapper(const std::shared_ptr<rclcpp::Node>& nh, const std::string& host_ip, double timeout_sec);
-    ~AirsimROSWrapper(){};
+    ~AirsimROSWrapper()= default;
 
     void initialize_airsim(double timeout_sec);
     void initialize_ros();
     void publish_track();
     void initialize_statistics();
 
     // ros::AsyncSpinner lidar_async_spinner_;
-    bool is_used_lidar_timer_cb_queue_;
-    bool is_used_img_timer_cb_queue_;
+    bool is_used_lidar_timer_cb_queue_{};
+    bool is_used_img_timer_cb_queue_{};
 
     rclcpp::Time first_ros_ts;
     int64_t first_unreal_ts = -1;
@@ -144,11 +144,11 @@ class AirsimROSWrapper
     std::vector<ros_bridge::Statistics> lidar_pub_vec_statistics;
     ros_bridge::Statistics imu_pub_statistics;
     ros_bridge::Statistics gss_pub_statistics;
-    
+
     // 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
@@ -164,17 +164,17 @@ class AirsimROSWrapper
     void gss_timer_cb();
     void wheel_states_timer_cb();
     void statictf_cb();
-    void car_control_cb(const fs_msgs::msg::ControlCommand& msg);
-    void lidar_timer_cb(const std::string& camera_name, const int lidar_index);
+    void car_control_cb(fs_msgs::msg::ControlCommand::SharedPtr msg);
+    void lidar_timer_cb(const std::string& camera_name, int lidar_index);
     void statistics_timer_cb();
     void go_signal_timer_cb();
 	void extra_info_cb();
 	void clock_timer_cb();
 
     /// ROS subscriber callbacks
-    void finished_signal_cb(const fs_msgs::msg::FinishedSignal& msg);
+    void finished_signal_cb(fs_msgs::msg::FinishedSignal::SharedPtr msg);
 
-    rclcpp::Time make_ts(uint64_t unreal_ts) const;
+    [[nodiscard]] static rclcpp::Time make_ts(uint64_t unreal_ts) ;
     // void set_zero_vel_cmd();
 
     /// ROS service callbacks
@@ -185,38 +185,38 @@ class AirsimROSWrapper
     void append_static_camera_tf(const std::string& vehicle_name, const std::string& camera_name, const CameraSetting& camera_setting);
     void append_static_lidar_tf(const std::string& vehicle_name, const std::string& lidar_name, const LidarSetting& lidar_setting);
     void append_static_vehicle_tf(const std::string& vehicle_name, const VehicleSetting& vehicle_setting);
-    void set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const;
-    void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) const;
-    void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, LidarSetting& lidar_setting) const;
+    static void set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) ;
+    static void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) ;
+    static void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, LidarSetting& lidar_setting) ;
 
     /// utils. todo parse into an Airlib<->ROS conversion class
-    tf2::Quaternion get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const;
-    msr::airlib::Quaternionr get_airlib_quat(const geometry_msgs::msg::Quaternion& geometry_msgs_quat) const;
-    msr::airlib::Quaternionr get_airlib_quat(const tf2::Quaternion& tf2_quat) const;
+    [[nodiscard]] tf2::Quaternion get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const;
+    [[nodiscard]] msr::airlib::Quaternionr get_airlib_quat(const geometry_msgs::msg::Quaternion& geometry_msgs_quat) const;
+    [[nodiscard]] msr::airlib::Quaternionr get_airlib_quat(const tf2::Quaternion& tf2_quat) const;
 
-    nav_msgs::msg::Odometry get_odom_msg_from_airsim_state(const msr::airlib::CarApiBase::CarState& car_state) const;
-    sensor_msgs::msg::NavSatFix get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const;
+    [[nodiscard]] nav_msgs::msg::Odometry get_odom_msg_from_airsim_state(const msr::airlib::CarApiBase::CarState& car_state) const;
+    [[nodiscard]] sensor_msgs::msg::NavSatFix get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const;
     sensor_msgs::msg::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data);
-    sensor_msgs::msg::PointCloud2 get_lidar_msg_from_airsim(const std::string &lidar_name, const msr::airlib::LidarData& lidar_data) const;
+    [[nodiscard]] static sensor_msgs::msg::PointCloud2 get_lidar_msg_from_airsim(const std::string &lidar_name, const msr::airlib::LidarData& lidar_data) ;
     static bool equalsMessage(const nav_msgs::msg::Odometry& a, const nav_msgs::msg::Odometry& b);
 
 
-    std::string readTextFromFile(std::string settingsFilepath);
+    static std::string readTextFromFile(const std::string& settingsFilepath);
 
 private:
     std::shared_ptr<rclcpp::Service<fs_msgs::srv::Reset>> reset_srvr_;
 
     std::string vehicle_name;
     std::string map_frame_id_;
     std::string vehicle_frame_id_;
-    CarApiBase::Point2D car_start_pos; // In Unreal coordinates
+    CarApiBase::Point2D car_start_pos{}; // In Unreal coordinates
 
 
     std::vector<std::string> lidar_names_vec_;
     std::vector<geometry_msgs::msg::TransformStamped> static_tf_msg_vec_;
     std::string mission_name_; // rosparam obtained from launch file
     std::string track_name_; // rosparam obtained from launch file
-    bool competition_mode_;
+    bool competition_mode_{};
     rclcpp::Time go_timestamp_;
 
     std::string host_ip_;
@@ -264,7 +264,7 @@ class AirsimROSWrapper
     std::shared_ptr<rclcpp::Publisher<fs_msgs::msg::GoSignal>> go_signal_pub_;
 	std::shared_ptr<rclcpp::Publisher<fs_msgs::msg::ExtraInfo>> extra_info_pub;
 	std::shared_ptr<rclcpp::Publisher<rosgraph_msgs::msg::Clock>> clock_pub;
-    
+
     /// ROS subscribers
     rclcpp::Subscription<fs_msgs::msg::ControlCommand>::SharedPtr control_cmd_sub;
     rclcpp::Subscription<fs_msgs::msg::FinishedSignal>::SharedPtr finished_signal_sub_;