Added GPULidar support (adds PCL (conversion) to dependencies) and Ec…

…ho/Lidar groundtruth label support with new message type StringArray

ros2/src/airsim_interfaces/CMakeLists.txt

@@ -25,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/CarControls.msg"
   "msg/CarState.msg"
   "msg/ComputerVisionState.msg"
+  "msg/StringArray.msg"
   "msg/Altimeter.msg"
   "msg/Environment.msg"
   "srv/SetGPSPosition.srv"

ros2/src/airsim_interfaces/msg/StringArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+string[] data

ros2/src/airsim_ros_pkgs/CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(rosidl_typesupport_cpp REQUIRED)
 find_package(rosidl_default_runtime REQUIRED)
 find_package(airsim_interfaces REQUIRED)
 find_package(OpenCV REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(PCL REQUIRED)
 
 set(AIRSIM_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/../../../)
 
@@ -80,6 +82,8 @@ ament_target_dependencies(airsim_ros
   tf2_geometry_msgs
   tf2_sensor_msgs
   tf2
+  pcl_conversions
+  PCL
 )
 
 add_executable(airsim_node src/airsim_node.cpp)
@@ -140,6 +144,8 @@ ament_export_dependencies(ament_cmake)
 ament_export_dependencies(std_msgs)
 ament_export_dependencies(rosidl_default_runtime)
 ament_export_dependencies(airsim_interfaces)
+ament_export_dependencies(pcl_conversions)
+ament_export_dependencies(PCL)
 
 ament_export_include_directories(${INCLUDE_DIRS})
 ament_export_libraries(airsim_settings_parser pd_position_controller_simple

ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h

@@ -10,6 +10,8 @@ STRICT_MODE_OFF //todo what does this do?
 #include "common/AirSimSettings.hpp"
 #include "common/common_utils/FileSystem.hpp"
 #include "sensors/lidar/LidarSimpleParams.hpp"
+#include "sensors/lidar/GPULidarSimpleParams.hpp"
+#include "sensors/echo/EchoSimpleParams.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "sensors/imu/ImuBase.hpp"
 #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
@@ -29,6 +31,7 @@ STRICT_MODE_OFF //todo what does this do?
 #include <airsim_interfaces/msg/car_controls.hpp>
 #include <airsim_interfaces/msg/car_state.hpp>
 #include <airsim_interfaces/msg/computer_vision_state.hpp>
+#include <airsim_interfaces/msg/string_array.hpp>
 #include <airsim_interfaces/msg/environment.hpp>
 #include <chrono>
 #include <cv_bridge/cv_bridge.h>
@@ -51,6 +54,10 @@ STRICT_MODE_OFF //todo what does this do?
 #include <airsim_interfaces/msg/altimeter.hpp> //hector_uav_msgs defunct?
 #include <sensor_msgs/msg/magnetic_field.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
 #include <sensor_msgs/msg/range.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
 #include <std_srvs/srv/empty.hpp>
@@ -65,7 +72,33 @@ STRICT_MODE_OFF //todo what does this do?
 #include <unordered_map>
 #include <memory>
 
-    struct SimpleMatrix
+struct PointXYZRGBI
+{
+    PCL_ADD_POINT4D;
+    float intensity;
+    union
+    {
+        struct
+        {
+            uint8_t b;
+            uint8_t g;
+            uint8_t r;
+        };
+        float rgb;
+    };
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;   
+
+// Register the point type
+POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZRGBI,
+                                  (float, x, x)
+                                  (float, y, y)
+                                  (float, z, z)
+                                  (float, rgb, rgb)
+                                  (float, intensity, intensity)
+)
+
+struct SimpleMatrix
 {
     int rows;
     int cols;
@@ -109,6 +142,8 @@ class AirsimROSWrapper
     using CameraSetting = msr::airlib::AirSimSettings::CameraSetting;
     using CaptureSetting = msr::airlib::AirSimSettings::CaptureSetting;
     using LidarSetting = msr::airlib::AirSimSettings::LidarSetting;
+    using GPULidarSetting = msr::airlib::AirSimSettings::GPULidarSetting;
+    using EchoSetting = msr::airlib::AirSimSettings::EchoSetting;
     using VehicleSetting = msr::airlib::AirSimSettings::VehicleSetting;
     using ImageRequest = msr::airlib::ImageCaptureBase::ImageRequest;
     using ImageResponse = msr::airlib::ImageCaptureBase::ImageResponse;
@@ -122,14 +157,16 @@ class AirsimROSWrapper
         COMPUTERVISION
     };
 
-    AirsimROSWrapper(const std::shared_ptr<rclcpp::Node> nh, const std::shared_ptr<rclcpp::Node> nh_img, const std::shared_ptr<rclcpp::Node> nh_lidar, const std::string& host_ip, const std::shared_ptr<rclcpp::CallbackGroup> callbackGroup, bool enable_api_control, uint16_t host_port);
+    AirsimROSWrapper(const std::shared_ptr<rclcpp::Node> nh, const std::shared_ptr<rclcpp::Node> nh_img, const std::shared_ptr<rclcpp::Node> nh_lidar, const std::shared_ptr<rclcpp::Node> nh_gpulidar, const std::shared_ptr<rclcpp::Node> nh_echo, const std::string& host_ip, const std::shared_ptr<rclcpp::CallbackGroup> callbackGroup, bool enable_api_control, uint16_t host_port);
     ~AirsimROSWrapper(){};
 
     void initialize_airsim();
     void initialize_ros();
 
     bool is_used_lidar_timer_cb_queue_;
     bool is_used_img_timer_cb_queue_;
+    bool is_used_gpulidar_timer_cb_queue_;
+    bool is_used_echo_timer_cb_queue_;
 
 private:
     // utility struct for a SINGLE robot
@@ -151,6 +188,12 @@ class AirsimROSWrapper
         std::vector<SensorPublisher<sensor_msgs::msg::MagneticField>> magnetometer_pubs_;
         std::vector<SensorPublisher<sensor_msgs::msg::Range>> distance_pubs_;
         std::vector<SensorPublisher<sensor_msgs::msg::PointCloud2>> lidar_pubs_;
+        std::vector<SensorPublisher<airsim_interfaces::msg::StringArray>> lidar_labels_pubs_;
+        std::vector<SensorPublisher<sensor_msgs::msg::PointCloud2>> gpulidar_pubs_;
+        std::vector<SensorPublisher<sensor_msgs::msg::PointCloud2>> echo_active_pubs_;
+        std::vector<SensorPublisher<airsim_interfaces::msg::StringArray>> echo_active_labels_pubs_;
+        std::vector<SensorPublisher<sensor_msgs::msg::PointCloud2>> echo_passive_pubs_;
+        std::vector<SensorPublisher<airsim_interfaces::msg::StringArray>> echo_passive_labels_pubs_;
 
         // handle lidar seperately for max performance as data is collected on its own thread/callback
 
@@ -206,6 +249,8 @@ class AirsimROSWrapper
     void img_response_timer_cb(); // update images from airsim_client_ every nth sec
     void drone_state_timer_cb(); // update drone state from airsim_client_ every nth sec
     void lidar_timer_cb();
+    void gpulidar_timer_cb();
+    void echo_timer_cb();
 
     /// ROS subscriber callbacks
     void vel_cmd_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name);
@@ -256,10 +301,14 @@ class AirsimROSWrapper
     void convert_tf_msg_to_ros(geometry_msgs::msg::TransformStamped& tf_msg);
     void append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting);
     void append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& lidar_setting);
