Added object transforms publish with service to refresh it on demand.…

… enabled by default but can be disabled in launch file

ros2/src/airsim_interfaces/CMakeLists.txt

@@ -27,6 +27,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ComputerVisionState.msg"
   "msg/InstanceSegmentationLabel.msg"
   "msg/InstanceSegmentationList.msg"
+  "msg/ObjectTransformsList.msg"
   "msg/StringArray.msg"
   "msg/Altimeter.msg"
   "msg/Environment.msg"
@@ -37,6 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/LandGroup.srv"
   "srv/Reset.srv"
   "srv/RefreshInstanceSegmentation.srv"
+  "srv/RefreshObjectTransforms.srv"
   "srv/SetLocalPosition.srv" DEPENDENCIES std_msgs geometry_msgs geographic_msgs LIBRARY_NAME ${PROJECT_NAME}
 )
 

ros2/src/airsim_interfaces/msg/ObjectTransformsList.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+geometry_msgs/TransformStamped[] objects

ros2/src/airsim_interfaces/srv/RefreshInstanceSegmentation.srv

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

ros2/src/airsim_interfaces/srv/RefreshObjectTransforms.srv

@@ -0,0 +1,2 @@
+---
+bool success

ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h

@@ -27,13 +27,15 @@ STRICT_MODE_OFF //todo what does this do?
 #include <airsim_interfaces/srv/takeoff.hpp>
 #include <airsim_interfaces/srv/takeoff_group.hpp>
 #include <airsim_interfaces/srv/refresh_instance_segmentation.hpp>
+#include <airsim_interfaces/srv/refresh_object_transforms.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/object_transforms_list.hpp>
 #include <airsim_interfaces/msg/string_array.hpp>
 #include <airsim_interfaces/msg/environment.hpp>
 #include <chrono>
@@ -160,7 +162,7 @@ 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::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(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, bool enable_object_transforms_list, uint16_t host_port);
     ~AirsimROSWrapper(){};
 
     void initialize_airsim();
@@ -184,6 +186,7 @@ class AirsimROSWrapper
         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_;
+        rclcpp::Publisher<airsim_interfaces::msg::ObjectTransformsList>::SharedPtr object_transforms_pub_;
         airsim_interfaces::msg::Environment env_msg_;
 
         std::vector<SensorPublisher<airsim_interfaces::msg::Altimeter>> barometer_pubs_;
@@ -207,6 +210,7 @@ class AirsimROSWrapper
         std::vector<geometry_msgs::msg::TransformStamped> static_tf_msg_vec_;
 
         rclcpp::Service<airsim_interfaces::srv::RefreshInstanceSegmentation>::SharedPtr instance_segmentation_refresh_srvr_;
+        rclcpp::Service<airsim_interfaces::srv::RefreshObjectTransforms>::SharedPtr object_transforms_refresh_srvr_;
 
         rclcpp::Time stamp_;
 
@@ -290,6 +294,7 @@ class AirsimROSWrapper
     bool land_all_srv_cb(const std::shared_ptr<airsim_interfaces::srv::Land::Request> request, const std::shared_ptr<airsim_interfaces::srv::Land::Response> response);
     bool reset_srv_cb(const std::shared_ptr<airsim_interfaces::srv::Reset::Request> request, const std::shared_ptr<airsim_interfaces::srv::Reset::Response> response);
     bool instance_segmentation_refresh_cb(const std::shared_ptr<airsim_interfaces::srv::RefreshInstanceSegmentation::Request> request, const std::shared_ptr<airsim_interfaces::srv::RefreshInstanceSegmentation::Response> response);
+    bool object_transforms_refresh_cb(const std::shared_ptr<airsim_interfaces::srv::RefreshObjectTransforms::Request> request, const std::shared_ptr<airsim_interfaces::srv::RefreshObjectTransforms::Response> response);
 
     /// ROS tf broadcasters
     void publish_odom_tf(const nav_msgs::msg::Odometry& odom_msg);
@@ -335,6 +340,7 @@ 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;
     airsim_interfaces::msg::InstanceSegmentationList get_instance_segmentation_list_msg_from_airsim() const;
+    airsim_interfaces::msg::ObjectTransformsList get_object_transforms_list_msg_from_airsim(rclcpp::Time timestamp) 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;
@@ -388,6 +394,7 @@ class AirsimROSWrapper
     std::string host_ip_;
     uint16_t host_port_;
     bool enable_api_control_;
+    bool enable_object_transforms_list_;
     std::unique_ptr<msr::airlib::RpcLibClientBase> airsim_client_;
     // seperate busy connections to airsim, update in their own thread
     msr::airlib::RpcLibClientBase airsim_client_images_;

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

@@ -34,6 +34,10 @@ def generate_launch_description():
     enable_api_control = DeclareLaunchArgument(
         "enable_api_control",
         default_value='False')
