From f67e5dda65da0ebfd1d36ff3ad9a693ac3acf5cf Mon Sep 17 00:00:00 2001 From: Jan Hrncir Date: Thu, 19 Sep 2024 15:06:24 +0200 Subject: [PATCH] added rangefinder sensor --- cfg/unreal_simulator.cfg | 5 ++ config/unreal_simulator.yaml | 5 ++ .../serialization/serializable_shared.h | 22 +++++++++ include/ueds_connector/ueds_connector.h | 2 + src/ueds_connector/ueds_connector.cpp | 11 ++++- src/unreal_simulator.cpp | 49 +++++++++++++++++++ 6 files changed, 93 insertions(+), 1 deletion(-) diff --git a/cfg/unreal_simulator.cfg b/cfg/unreal_simulator.cfg index 4f14b35..194a5bf 100755 --- a/cfg/unreal_simulator.cfg +++ b/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) diff --git a/config/unreal_simulator.yaml b/config/unreal_simulator.yaml index 6e88397..25b308e 100644 --- a/config/unreal_simulator.yaml +++ b/config/unreal_simulator.yaml @@ -39,6 +39,11 @@ ueds_forest_hilly_level: 3 sensors: + rangefinder: + + enabled: true + rate: 10.0 # [Hz] + lidar: enabled: false diff --git a/include/ueds_connector/serialization/serializable_shared.h b/include/ueds_connector/serialization/serializable_shared.h index 9e5d7ff..779a661 100644 --- a/include/ueds_connector/serialization/serializable_shared.h +++ b/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(MessageType::get_rangefinder_data)){}; + }; + + struct Response : public Common::NetworkResponse + { + Response() : Common::NetworkResponse(static_cast(MessageType::get_rangefinder_data)){}; + explicit Response(bool _status) : Common::NetworkResponse(MessageType::get_rangefinder_data, _status){}; + + double range; + + template + void serialize(Archive& archive) { + archive(cereal::base_class(this), range); + } + }; +} // namespace GetRangefinderData + /* GetLidarData //{ */ namespace GetLidarData diff --git a/include/ueds_connector/ueds_connector.h b/include/ueds_connector/ueds_connector.h index a948a0e..bcfc953 100644 --- a/include/ueds_connector/ueds_connector.h +++ b/include/ueds_connector/ueds_connector.h @@ -41,6 +41,8 @@ class UedsConnector : public SocketClient { std::tuple SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation, const bool should_collide); + std::tuple GetRangefinderData(); + std::tuple, Coordinates> GetLidarData(); std::tuple, Coordinates> GetLidarSegData(); diff --git a/src/ueds_connector/ueds_connector.cpp b/src/ueds_connector/ueds_connector.cpp index e840ae1..3708f66 100644 --- a/src/ueds_connector/ueds_connector.cpp +++ b/src/ueds_connector/ueds_connector.cpp @@ -292,7 +292,16 @@ std::tuple, Coordinates> UedsConnector::GetLidarDat return std::make_tuple(success, lidarData, start); } -//} +std::tuple 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() //{ */ diff --git a/src/unreal_simulator.cpp b/src/unreal_simulator.cpp index 688922f..f4bb82f 100644 --- a/src/unreal_simulator.cpp +++ b/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 ph_clock_; mrs_lib::PublisherHandler ph_poses_; + std::vector> ph_rangefinders_; + std::vector> ph_lidars_; std::vector> 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(nh_, "/" + uav_name + "/rangefinder", 10)); + ph_lidars_.push_back(mrs_lib::PublisherHandler(nh_, "/" + uav_name + "/lidar/points", 10)); ph_seg_lidars_.push_back(mrs_lib::PublisherHandler(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) {