+    void append_static_gpulidar_tf(VehicleROS* vehicle_ros, const std::string& gpulidar_name, const msr::airlib::GPULidarSimpleParams& gpulidar_setting);
+    void append_static_echo_tf(VehicleROS* vehicle_ros, const std::string& echo_name, const msr::airlib::EchoSimpleParams& echo_setting);
     void append_static_vehicle_tf(VehicleROS* vehicle_ros, 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;
+    void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, GPULidarSetting& gpulidar_setting) const;
+    void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, EchoSetting& echo_setting) const;
     geometry_msgs::msg::Transform get_camera_optical_tf_from_body_tf(const geometry_msgs::msg::Transform& body_tf) const;
 
     /// utils. todo parse into an Airlib<->ROS conversion class
@@ -279,6 +328,12 @@ class AirsimROSWrapper
     airsim_interfaces::msg::Altimeter get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const;
     sensor_msgs::msg::Range get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const;
     sensor_msgs::msg::PointCloud2 get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name, const std::string& sensor_name) const;
+    airsim_interfaces::msg::StringArray get_lidar_labels_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name, const std::string& sensor_name) const;
+    sensor_msgs::msg::PointCloud2 get_gpulidar_msg_from_airsim(const msr::airlib::GPULidarData& gpulidar_data, const std::string& vehicle_name, const std::string& sensor_name) const;
+    sensor_msgs::msg::PointCloud2 get_active_echo_msg_from_airsim(const msr::airlib::EchoData& echo_data, const std::string& vehicle_name, const std::string& sensor_name) const;   
+    airsim_interfaces::msg::StringArray get_active_echo_labels_msg_from_airsim(const msr::airlib::EchoData& echo_data, const std::string& vehicle_name, const std::string& sensor_name) const;
+    sensor_msgs::msg::PointCloud2 get_passive_echo_msg_from_airsim(const msr::airlib::EchoData& echo_data, const std::string& vehicle_name, const std::string& sensor_name) const;
+    airsim_interfaces::msg::StringArray get_passive_echo_labels_msg_from_airsim(const msr::airlib::EchoData& echo_data, const std::string& vehicle_name, const std::string& sensor_name) const;   
     sensor_msgs::msg::NavSatFix get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const;
     sensor_msgs::msg::MagneticField get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const;
     airsim_interfaces::msg::Environment get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const;