+
+    enable_object_transforms_list = DeclareLaunchArgument(
+        "enable_object_transforms_list",
+        default_value='True')
 
     airsim_node = Node(
             package='airsim_ros_pkgs',
@@ -50,7 +54,8 @@ def generate_launch_description():
                 'publish_clock': LaunchConfiguration('publish_clock'),
                 'host_ip': LaunchConfiguration('host_ip'),
                 'host_port': LaunchConfiguration('host_port'),
-                'enable_api_control': LaunchConfiguration('enable_api_control')
+                'enable_api_control': LaunchConfiguration('enable_api_control'),
+                'enable_object_transforms_list': LaunchConfiguration('enable_object_transforms_list')
             }])
 
     # Create the launch description and populate
@@ -63,6 +68,7 @@ def generate_launch_description():
     ld.add_action(host_ip)
     ld.add_action(host_port)
     ld.add_action(enable_api_control)
+    ld.add_action(enable_object_transforms_list)
     ld.add_action(airsim_node)
 
     return ld

ros2/src/airsim_ros_pkgs/src/airsim_node.cpp

@@ -14,11 +14,13 @@ int main(int argc, char** argv)
     std::string host_ip;
     uint16_t host_port = 41451;
     bool enable_api_control = false;
+    bool enable_object_transforms_list = true;
     nh->get_parameter("host_ip", host_ip);
     nh->get_parameter("host_port", host_port);
     nh->get_parameter("enable_api_control", enable_api_control);
+    nh->get_parameter("enable_object_transforms_list", enable_object_transforms_list);
     auto callbackGroup = nh->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-    AirsimROSWrapper airsim_ros_wrapper(nh, nh_img, nh_lidar, nh_gpulidar, nh_echo, 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, enable_object_transforms_list, host_port);
 
     rclcpp::executors::MultiThreadedExecutor executor;
     executor.add_node(nh);

ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp

@@ -28,7 +28,7 @@ const std::unordered_map<int, std::string> AirsimROSWrapper::image_type_int_to_s
 
 };
 
-AirsimROSWrapper::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::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, bool enable_object_transforms_list, 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)
@@ -37,6 +37,7 @@ AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr<rclcpp::Node> nh, const
     , host_ip_(host_ip)
     , host_port_(host_port)
     , enable_api_control_(enable_api_control)
+    , enable_object_transforms_list_(enable_object_transforms_list)
     , airsim_client_(nullptr)
     , airsim_client_images_(host_ip, host_port)
     , airsim_client_lidar_(host_ip, host_port)
@@ -103,10 +104,15 @@ void AirsimROSWrapper::initialize_airsim()
 
         auto vehicle_name_ptr_pair = vehicle_name_ptr_map_.begin();
         auto& vehicle_ros = vehicle_name_ptr_pair->second;
-        airsim_interfaces::msg::InstanceSegmentationList instance_segmentation_list_msg  = get_instance_segmentation_list_msg_from_airsim();      
+        airsim_interfaces::msg::InstanceSegmentationList instance_segmentation_list_msg = get_instance_segmentation_list_msg_from_airsim();      
         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);
