From 161fd4b5e9ad87bce1665960ebf5b2a124e84d16 Mon Sep 17 00:00:00 2001 From: Wouter Jansen Date: Fri, 5 Jul 2024 13:32:50 +0200 Subject: [PATCH] Added GPULidar support (adds PCL (conversion) to dependencies) and Echo/Lidar groundtruth label support with new message type StringArray --- ros2/src/airsim_interfaces/CMakeLists.txt | 1 + .../src/airsim_interfaces/msg/StringArray.msg | 2 + ros2/src/airsim_ros_pkgs/CMakeLists.txt | 6 + .../include/airsim_ros_wrapper.h | 68 ++- .../launch/airsim_node.launch.py | 2 + ros2/src/airsim_ros_pkgs/src/airsim_node.cpp | 4 +- .../src/airsim_ros_wrapper.cpp | 419 +++++++++++++++++- 7 files changed, 484 insertions(+), 18 deletions(-) create mode 100644 ros2/src/airsim_interfaces/msg/StringArray.msg diff --git a/ros2/src/airsim_interfaces/CMakeLists.txt b/ros2/src/airsim_interfaces/CMakeLists.txt index 381cf07a4..8acf04ab9 100644 --- a/ros2/src/airsim_interfaces/CMakeLists.txt +++ b/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" diff --git a/ros2/src/airsim_interfaces/msg/StringArray.msg b/ros2/src/airsim_interfaces/msg/StringArray.msg new file mode 100644 index 000000000..667433ccb --- /dev/null +++ b/ros2/src/airsim_interfaces/msg/StringArray.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +string[] data \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/CMakeLists.txt b/ros2/src/airsim_ros_pkgs/CMakeLists.txt index e4fee5265..3ca38237a 100644 --- a/ros2/src/airsim_ros_pkgs/CMakeLists.txt +++ b/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 diff --git a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h index 084886cfa..47b9a9761 100755 --- a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/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 #include #include +#include #include #include #include @@ -51,6 +54,10 @@ STRICT_MODE_OFF //todo what does this do? #include //hector_uav_msgs defunct? #include #include +#include +#include +#include +#include #include #include #include @@ -65,7 +72,33 @@ STRICT_MODE_OFF //todo what does this do? #include #include - 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,7 +157,7 @@ class AirsimROSWrapper COMPUTERVISION }; - AirsimROSWrapper(const std::shared_ptr nh, const std::shared_ptr nh_img, const std::shared_ptr nh_lidar, const std::string& host_ip, const std::shared_ptr callbackGroup, bool enable_api_control, uint16_t host_port); + AirsimROSWrapper(const std::shared_ptr nh, const std::shared_ptr nh_img, const std::shared_ptr nh_lidar, const std::shared_ptr nh_gpulidar, const std::shared_ptr nh_echo, const std::string& host_ip, const std::shared_ptr callbackGroup, bool enable_api_control, uint16_t host_port); ~AirsimROSWrapper(){}; void initialize_airsim(); @@ -130,6 +165,8 @@ class AirsimROSWrapper 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> magnetometer_pubs_; std::vector> distance_pubs_; std::vector> lidar_pubs_; + std::vector> lidar_labels_pubs_; + std::vector> gpulidar_pubs_; + std::vector> echo_active_pubs_; + std::vector> echo_active_labels_pubs_; + std::vector> echo_passive_pubs_; + std::vector> 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 nh_; std::shared_ptr nh_img_; std::shared_ptr nh_lidar_; + std::shared_ptr nh_gpulidar_; + std::shared_ptr nh_echo_; std::shared_ptr 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::string> airsim_img_request_vehicle_name_pair; std::vector airsim_img_request_vehicle_name_pair_vec_; std::vector image_pub_vec_; std::vector::SharedPtr> cam_info_pub_vec_; + std::unordered_map sensor_name_to_passive_enable_map_; + std::unordered_map sensor_name_to_active_enable_map_; + std::vector camera_info_msg_vec_; /// ROS other publishers diff --git a/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py b/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py index 8935584a6..d7c747ee3 100644 --- a/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py +++ b/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'), diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp index 0592dab55..4c9edb8bb 100644 --- a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp @@ -9,6 +9,8 @@ int main(int argc, char** argv) std::shared_ptr nh = rclcpp::Node::make_shared("airsim_node", node_options); std::shared_ptr nh_img = nh->create_sub_node("img"); std::shared_ptr nh_lidar = nh->create_sub_node("lidar"); + std::shared_ptr nh_gpulidar = nh->create_sub_node("gpulidar"); + std::shared_ptr nh_echo = nh->create_sub_node("echo"); std::string host_ip; uint16_t host_port = 41451; bool enable_api_control = false; @@ -16,7 +18,7 @@ int main(int argc, char** argv) 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); diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 20512df3e..6df83e27e 100755 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -28,9 +28,11 @@ const std::unordered_map AirsimROSWrapper::image_type_int_to_s }; -AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr nh, const std::shared_ptr nh_img, const std::shared_ptr nh_lidar, const std::string& host_ip, const std::shared_ptr callbackGroup, bool enable_api_control, uint16_t host_port) +AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr nh, const std::shared_ptr nh_img, const std::shared_ptr nh_lidar, const std::shared_ptr nh_gpulidar, const std::shared_ptr nh_echo, const std::string& host_ip, const std::shared_ptr callbackGroup, bool enable_api_control, uint16_t host_port) : is_used_lidar_timer_cb_queue_(false) , is_used_img_timer_cb_queue_(false) + , is_used_gpulidar_timer_cb_queue_(false) + , is_used_echo_timer_cb_queue_(false) , airsim_settings_parser_(host_ip, host_port) , host_ip_(host_ip) , host_port_(host_port) @@ -38,9 +40,13 @@ AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr nh, const , airsim_client_(nullptr) , airsim_client_images_(host_ip, host_port) , airsim_client_lidar_(host_ip, host_port) + , airsim_client_gpulidar_(host_ip, host_port) + , airsim_client_echo_(host_ip, host_port) , nh_(nh) , nh_img_(nh_img) , nh_lidar_(nh_lidar) + , nh_gpulidar_(nh_gpulidar) + , nh_echo_(nh_echo) , cb_(callbackGroup) , publish_clock_(false) { @@ -81,6 +87,8 @@ void AirsimROSWrapper::initialize_airsim() airsim_client_->confirmConnection(); airsim_client_images_.confirmConnection(); airsim_client_lidar_.confirmConnection(); + airsim_client_gpulidar_.confirmConnection(); + airsim_client_echo_.confirmConnection(); if(enable_api_control_){ for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { @@ -133,6 +141,8 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() camera_info_msg_vec_.clear(); vehicle_name_ptr_map_.clear(); size_t lidar_cnt = 0; + size_t gpulidar_cnt = 0; + size_t echo_cnt = 0; image_transport::ImageTransport image_transporter(nh_); @@ -259,31 +269,31 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() switch (sensor_setting->sensor_type) { case SensorBase::SensorType::Barometer: { SensorPublisher sensor_publisher = - create_sensor_publisher("Barometer", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/altimeter/" + sensor_name, 10); + create_sensor_publisher("Barometer sensor", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/altimeter/" + sensor_name, 10); vehicle_ros->barometer_pubs_.emplace_back(sensor_publisher); break; } case SensorBase::SensorType::Imu: { SensorPublisher sensor_publisher = - create_sensor_publisher("Imu", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/imu/" + sensor_name, 10); + create_sensor_publisher("Imu sensor", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/imu/" + sensor_name, 10); vehicle_ros->imu_pubs_.emplace_back(sensor_publisher); break; } case SensorBase::SensorType::Gps: { SensorPublisher sensor_publisher = - create_sensor_publisher("Gps", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/gps/" + sensor_name, 10); + create_sensor_publisher("Gps sensor", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/gps/" + sensor_name, 10); vehicle_ros->gps_pubs_.emplace_back(sensor_publisher); break; } case SensorBase::SensorType::Magnetometer: { SensorPublisher sensor_publisher = - create_sensor_publisher("Magnetometer", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/magnetometer/" + sensor_name, 10); + create_sensor_publisher("Magnetometer sensor", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/magnetometer/" + sensor_name, 10); vehicle_ros->magnetometer_pubs_.emplace_back(sensor_publisher); break; } case SensorBase::SensorType::Distance: { SensorPublisher sensor_publisher = - create_sensor_publisher("Distance", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/distance/" + sensor_name, 10); + create_sensor_publisher("Distance sensor", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/distance/" + sensor_name, 10); vehicle_ros->distance_pubs_.emplace_back(sensor_publisher); break; } @@ -294,11 +304,51 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() append_static_lidar_tf(vehicle_ros.get(), sensor_name, params); SensorPublisher sensor_publisher = - create_sensor_publisher("Lidar", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/lidar/" + sensor_name, 10); + create_sensor_publisher("Lidar sensor", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/lidar/points/" + sensor_name, 10); vehicle_ros->lidar_pubs_.emplace_back(sensor_publisher); + SensorPublisher sensor_publisher_labels = + create_sensor_publisher("", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/lidar/labels/" + sensor_name, 10); + vehicle_ros->lidar_labels_pubs_.emplace_back(sensor_publisher_labels); lidar_cnt += 1; break; } + case SensorBase::SensorType::GPULidar: { + auto gpulidar_setting = *static_cast(sensor_setting.get()); + msr::airlib::GPULidarSimpleParams params; + params.initializeFromSettings(gpulidar_setting); + append_static_gpulidar_tf(vehicle_ros.get(), sensor_name, params); + + SensorPublisher sensor_publisher = + create_sensor_publisher("GPULidar sensor", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/gpulidar/points/" + sensor_name, 10); + vehicle_ros->gpulidar_pubs_.emplace_back(sensor_publisher); + gpulidar_cnt += 1; + break; + } + case SensorBase::SensorType::Echo: { + auto echo_setting = *static_cast(sensor_setting.get()); + msr::airlib::EchoSimpleParams params; + params.initializeFromSettings(echo_setting); + append_static_echo_tf(vehicle_ros.get(), sensor_name, params); + if(params.active){ + SensorPublisher sensor_publisher = + create_sensor_publisher("Echo (active) sensor", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/echo/active/points/" + sensor_name, 10); + vehicle_ros->echo_active_pubs_.emplace_back(sensor_publisher); + SensorPublisher sensor_publisher_labels = + create_sensor_publisher("", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/echo/active/labels/" + sensor_name, 10); + vehicle_ros->echo_active_labels_pubs_.emplace_back(sensor_publisher_labels); + + } + if(params.passive){ + SensorPublisher sensor_publisher = + create_sensor_publisher("Echo (passive) sensor ", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/echo/passive/points/" + sensor_name, 10); + vehicle_ros->echo_passive_pubs_.emplace_back(sensor_publisher); + SensorPublisher sensor_publisher_labels = + create_sensor_publisher("", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/echo/passive/labels/" + sensor_name, 10); + vehicle_ros->echo_passive_labels_pubs_.emplace_back(sensor_publisher_labels); + } + echo_cnt += 1; + break; + } default: { throw std::invalid_argument("Unexpected sensor type"); } @@ -340,13 +390,27 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() is_used_img_timer_cb_queue_ = true; } - // lidars update on their own callback/thread at a given rate + // lidar, gpulidar and echo sensors update on their own callbacks/threads at a given rate if (lidar_cnt > 0) { double update_lidar_every_n_sec; nh_->get_parameter("update_lidar_every_n_sec", update_lidar_every_n_sec); airsim_lidar_update_timer_ = nh_lidar_->create_wall_timer(std::chrono::duration(update_lidar_every_n_sec), std::bind(&AirsimROSWrapper::lidar_timer_cb, this), cb_); is_used_lidar_timer_cb_queue_ = true; } + if (gpulidar_cnt > 0) { + double update_gpulidar_every_n_sec; + nh_->get_parameter("update_gpulidar_every_n_sec", update_gpulidar_every_n_sec); + airsim_gpulidar_update_timer_ = nh_gpulidar_->create_wall_timer(std::chrono::duration(update_gpulidar_every_n_sec), std::bind(&AirsimROSWrapper::gpulidar_timer_cb, this), cb_); + is_used_gpulidar_timer_cb_queue_ = true; + } + + if (echo_cnt > 0) { + double update_echo_every_n_sec; + nh_->get_parameter("update_echo_every_n_sec", update_echo_every_n_sec); + airsim_echo_update_timer_ = nh_echo_->create_wall_timer(std::chrono::duration(update_echo_every_n_sec), std::bind(&AirsimROSWrapper::echo_timer_cb, this), cb_); + is_used_echo_timer_cb_queue_ = true; + } + initialize_airsim(); } @@ -357,7 +421,9 @@ template const SensorPublisher AirsimROSWrapper::create_sensor_publisher(const std::string& sensor_type_name, const std::string& sensor_name, SensorBase::SensorType sensor_type, const std::string& topic_name, int QoS) { - RCLCPP_INFO_STREAM(nh_->get_logger(), sensor_type_name); + if(sensor_type_name != "") + RCLCPP_INFO_STREAM(nh_->get_logger(), "Publishing " << sensor_type_name << " '" << sensor_name << "'"); + SensorPublisher sensor_publisher; sensor_publisher.sensor_name = sensor_name; sensor_publisher.sensor_type = sensor_type; @@ -684,11 +750,13 @@ nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_multirotor_state(con return get_odom_msg_from_kinematic_state(drone_state.kinematics_estimated); } -void flipSigns(std::vector& data) { - for (size_t i = 1; i < data.size(); i += 3) { +void fixPointCloud(std::vector& data, int offset, std::vector flip_indexes) { + for (size_t i = 1; i < data.size(); i += offset) { data[i] = -data[i]; - if (i + 1 < data.size()) { - data[i + 1] = -data[i + 1]; + for (int flip_index : flip_indexes) { + if (i + flip_index < data.size()) { + data[i + flip_index] = -data[i + flip_index]; + } } } } @@ -725,7 +793,7 @@ sensor_msgs::msg::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const lidar_msg.is_dense = true; // todo std::vector data_std = lidar_data.point_cloud; - flipSigns(data_std); + fixPointCloud(data_std, 3, {1}); const unsigned char* bytes = reinterpret_cast(data_std.data()); std::vector lidar_msg_data(bytes, bytes + sizeof(float) * data_std.size()); @@ -738,6 +806,201 @@ sensor_msgs::msg::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const return lidar_msg; } +airsim_interfaces::msg::StringArray AirsimROSWrapper::get_lidar_labels_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name, const std::string& sensor_name) const +{ + airsim_interfaces::msg::StringArray lidar_labels_msg; + lidar_labels_msg.header.stamp = rclcpp::Time(lidar_data.time_stamp); + lidar_labels_msg.header.frame_id = vehicle_name + "/" + sensor_name; + + if (lidar_data.point_cloud.size() > 3) { + lidar_labels_msg.data = std::move(lidar_data.groundtruth); + } + else { + // msg = [] + } + + return lidar_labels_msg; +} + +sensor_msgs::msg::PointCloud2 AirsimROSWrapper::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 gpulidar_msg; + gpulidar_msg.header.stamp = rclcpp::Time(gpulidar_data.time_stamp); + gpulidar_msg.header.frame_id = vehicle_name + "/" + sensor_name; + + if (gpulidar_data.point_cloud.size() > 5) { + + std::vector data_std = gpulidar_data.point_cloud; + fixPointCloud(data_std, 5, {1}); + + size_t num_points = data_std.size() / 5; + pcl::PointCloud cloud; + cloud.points.resize(num_points); + cloud.width = static_cast(num_points); + cloud.height = 1; + cloud.is_dense = true; + + for (size_t i = 0; i < num_points; ++i) + { + // Extract x, y, z, and rgb values + float x = data_std[i * 5 + 0]; + float y = data_std[i * 5 + 1]; + float z = data_std[i * 5 + 2]; + float rgb_packed = data_std[i * 5 + 3]; + float intensity = data_std[i * 5 + 4]; + + // Unpack the RGB value + uint8_t r = ((std::uint32_t)rgb_packed >> 16) & 0x0000ff; + uint8_t g = ((std::uint32_t)rgb_packed >> 8) & 0x0000ff; + uint8_t b = ((std::uint32_t)rgb_packed) & 0x0000ff; + + // Populate the point + PointXYZRGBI point; + point.x = x; + point.y = y; + point.z = z; + std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b); + point.rgb = *reinterpret_cast(&rgb); + point.intensity = intensity; + cloud.points[i] = point; + } + + pcl::toROSMsg(cloud, gpulidar_msg); + gpulidar_msg.header.stamp = rclcpp::Time(gpulidar_data.time_stamp); + gpulidar_msg.header.frame_id = vehicle_name + "/" + sensor_name; + } + else { + // msg = [] + } + + return gpulidar_msg; +} + +sensor_msgs::msg::PointCloud2 AirsimROSWrapper::get_active_echo_msg_from_airsim(const msr::airlib::EchoData& echo_data, const std::string& vehicle_name, const std::string& sensor_name) const +{ + sensor_msgs::msg::PointCloud2 echo_msg; + echo_msg.header.stamp = rclcpp::Time(echo_data.time_stamp); + echo_msg.header.frame_id = vehicle_name + "/" + sensor_name; + + if (echo_data.point_cloud.size() > 6) { + echo_msg.height = 1; + echo_msg.width = echo_data.point_cloud.size() / 6; + + echo_msg.fields.resize(6); + echo_msg.fields[0].name = "x"; + echo_msg.fields[1].name = "y"; + echo_msg.fields[2].name = "z"; + echo_msg.fields[3].name = "a"; + echo_msg.fields[4].name = "d"; + echo_msg.fields[5].name = "r"; + + + int offset = 0; + for (size_t d = 0; d < echo_msg.fields.size(); ++d, offset += 4) { + echo_msg.fields[d].offset = offset; + echo_msg.fields[d].datatype = sensor_msgs::msg::PointField::FLOAT32; + echo_msg.fields[d].count = 1; + } + + echo_msg.is_bigendian = false; + echo_msg.point_step = offset; + echo_msg.row_step = echo_msg.point_step * echo_msg.width; + + echo_msg.is_dense = true; + std::vector data_std = echo_data.point_cloud; + fixPointCloud(data_std, 6, {1}); + + const unsigned char* bytes = reinterpret_cast(data_std.data()); + std::vector echo_msg_data(bytes, bytes + sizeof(float) * data_std.size()); + echo_msg.data = std::move(echo_msg_data); + } + else { + // msg = [] + } + + return echo_msg; +} + +airsim_interfaces::msg::StringArray AirsimROSWrapper::get_active_echo_labels_msg_from_airsim(const msr::airlib::EchoData& echo_data, const std::string& vehicle_name, const std::string& sensor_name) const +{ + airsim_interfaces::msg::StringArray echo_active_labels_msg; + echo_active_labels_msg.header.stamp = rclcpp::Time(echo_data.time_stamp); + echo_active_labels_msg.header.frame_id = vehicle_name + "/" + sensor_name; + + if (echo_data.point_cloud.size() > 6) { + echo_active_labels_msg.data = std::move(echo_data.groundtruth); + } + else { + // msg = [] + } + + return echo_active_labels_msg; +} + +sensor_msgs::msg::PointCloud2 AirsimROSWrapper::get_passive_echo_msg_from_airsim(const msr::airlib::EchoData& echo_data, const std::string& vehicle_name, const std::string& sensor_name) const +{ + sensor_msgs::msg::PointCloud2 echo_msg; + echo_msg.header.stamp = rclcpp::Time(echo_data.time_stamp); + echo_msg.header.frame_id = vehicle_name + "/" + sensor_name; + + if (echo_data.passive_beacons_point_cloud.size() > 9) { + echo_msg.height = 1; + echo_msg.width = echo_data.passive_beacons_point_cloud.size() / 9; + + echo_msg.fields.resize(9); + echo_msg.fields[0].name = "x"; + echo_msg.fields[1].name = "y"; + echo_msg.fields[2].name = "z"; + echo_msg.fields[3].name = "a"; + echo_msg.fields[4].name = "d"; + echo_msg.fields[5].name = "r"; + echo_msg.fields[6].name = "xd"; + echo_msg.fields[7].name = "yd"; + echo_msg.fields[8].name = "zd"; + + + int offset = 0; + for (size_t d = 0; d < echo_msg.fields.size(); ++d, offset += 4) { + echo_msg.fields[d].offset = offset; + echo_msg.fields[d].datatype = sensor_msgs::msg::PointField::FLOAT32; + echo_msg.fields[d].count = 1; + } + + echo_msg.is_bigendian = false; + echo_msg.point_step = offset; + echo_msg.row_step = echo_msg.point_step * echo_msg.width; + + echo_msg.is_dense = true; + std::vector data_std = echo_data.passive_beacons_point_cloud; + fixPointCloud(data_std, 9, {1, 6, 7}); + + const unsigned char* bytes = reinterpret_cast(data_std.data()); + std::vector echo_msg_data(bytes, bytes + sizeof(float) * data_std.size()); + echo_msg.data = std::move(echo_msg_data); + } + else { + // msg = [] + } + + return echo_msg; +} + +airsim_interfaces::msg::StringArray AirsimROSWrapper::get_passive_echo_labels_msg_from_airsim(const msr::airlib::EchoData& echo_data, const std::string& vehicle_name, const std::string& sensor_name) const +{ + airsim_interfaces::msg::StringArray echo_passive_labels_msg; + echo_passive_labels_msg.header.stamp = rclcpp::Time(echo_data.time_stamp); + echo_passive_labels_msg.header.frame_id = vehicle_name + "/" + sensor_name; + + if (echo_data.point_cloud.size() > 9) { + echo_passive_labels_msg.data = std::move(echo_data.passive_beacons_groundtruth); + } + else { + // msg = [] + } + + return echo_passive_labels_msg; +} + airsim_interfaces::msg::Environment AirsimROSWrapper::get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const { airsim_interfaces::msg::Environment env_msg; @@ -1261,6 +1524,40 @@ void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std vehicle_ros->static_tf_msg_vec_.emplace_back(lidar_tf_msg); } +void AirsimROSWrapper::append_static_gpulidar_tf(VehicleROS* vehicle_ros, const std::string& gpulidar_name, const msr::airlib::GPULidarSimpleParams& gpulidar_setting) +{ + geometry_msgs::msg::TransformStamped gpulidar_tf_msg; + if(gpulidar_setting.external) + gpulidar_tf_msg.header.frame_id = world_frame_id_; + else + gpulidar_tf_msg.header.frame_id = vehicle_ros->vehicle_name_ + "/" + odom_frame_id_; + gpulidar_tf_msg.child_frame_id = vehicle_ros->vehicle_name_ + "/" + gpulidar_name; + auto gpulidar_data = airsim_client_gpulidar_.getGPULidarData(gpulidar_name, vehicle_ros->vehicle_name_); + + gpulidar_tf_msg.transform = get_transform_msg_from_airsim(gpulidar_data.pose.position, gpulidar_data.pose.orientation); + + convert_tf_msg_to_ros(gpulidar_tf_msg); + + vehicle_ros->static_tf_msg_vec_.emplace_back(gpulidar_tf_msg); +} + +void AirsimROSWrapper::append_static_echo_tf(VehicleROS* vehicle_ros, const std::string& echo_name, const msr::airlib::EchoSimpleParams& echo_setting) +{ + geometry_msgs::msg::TransformStamped echo_tf_msg; + if(echo_setting.external) + echo_tf_msg.header.frame_id = world_frame_id_; + else + echo_tf_msg.header.frame_id = vehicle_ros->vehicle_name_ + "/" + odom_frame_id_; + echo_tf_msg.child_frame_id = vehicle_ros->vehicle_name_ + "/" + echo_name; + auto echo_data = airsim_client_echo_.getEchoData(echo_name, vehicle_ros->vehicle_name_); + + echo_tf_msg.transform = get_transform_msg_from_airsim(echo_data.pose.position, echo_data.pose.orientation); + + convert_tf_msg_to_ros(echo_tf_msg); + + vehicle_ros->static_tf_msg_vec_.emplace_back(echo_tf_msg); +} + void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting) { geometry_msgs::msg::TransformStamped static_cam_tf_body_msg; @@ -1312,17 +1609,109 @@ void AirsimROSWrapper::lidar_timer_cb() try { for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { if (!vehicle_name_ptr_pair.second->lidar_pubs_.empty()) { + std::unordered_map sensor_names_to_lidar_data_map; for (auto& lidar_publisher : vehicle_name_ptr_pair.second->lidar_pubs_) { auto lidar_data = airsim_client_lidar_.getLidarData(lidar_publisher.sensor_name, vehicle_name_ptr_pair.first); + sensor_names_to_lidar_data_map[lidar_publisher.sensor_name] = lidar_data; sensor_msgs::msg::PointCloud2 lidar_msg = get_lidar_msg_from_airsim(lidar_data, vehicle_name_ptr_pair.first, lidar_publisher.sensor_name); lidar_publisher.publisher->publish(lidar_msg); } + for (auto& lidar_labels_publisher : vehicle_name_ptr_pair.second->lidar_labels_pubs_) { + msr::airlib::LidarData lidar_data; + auto it = sensor_names_to_lidar_data_map.find(lidar_labels_publisher.sensor_name); + if (it != sensor_names_to_lidar_data_map.end()) { + lidar_data = it->second; + } + else { + lidar_data = airsim_client_echo_.getLidarData(lidar_labels_publisher.sensor_name, vehicle_name_ptr_pair.first); + } + airsim_interfaces::msg::StringArray lidar_label_msg = get_lidar_labels_msg_from_airsim(lidar_data, vehicle_name_ptr_pair.first, lidar_labels_publisher.sensor_name); + lidar_labels_publisher.publisher->publish(lidar_label_msg); + } } } } catch (rpc::rpc_error& e) { std::string msg = e.get_error().as(); - RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, didn't get image response.\n%s", msg.c_str()); + RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, didn't get lidar response.\n%s", msg.c_str()); + } +} + +void AirsimROSWrapper::gpulidar_timer_cb() +{ + try { + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + if (!vehicle_name_ptr_pair.second->gpulidar_pubs_.empty()) { + for (auto& gpulidar_publisher : vehicle_name_ptr_pair.second->gpulidar_pubs_) { + auto gpulidar_data = airsim_client_gpulidar_.getGPULidarData(gpulidar_publisher.sensor_name, vehicle_name_ptr_pair.first); + sensor_msgs::msg::PointCloud2 gpulidar_msg = get_gpulidar_msg_from_airsim(gpulidar_data, vehicle_name_ptr_pair.first, gpulidar_publisher.sensor_name); + gpulidar_publisher.publisher->publish(gpulidar_msg); + } + } + } + } + catch (rpc::rpc_error& e) { + std::string msg = e.get_error().as(); + RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, didn't get gpulidar response.\n%s", msg.c_str()); + } +} + +void AirsimROSWrapper::echo_timer_cb() +{ + try { + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { + if (!vehicle_name_ptr_pair.second->echo_active_pubs_.empty() && !vehicle_name_ptr_pair.second->echo_passive_pubs_.empty()) { + std::unordered_map sensor_names_to_echo_data_map; + for (auto& active_echo_publisher : vehicle_name_ptr_pair.second->echo_active_pubs_) { + auto echo_data = airsim_client_echo_.getEchoData(active_echo_publisher.sensor_name, vehicle_name_ptr_pair.first); + sensor_names_to_echo_data_map[active_echo_publisher.sensor_name] = echo_data; + sensor_msgs::msg::PointCloud2 echo_msg = get_active_echo_msg_from_airsim(echo_data, vehicle_name_ptr_pair.first, active_echo_publisher.sensor_name); + active_echo_publisher.publisher->publish(echo_msg); + } + for (auto& passive_echo_publisher : vehicle_name_ptr_pair.second->echo_passive_pubs_) { + msr::airlib::EchoData echo_data; + auto it = sensor_names_to_echo_data_map.find(passive_echo_publisher.sensor_name); + if (it != sensor_names_to_echo_data_map.end()) { + echo_data = it->second; + } + else { + echo_data = airsim_client_echo_.getEchoData(passive_echo_publisher.sensor_name, vehicle_name_ptr_pair.first); + sensor_names_to_echo_data_map[passive_echo_publisher.sensor_name] = echo_data; + } + sensor_msgs::msg::PointCloud2 echo_msg = get_passive_echo_msg_from_airsim(echo_data, vehicle_name_ptr_pair.first, passive_echo_publisher.sensor_name); + passive_echo_publisher.publisher->publish(echo_msg); + } + for (auto& active_echo_labels_publisher : vehicle_name_ptr_pair.second->echo_active_labels_pubs_) { + msr::airlib::EchoData echo_data; + auto it = sensor_names_to_echo_data_map.find(active_echo_labels_publisher.sensor_name); + if (it != sensor_names_to_echo_data_map.end()) { + echo_data = it->second; + } + else { + echo_data = airsim_client_echo_.getEchoData(active_echo_labels_publisher.sensor_name, vehicle_name_ptr_pair.first); + sensor_names_to_echo_data_map[active_echo_labels_publisher.sensor_name] = echo_data; + } + airsim_interfaces::msg::StringArray echo_labels_msg = get_active_echo_labels_msg_from_airsim(echo_data, vehicle_name_ptr_pair.first, active_echo_labels_publisher.sensor_name); + active_echo_labels_publisher.publisher->publish(echo_labels_msg); + } + for (auto& passive_echo_labels_publisher : vehicle_name_ptr_pair.second->echo_passive_labels_pubs_) { + msr::airlib::EchoData echo_data; + auto it = sensor_names_to_echo_data_map.find(passive_echo_labels_publisher.sensor_name); + if (it != sensor_names_to_echo_data_map.end()) { + echo_data = it->second; + } + else { + echo_data = airsim_client_echo_.getEchoData(passive_echo_labels_publisher.sensor_name, vehicle_name_ptr_pair.first); + } + airsim_interfaces::msg::StringArray echo_labels_msg = get_passive_echo_labels_msg_from_airsim(echo_data, vehicle_name_ptr_pair.first, passive_echo_labels_publisher.sensor_name); + passive_echo_labels_publisher.publisher->publish(echo_labels_msg); + } + } + } + } + catch (rpc::rpc_error& e) { + std::string msg = e.get_error().as(); + RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, didn't get echo response.\n%s", msg.c_str()); } }