first WIP part for instance segmentation publish

ros2/src/airsim_interfaces/CMakeLists.txt

@@ -25,6 +25,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/CarControls.msg"
   "msg/CarState.msg"
   "msg/ComputerVisionState.msg"
+  "msg/InstanceSegmentationLabel.msg"
+  "msg/InstanceSegmentationList.msg"
   "msg/StringArray.msg"
   "msg/Altimeter.msg"
   "msg/Environment.msg"
@@ -34,6 +36,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/Land.srv"
   "srv/LandGroup.srv"
   "srv/Reset.srv"
+  "srv/RefreshInstanceSegmentation.srv"
   "srv/SetLocalPosition.srv" DEPENDENCIES std_msgs geometry_msgs geographic_msgs LIBRARY_NAME ${PROJECT_NAME}
 )
 

ros2/src/airsim_interfaces/msg/InstanceSegmentationLabel.msg

@@ -0,0 +1,5 @@
+string name
+uint32 index
+uint8 r
+uint8 g
+uint8 b

ros2/src/airsim_interfaces/msg/InstanceSegmentationList.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+InstanceSegmentationLabel[] labels

ros2/src/airsim_interfaces/srv/RefreshInstanceSegmentation.srv

@@ -0,0 +1,3 @@
+bool wait_on_last_task
+---
+bool success

ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h

@@ -26,11 +26,14 @@ STRICT_MODE_OFF //todo what does this do?
 #include <airsim_interfaces/srv/reset.hpp>
 #include <airsim_interfaces/srv/takeoff.hpp>
 #include <airsim_interfaces/srv/takeoff_group.hpp>
+#include <airsim_interfaces/srv/refresh_instance_segmentation.hpp>
 #include <airsim_interfaces/msg/vel_cmd.hpp>
 #include <airsim_interfaces/msg/vel_cmd_group.hpp>
 #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/instance_segmentation_label.hpp>
+#include <airsim_interfaces/msg/instance_segmentation_list.hpp>
 #include <airsim_interfaces/msg/string_array.hpp>
 #include <airsim_interfaces/msg/environment.hpp>
 #include <chrono>
@@ -180,6 +183,7 @@ class AirsimROSWrapper
         rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_local_pub_;
         rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr global_gps_pub_;
         rclcpp::Publisher<airsim_interfaces::msg::Environment>::SharedPtr env_pub_;
+        rclcpp::Publisher<airsim_interfaces::msg::InstanceSegmentationList>::SharedPtr instance_segmentation_pub_;
         airsim_interfaces::msg::Environment env_msg_;
 
         std::vector<SensorPublisher<airsim_interfaces::msg::Altimeter>> barometer_pubs_;

ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp

@@ -100,6 +100,28 @@ void AirsimROSWrapper::initialize_airsim()
         origin_geo_point_ = get_origin_geo_point();
         // todo there's only one global origin geopoint for environment. but airsim API accept a parameter vehicle_name? inside carsimpawnapi.cpp, there's a geopoint being assigned in the constructor. by?
         origin_geo_point_msg_ = get_gps_msg_from_airsim_geo_point(origin_geo_point_);
+
+        auto& vehicle_ros = vehicle_name_ptr_map_.begin().second;
+
+        airsim_interfaces::msg::InstanceSegmentationList instance_segmentation_list_msg;
+        std::vector<std::string> object_name_list = airsim_client_->simListInstanceSegmentationObjects();
+        std::vector<msr::airlib::Vector3r> color_map = airsim_client_->simGetInstanceSegmentationColorMap();
+        int object_index = 0;
+        std::vector<std::string>::iterator it = object_name_list.begin();
+        for(; it < object_name_list.end(); it++, object_index++ )
+        {
+            airsim_interfaces::msg::InstanceSegmentationLabel instance_segmentation_label_msg;
+            instance_segmentation_label_msg.name = *it;
+            instance_segmentation_label_msg.r = color_map[object_index].x();
+            instance_segmentation_label_msg.g = color_map[object_index].y();
+            instance_segmentation_label_msg.b = color_map[object_index].z();
+            instance_segmentation_label_msg.index = object_index;
+            instance_segmentation_list_msg.labels.push_back(instance_segmentation_label_msg);
+        }
+        instance_segmentation_list_msg.header.stamp = vehicle_ros->stamp_;
+        instance_segmentation_list_msg.header.frame_id = world_frame_id_;
+
+        vehicle_ros->instance_segmentation_pub_->publish(instance_segmentation_list_msg);
     }
     catch (rpc::rpc_error& e) {
         std::string msg = e.get_error().as<std::string>();
@@ -178,6 +200,8 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
 
         vehicle_ros->global_gps_pub_ = nh_->create_publisher<sensor_msgs::msg::NavSatFix>(topic_prefix + "/global_gps", 10);
 
+        vehicle_ros->instance_segmentation_pub_ = nh_->create_publisher<airsim_interfaces::msg::InstanceSegmentationList>("~/instance_segmentation_labels", 10);
+
         if (airsim_mode_ == AIRSIM_MODE::DRONE) {
             auto drone = static_cast<MultiRotorROS*>(vehicle_ros.get());
 
@@ -357,7 +381,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
         }
 
         vehicle_name_ptr_map_.emplace(curr_vehicle_name, std::move(vehicle_ros)); // allows fast lookup in command callbacks in case of a lot of drones
-    }
+
+
+    }   
 
     // add takeoff and land all services if more than 2 drones
     if (vehicle_name_ptr_map_.size() > 1 && airsim_mode_ == AIRSIM_MODE::DRONE) {