+
+        if(enable_object_transforms_list_){
+            airsim_interfaces::msg::ObjectTransformsList object_transforms_list_msg = get_object_transforms_list_msg_from_airsim(vehicle_ros->stamp_);      
+            vehicle_ros->object_transforms_pub_->publish(object_transforms_list_msg);
+        }
     }
     catch (rpc::rpc_error& e) {
         std::string msg = e.get_error().as<std::string>();
@@ -184,12 +190,21 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
         vehicle_ros->env_pub_ = nh_->create_publisher<airsim_interfaces::msg::Environment>(topic_prefix + "/environment", 10);
 
         vehicle_ros->global_gps_pub_ = nh_->create_publisher<sensor_msgs::msg::NavSatFix>(topic_prefix + "/global_gps", 10);
+
         auto qos_settings = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
+
         vehicle_ros->instance_segmentation_pub_ = nh_->create_publisher<airsim_interfaces::msg::InstanceSegmentationList>("~/instance_segmentation_labels", qos_settings);
 
         std::function<bool(std::shared_ptr<airsim_interfaces::srv::RefreshInstanceSegmentation::Request>, std::shared_ptr<airsim_interfaces::srv::RefreshInstanceSegmentation::Response>)> fcn_ins_seg_refresh_srvr = std::bind(&AirsimROSWrapper::instance_segmentation_refresh_cb, this, _1, _2);
         vehicle_ros->instance_segmentation_refresh_srvr_ = nh_->create_service<airsim_interfaces::srv::RefreshInstanceSegmentation>(topic_prefix + "/instance_segmentation_refresh", fcn_ins_seg_refresh_srvr);
 
+        if(enable_object_transforms_list_){
+            vehicle_ros->object_transforms_pub_ = nh_->create_publisher<airsim_interfaces::msg::ObjectTransformsList>("~/object_transforms", qos_settings);
+
+            std::function<bool(std::shared_ptr<airsim_interfaces::srv::RefreshObjectTransforms::Request>, std::shared_ptr<airsim_interfaces::srv::RefreshObjectTransforms::Response>)> fcn_obj_trans_refresh_srvr = std::bind(&AirsimROSWrapper::object_transforms_refresh_cb, this, _1, _2);
+            vehicle_ros->object_transforms_refresh_srvr_ = nh_->create_service<airsim_interfaces::srv::RefreshObjectTransforms>(topic_prefix + "/object_transforms_refresh", fcn_obj_trans_refresh_srvr);
+        }
+
         if (airsim_mode_ == AIRSIM_MODE::DRONE) {
             auto drone = static_cast<MultiRotorROS*>(vehicle_ros.get());
 
@@ -498,6 +513,7 @@ bool AirsimROSWrapper::takeoff_all_srv_cb(std::shared_ptr<airsim_interfaces::srv
 bool AirsimROSWrapper::instance_segmentation_refresh_cb(const std::shared_ptr<airsim_interfaces::srv::RefreshInstanceSegmentation::Request> request, const std::shared_ptr<airsim_interfaces::srv::RefreshInstanceSegmentation::Response> response)
 {
     unused(response);
+    unused(request);
     std::lock_guard<std::mutex> guard(control_mutex_);
 
     RCLCPP_INFO_STREAM(nh_->get_logger(), "Starting instance segmentation refresh...");
@@ -509,7 +525,25 @@ bool AirsimROSWrapper::instance_segmentation_refresh_cb(const std::shared_ptr<ai
     instance_segmentation_list_msg.header.frame_id = world_frame_id_;
     vehicle_ros->instance_segmentation_pub_->publish(instance_segmentation_list_msg);
 
-        RCLCPP_INFO_STREAM(nh_->get_logger(), "Completed instance segmentation refresh!");
+    RCLCPP_INFO_STREAM(nh_->get_logger(), "Completed instance segmentation refresh!");
+
+    return true;
+}
+
+bool AirsimROSWrapper::object_transforms_refresh_cb(const std::shared_ptr<airsim_interfaces::srv::RefreshObjectTransforms::Request> request, const std::shared_ptr<airsim_interfaces::srv::RefreshObjectTransforms::Response> response)
+{
+    unused(response);
+    unused(request);
+    std::lock_guard<std::mutex> guard(control_mutex_);
+
+    RCLCPP_INFO_STREAM(nh_->get_logger(), "Starting object transforms refresh...");
+
+    auto vehicle_name_ptr_pair = vehicle_name_ptr_map_.begin();
+    auto& vehicle_ros = vehicle_name_ptr_pair->second;
+    airsim_interfaces::msg::ObjectTransformsList object_transforms_list_msg  = get_object_transforms_list_msg_from_airsim(vehicle_ros->stamp_);      
+    vehicle_ros->object_transforms_pub_->publish(object_transforms_list_msg);
+
+    RCLCPP_INFO_STREAM(nh_->get_logger(), "Completed object transforms refresh!");
 
     return true;
 }
@@ -1114,6 +1148,34 @@ airsim_interfaces::msg::InstanceSegmentationList AirsimROSWrapper::get_instance_
     return instance_segmentation_list_msg;
 }
 
+airsim_interfaces::msg::ObjectTransformsList AirsimROSWrapper::get_object_transforms_list_msg_from_airsim(rclcpp::Time timestamp) const{
+    airsim_interfaces::msg::ObjectTransformsList object_transforms_list_msg;
+    std::vector<std::string> object_name_list = airsim_client_->simListInstanceSegmentationObjects();
+    std::vector<msr::airlib::Pose> poses = airsim_client_->simListInstanceSegmentationPoses();
+    int object_index = 0;
+    std::vector<std::string>::iterator it = object_name_list.begin();
+    for(; it < object_name_list.end(); it++, object_index++ )
+    {
+        msr::airlib::Pose cur_object_pose = poses[object_index]; 
+        if(!std::isnan(cur_object_pose.position.x())){
+            geometry_msgs::msg::TransformStamped object_transform_msg;
+            object_transform_msg.child_frame_id = *it;
+            object_transform_msg.transform.translation.x = cur_object_pose.position.x();
+            object_transform_msg.transform.translation.y = -cur_object_pose.position.y();
+            object_transform_msg.transform.translation.z = -cur_object_pose.position.z();
+            object_transform_msg.transform.rotation.x = cur_object_pose.orientation.inverse().x();
+            object_transform_msg.transform.rotation.y = cur_object_pose.orientation.inverse().y();
+            object_transform_msg.transform.rotation.z = cur_object_pose.orientation.inverse().z();
+            object_transform_msg.transform.rotation.w = cur_object_pose.orientation.inverse().w();
+            object_transform_msg.header.stamp = timestamp;
+            object_transforms_list_msg.objects.push_back(object_transform_msg);            
+        }
+    }
+    object_transforms_list_msg.header.stamp = timestamp;
+    object_transforms_list_msg.header.frame_id = world_frame_id_;
+    return object_transforms_list_msg;
+}
+
 airsim_interfaces::msg::Altimeter AirsimROSWrapper::get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const
 {
     airsim_interfaces::msg::Altimeter alt_msg;