added rangefinder sensor

cfg/unreal_simulator.cfg

@@ -14,6 +14,11 @@ simulation.add("realtime_factor", double_t, 0, "Realtime factor", 0.0, 0.01, 10.
 simulation.add("dynamic_rtf", bool_t, 0, "Dynamic RTF", False)
 simulation.add("paused", bool_t, 0, "Paused", False)
 
+rangefinder = gen.add_group("Rangefinder")
+
+rangefinder.add("rangefinder_enabled", bool_t, 0, "Rangefinder enabled", True)
+rangefinder.add("rangefinder_rate", double_t, 0, "LiDAR rate", 1.0, 1.0, 200.0)
+
 lidar = gen.add_group("LiDAR");
 
 lidar.add("lidar_enabled", bool_t, 0, "LiDAR enabled", True)

config/unreal_simulator.yaml

@@ -39,6 +39,11 @@ ueds_forest_hilly_level: 3
 
 sensors:
 
+  rangefinder:
+
+    enabled: true
+    rate: 10.0 # [Hz]
+
   lidar:
 
     enabled: false

include/ueds_connector/serialization/serializable_shared.h

@@ -104,6 +104,7 @@ enum MessageType : unsigned short
   get_lidar_seg                   = 19,
   set_location_and_rotation_async = 20,
   get_crash_state                 = 21,
+  get_rangefinder_data            = 22
 };
 
 /* struct LidarConfig //{ */
@@ -541,6 +542,27 @@ struct Response : public Common::NetworkResponse
 
 //}
 
+namespace GetRangefinderData
+{
+  struct Request : public Common::NetworkRequest
+  {
+    Request() : Common::NetworkRequest(static_cast<unsigned short>(MessageType::get_rangefinder_data)){};
+  };
+
+  struct Response : public Common::NetworkResponse
+  {
+    Response() : Common::NetworkResponse(static_cast<unsigned short>(MessageType::get_rangefinder_data)){};
+    explicit Response(bool _status) : Common::NetworkResponse(MessageType::get_rangefinder_data, _status){};
+
+    double range;
+
+    template <class Archive>
+    void serialize(Archive& archive) {
+      archive(cereal::base_class<Common::NetworkResponse>(this), range);
+    }
+  };
+}  // namespace GetRangefinderData  
+
 /* GetLidarData //{ */
 
 namespace GetLidarData

include/ueds_connector/ueds_connector.h

@@ -41,6 +41,8 @@ class UedsConnector : public SocketClient {
 
   std::tuple<bool> SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation, const bool should_collide);
 
+  std::tuple<bool, double> GetRangefinderData();
+
   std::tuple<bool, std::vector<LidarData>, Coordinates> GetLidarData();
 
   std::tuple<bool, std::vector<LidarSegData>, Coordinates> GetLidarSegData();

src/ueds_connector/ueds_connector.cpp

@@ -292,7 +292,16 @@ std::tuple<bool, std::vector<LidarData>, Coordinates> UedsConnector::GetLidarDat
   return std::make_tuple(success, lidarData, start);
 }
 
-//}
+std::tuple<bool, double> ueds_connector::UedsConnector::GetRangefinderData()
+{
+  Serializable::Drone::GetRangefinderData::Request request{};
+
+  Serializable::Drone::GetRangefinderData::Response response{};
+  const auto                                        status  = Request(request, response);
+  const auto                                        success = status && response.status;
+
+  return std::make_pair(success, success ? response.range : -1);
+} //}
 
 /* getLidarSegData() //{ */
 

src/unreal_simulator.cpp

