From a76665cb19ec0bc8442d9a019543333a410dad2f Mon Sep 17 00:00:00 2001 From: Wouter Jansen Date: Fri, 5 Jul 2024 16:12:06 +0200 Subject: [PATCH] first WIP part for instance segmentation publish --- ros2/src/airsim_interfaces/CMakeLists.txt | 3 ++ .../msg/InstanceSegmentationLabel.msg | 5 ++++ .../msg/InstanceSegmentationList.msg | 2 ++ .../srv/RefreshInstanceSegmentation.srv | 3 ++ .../include/airsim_ros_wrapper.h | 4 +++ .../src/airsim_ros_wrapper.cpp | 28 ++++++++++++++++++- 6 files changed, 44 insertions(+), 1 deletion(-) create mode 100755 ros2/src/airsim_interfaces/msg/InstanceSegmentationLabel.msg create mode 100755 ros2/src/airsim_interfaces/msg/InstanceSegmentationList.msg create mode 100755 ros2/src/airsim_interfaces/srv/RefreshInstanceSegmentation.srv diff --git a/ros2/src/airsim_interfaces/CMakeLists.txt b/ros2/src/airsim_interfaces/CMakeLists.txt index 8acf04ab9..ecae3efd7 100644 --- a/ros2/src/airsim_interfaces/CMakeLists.txt +++ b/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} ) diff --git a/ros2/src/airsim_interfaces/msg/InstanceSegmentationLabel.msg b/ros2/src/airsim_interfaces/msg/InstanceSegmentationLabel.msg new file mode 100755 index 000000000..9489295d4 --- /dev/null +++ b/ros2/src/airsim_interfaces/msg/InstanceSegmentationLabel.msg @@ -0,0 +1,5 @@ +string name +uint32 index +uint8 r +uint8 g +uint8 b \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/InstanceSegmentationList.msg b/ros2/src/airsim_interfaces/msg/InstanceSegmentationList.msg new file mode 100755 index 000000000..f472e869e --- /dev/null +++ b/ros2/src/airsim_interfaces/msg/InstanceSegmentationList.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +InstanceSegmentationLabel[] labels \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/RefreshInstanceSegmentation.srv b/ros2/src/airsim_interfaces/srv/RefreshInstanceSegmentation.srv new file mode 100755 index 000000000..2100b641c --- /dev/null +++ b/ros2/src/airsim_interfaces/srv/RefreshInstanceSegmentation.srv @@ -0,0 +1,3 @@ +bool wait_on_last_task +--- +bool success \ No newline at end of file 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 47b9a9761..51103a8ff 100755 --- a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -26,11 +26,14 @@ STRICT_MODE_OFF //todo what does this do? #include #include #include +#include #include #include #include #include #include +#include +#include #include #include #include @@ -180,6 +183,7 @@ class AirsimROSWrapper rclcpp::Publisher::SharedPtr odom_local_pub_; rclcpp::Publisher::SharedPtr global_gps_pub_; rclcpp::Publisher::SharedPtr env_pub_; + rclcpp::Publisher::SharedPtr instance_segmentation_pub_; airsim_interfaces::msg::Environment env_msg_; std::vector> barometer_pubs_; 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 6df83e27e..c7acd5e69 100755 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/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 object_name_list = airsim_client_->simListInstanceSegmentationObjects(); + std::vector color_map = airsim_client_->simGetInstanceSegmentationColorMap(); + int object_index = 0; + std::vector::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(); @@ -178,6 +200,8 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() vehicle_ros->global_gps_pub_ = nh_->create_publisher(topic_prefix + "/global_gps", 10); + vehicle_ros->instance_segmentation_pub_ = nh_->create_publisher("~/instance_segmentation_labels", 10); + if (airsim_mode_ == AIRSIM_MODE::DRONE) { auto drone = static_cast(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) {