@@ -329,10 +384,14 @@ class AirsimROSWrapper
     // seperate busy connections to airsim, update in their own thread
     msr::airlib::RpcLibClientBase airsim_client_images_;
     msr::airlib::RpcLibClientBase airsim_client_lidar_;
+    msr::airlib::RpcLibClientBase airsim_client_gpulidar_;
+    msr::airlib::RpcLibClientBase airsim_client_echo_;
 
     std::shared_ptr<rclcpp::Node> nh_;
     std::shared_ptr<rclcpp::Node> nh_img_;
     std::shared_ptr<rclcpp::Node> nh_lidar_;
+    std::shared_ptr<rclcpp::Node> nh_gpulidar_;
+    std::shared_ptr<rclcpp::Node> nh_echo_;
     std::shared_ptr<rclcpp::CallbackGroup> cb_;
 
     // 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
@@ -359,12 +418,17 @@ class AirsimROSWrapper
     rclcpp::TimerBase::SharedPtr airsim_img_response_timer_;
     rclcpp::TimerBase::SharedPtr airsim_control_update_timer_;
     rclcpp::TimerBase::SharedPtr airsim_lidar_update_timer_;
+    rclcpp::TimerBase::SharedPtr airsim_gpulidar_update_timer_;
+    rclcpp::TimerBase::SharedPtr airsim_echo_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_;
     std::vector<image_transport::Publisher> image_pub_vec_;
     std::vector<rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> cam_info_pub_vec_;
 
+    std::unordered_map<std::string, bool> sensor_name_to_passive_enable_map_;
+    std::unordered_map<std::string, bool> sensor_name_to_active_enable_map_;
+
     std::vector<sensor_msgs::msg::CameraInfo> camera_info_msg_vec_;
 
     /// ROS other publishers

ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py

@@ -45,6 +45,8 @@ def generate_launch_description():
                 'update_airsim_img_response_every_n_sec': 0.05,
                 'update_airsim_control_every_n_sec': 0.01,
                 'update_lidar_every_n_sec': 0.01,
+                'update_gpulidar_every_n_sec': 0.01,
+                'update_echo_every_n_sec': 0.01,
                 'publish_clock': LaunchConfiguration('publish_clock'),
                 'host_ip': LaunchConfiguration('host_ip'),
                 'host_port': LaunchConfiguration('host_port'),

ros2/src/airsim_ros_pkgs/src/airsim_node.cpp

@@ -9,14 +9,16 @@ int main(int argc, char** argv)
     std::shared_ptr<rclcpp::Node> nh = rclcpp::Node::make_shared("airsim_node", node_options);
     std::shared_ptr<rclcpp::Node> nh_img = nh->create_sub_node("img");
     std::shared_ptr<rclcpp::Node> nh_lidar = nh->create_sub_node("lidar");
+    std::shared_ptr<rclcpp::Node> nh_gpulidar = nh->create_sub_node("gpulidar");
+    std::shared_ptr<rclcpp::Node> nh_echo = nh->create_sub_node("echo");
     std::string host_ip;
     uint16_t host_port = 41451;
     bool enable_api_control = false;
     nh->get_parameter("host_ip", host_ip);
     nh->get_parameter("host_port", host_port);
     nh->get_parameter("enable_api_control", enable_api_control);
     auto callbackGroup = nh->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-    AirsimROSWrapper airsim_ros_wrapper(nh, nh_img, nh_lidar, host_ip, callbackGroup, enable_api_control, host_port);
+    AirsimROSWrapper airsim_ros_wrapper(nh, nh_img, nh_lidar, nh_gpulidar, nh_echo, host_ip, callbackGroup, enable_api_control, host_port);
 
     rclcpp::executors::MultiThreadedExecutor executor;
     executor.add_node(nh);