@@ -97,6 +97,9 @@ class UnrealSimulator : public nodelet::Nodelet {
   ros::Timer timer_unreal_sync_;
   void       timerUnrealSync(const ros::TimerEvent& event);
 
+  ros::Timer timer_rangefinder_;
+  void       timerRangefinder(const ros::TimerEvent& event);
+
   ros::Timer timer_lidar_;
   void       timerLidar(const ros::TimerEvent& event);
 
@@ -182,6 +185,8 @@ class UnrealSimulator : public nodelet::Nodelet {
   mrs_lib::PublisherHandler<rosgraph_msgs::Clock>     ph_clock_;
   mrs_lib::PublisherHandler<geometry_msgs::PoseArray> ph_poses_;
 
+  std::vector<mrs_lib::PublisherHandler<sensor_msgs::Range>>       ph_rangefinders_;
+
   std::vector<mrs_lib::PublisherHandler<sensor_msgs::PointCloud2>> ph_lidars_;
   std::vector<mrs_lib::PublisherHandler<sensor_msgs::PointCloud2>> ph_seg_lidars_;
 
@@ -308,6 +313,9 @@ void UnrealSimulator::onInit() {
 
   param_loader.loadParam("collisions", _collisions_);
 
+  param_loader.loadParam("sensors/rangefinder/enabled", drs_params_.rangefinder_enabled);
+  param_loader.loadParam("sensors/rangefinder/rate", drs_params_.rangefinder_rate);
+
   param_loader.loadParam("sensors/lidar/enabled", drs_params_.lidar_enabled);
   param_loader.loadParam("sensors/lidar/rate", drs_params_.lidar_rate);
   param_loader.loadParam("sensors/lidar/lidar_segmented/enabled", drs_params_.lidar_seg_enabled);
@@ -539,6 +547,8 @@ void UnrealSimulator::onInit() {
       // }
     }
 
+    ph_rangefinders_.push_back(mrs_lib::PublisherHandler<sensor_msgs::Range>(nh_, "/" + uav_name + "/rangefinder", 10));
+
     ph_lidars_.push_back(mrs_lib::PublisherHandler<sensor_msgs::PointCloud2>(nh_, "/" + uav_name + "/lidar/points", 10));
     ph_seg_lidars_.push_back(mrs_lib::PublisherHandler<sensor_msgs::PointCloud2>(nh_, "/" + uav_name + "/lidar_segmented/points", 10));
 
@@ -669,6 +679,8 @@ void UnrealSimulator::onInit() {
 
   timer_time_sync_ = nh_.createWallTimer(ros::WallDuration(1.0), &UnrealSimulator::timerTimeSync, this);
 
+  timer_rangefinder_ = nh_.createTimer(ros::Duration(1.0 / drs_params_.rangefinder_rate), &UnrealSimulator::timerRangefinder, this);
+
   timer_lidar_ = nh_.createTimer(ros::Duration(1.0 / drs_params_.lidar_rate), &UnrealSimulator::timerLidar, this);
 
   timer_unreal_sync_ = nh_.createTimer(ros::Duration(1.0 / _simulation_rate_), &UnrealSimulator::timerUnrealSync, this);
@@ -905,6 +917,43 @@ void UnrealSimulator::timerTimeSync([[maybe_unused]] const ros::WallTimerEvent&
 
 //}
 
+void UnrealSimulator::timerRangefinder([[maybe_unused]] const ros::TimerEvent& event){
+  if(!is_initialized_){
+    return;
+  }
+
+  auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
+
+  if (!drs_params_.rangefinder_enabled) {
+    return;
+  }
+
+  for(size_t i = 0; i < uavs_.size(); i++){
+    bool res;
+    double range;
+    {
+      std::scoped_lock lock(mutex_ueds_);
+
+      std::tie(res, range) = ueds_connectors_[i]->GetRangefinderData();
+    }
+
+    if(!res){
+      ROS_ERROR_THROTTLE(1.0, "[UnrealSimulator]: [uav%d] - ERROR GetRangefinderData", int(i));
+      continue;
+    }
+
+    sensor_msgs::Range msg_range;
+    msg_range.header.stamp    = ros::Time::now();
+    msg_range.header.frame_id = "uav" + std::to_string(i + 1) + "/fcu";
+    msg_range.radiation_type  = 1;
+    msg_range.min_range       = 0.1;
+    msg_range.max_range       = 30;
+    msg_range.range           = range / 100;
+
+    ph_rangefinders_[i].publish(msg_range);
+  }
+}
+
 /* timerLidar() //{ */
 
 void UnrealSimulator::timerLidar([[maybe_unused]] const ros::TimerEvent& event) {