Skip to content

Commit

Permalink
Added working latched instance segmentation list publish with callabl…
Browse files Browse the repository at this point in the history 
…e service to refresh list
  • Loading branch information
@WouterJansen
WouterJansen committed Jul 8, 2024
1 parent 5ac5b35 commit 711642c
Show file tree
Hide file tree
Showing 2 changed files with 50 additions and 19 deletions.
4 changes: 4 additions & 0 deletions ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,6 +206,8 @@ class AirsimROSWrapper

std::vector<geometry_msgs::msg::TransformStamped> static_tf_msg_vec_;

rclcpp::Service<airsim_interfaces::srv::RefreshInstanceSegmentation>::SharedPtr instance_segmentation_refresh_srvr_;

rclcpp::Time stamp_;

std::string odom_frame_id_;
Expand Down Expand Up @@ -287,6 +289,7 @@ class AirsimROSWrapper
bool land_group_srv_cb(const std::shared_ptr<airsim_interfaces::srv::LandGroup::Request> request, const std::shared_ptr<airsim_interfaces::srv::LandGroup::Response> response);
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);

/// ROS tf broadcasters
void publish_odom_tf(const nav_msgs::msg::Odometry& odom_msg);
Expand Down Expand Up @@ -331,6 +334,7 @@ class AirsimROSWrapper
sensor_msgs::msg::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const;
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;
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;
Expand Down
65 changes: 46 additions & 19 deletions ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,26 +101,11 @@ void AirsimROSWrapper::initialize_airsim()
// 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);
}
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();
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) {
Expand Down Expand Up @@ -199,8 +184,11 @@ 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);

vehicle_ros->instance_segmentation_pub_ = nh_->create_publisher<airsim_interfaces::msg::InstanceSegmentationList>("~/instance_segmentation_labels", 10);
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 (airsim_mode_ == AIRSIM_MODE::DRONE) {
auto drone = static_cast<MultiRotorROS*>(vehicle_ros.get());
Expand Down Expand Up @@ -507,6 +495,25 @@ bool AirsimROSWrapper::takeoff_all_srv_cb(std::shared_ptr<airsim_interfaces::srv
return true;
}

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);
std::lock_guard<std::mutex> guard(control_mutex_);

RCLCPP_INFO_STREAM(nh_->get_logger(), "Starting instance segmentation refresh...");

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();
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);

RCLCPP_INFO_STREAM(nh_->get_logger(), "Completed instance segmentation refresh!");

return true;
}

bool AirsimROSWrapper::land_srv_cb(std::shared_ptr<airsim_interfaces::srv::Land::Request> request, std::shared_ptr<airsim_interfaces::srv::Land::Response> response, const std::string& vehicle_name)
{
unused(response);
Expand Down Expand Up @@ -1087,6 +1094,26 @@ sensor_msgs::msg::Range AirsimROSWrapper::get_range_from_airsim(const msr::airli
return dist_msg;
}


airsim_interfaces::msg::InstanceSegmentationList AirsimROSWrapper::get_instance_segmentation_list_msg_from_airsim() const{
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);
}
return instance_segmentation_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;
Expand Down

0 comments on commit 711642c

Please sign in to comment.