rewrite spawner to uavs spawn directly at location

include/ueds_connector/game_mode_controller.h

@@ -23,6 +23,8 @@ class GameModeController : public SocketClient {
 
   std::pair<bool, int> SpawnDrone();
 
+  std::pair<bool, int> SpawnDroneAtLocation(ueds_connector::Coordinates &Location);
+
   bool RemoveDrone(const int port);
 
   std::pair<bool, CameraCaptureModeEnum> GetCameraCaptureMode();
@@ -42,6 +44,8 @@ class GameModeController : public SocketClient {
   bool SetForestDensity(const int DensityLevel);
 
   bool SetForestHillyLevel(const int HillyLevel);
+
+  std::pair<bool, ueds_connector::Coordinates> GetWorldOrigin();
 };
 
 }  // namespace ueds_connector

include/ueds_connector/serialization/serializable_extended.h

@@ -7,6 +7,24 @@
 #include <ueds_connector/data_types.h>
 #include <ueds_connector/serialization/serializable_shared.h>
 
+namespace Serializable::GameMode::GetWorldOrigin
+{
+inline std::unique_ptr<ueds_connector::Coordinates> ResponseToCoordinates(std::unique_ptr<Response> response) {
+  return std::make_unique<ueds_connector::Coordinates>(response->x, response->y, response->z);
+}
+}
+
+namespace Serializable::GameMode::SpawnDroneAtLocation
+{
+inline std::unique_ptr<Request> CoordinateToRequest(const ueds_connector::Coordinates& coordinate) {
+  auto request = std::make_unique<Request>();
+  request->x   = coordinate.x;
+  request->y   = coordinate.y;
+  request->z   = coordinate.z;
+  return request;
+}
+} 
+
 namespace Serializable::Drone::GetLocation
 {
 inline std::unique_ptr<ueds_connector::Coordinates> ResponseToCoordinates(std::unique_ptr<Response> response) {

include/ueds_connector/serialization/serializable_shared.h

@@ -880,7 +880,9 @@ enum MessageType : unsigned short
   set_graphics_settings   = 10,
   switch_world_level      = 11,
   set_forest_density      = 12,
-  set_forest_hilly_level  = 13
+  set_forest_hilly_level  = 13,
+  get_world_origin        = 14,
+  spawn_drone_at_location = 15
 };
 
 namespace GetDrones
@@ -972,6 +974,35 @@ struct Response : public Common::NetworkResponse
 };
 }  // namespace SpawnDrone
 
+namespace SpawnDroneAtLocation
+{
+  struct Request : public Common::NetworkRequest
+  {
+    Request() : Common::NetworkRequest(MessageType::spawn_drone_at_location){};
+
+    double x;
+    double y;
+    double z;
+    template <class Archive>
+    void serialize(Archive& archive) {
+      archive(cereal::base_class<Common::NetworkRequest>(this), x, y, z);
+    }
+  };
+
+  struct Response : public Common::NetworkResponse
+  {
+    Response() : Common::NetworkResponse(static_cast<unsigned short>(MessageType::spawn_drone_at_location)){};
+    explicit Response(bool _status) : Common::NetworkResponse(MessageType::spawn_drone, _status){};
+
+    int port;
+
+    template <class Archive>
+    void serialize(Archive& archive) {
+      archive(cereal::base_class<Common::NetworkResponse>(this), port);
+    }
+  };
+};  // namespace SpawnDrone 
+
 namespace RemoveDrone
 {
 struct Request : public Common::NetworkRequest
@@ -1172,6 +1203,29 @@ struct Response : public Common::NetworkResponse
 };
 }  // namespace GetTime
 
+namespace GetWorldOrigin
+{
+  struct Request : public Common::NetworkRequest
+  {
+    Request() : Common::NetworkRequest(MessageType::get_world_origin){};
+  };
+
+  struct Response : public Common::NetworkResponse
+  {
+    Response() : Common::NetworkResponse(static_cast<unsigned short>(MessageType::get_world_origin)){};
+    explicit Response(bool _status) : Common::NetworkResponse(MessageType::get_world_origin, _status){};
+
+    double x;
+    double y;
+    double z;
+
+    template <class Archive>
+    void serialize(Archive& archive) {
+      archive(cereal::base_class<Common::NetworkResponse>(this), x, y, z);
+    }
+  };
+}  // namespace GetWorldOrigin
+
 }  // namespace GameMode
 
 }  // namespace Serializable

src/ueds_connector/game_mode_controller.cpp

@@ -24,6 +24,29 @@ std::pair<bool, std::vector<int>> GameModeController::GetDrones() {
 
 //}
 
+/* GetWorldOrigin() //{ */
+
+std::pair<bool, ueds_connector::Coordinates> GameModeController::GetWorldOrigin() {
+
+  Serializable::GameMode::GetWorldOrigin::Request request{};
+
+  Serializable::GameMode::GetWorldOrigin::Response response{};
+  const auto                                 status  = Request(request, response);
+  const auto                                 success = status && response.status;
+
+  ueds_connector::Coordinates coordinates{};
+
+  if (success) {
+    coordinates.x = response.x;
+    coordinates.y = response.y;
+    coordinates.z = response.z;
+  }
+
+  return std::make_pair(success, coordinates);
+}
+
+//}
+
 /* spawnDrone() //{ */
 
 std::pair<bool, int> GameModeController::SpawnDrone() {
@@ -39,6 +62,24 @@ std::pair<bool, int> GameModeController::SpawnDrone() {
 
 //}
 
+/* spawnDrone() //{ */
+
+std::pair<bool, int> GameModeController::SpawnDroneAtLocation(ueds_connector::Coordinates &Location) {
+
+  Serializable::GameMode::SpawnDroneAtLocation::Request request{};
+  request.x = Location.x;
+  request.y = Location.y;
+  request.z = Location.z;
+
+  Serializable::GameMode::SpawnDroneAtLocation::Response response{};
+  const auto                                   status  = Request(request, response);
+  const auto                                   success = status && response.status;
+
+  return std::make_pair(success, success ? response.port : 0);
+}
+
+//}
+
 /* removeDrone() //{ */
 
 bool GameModeController::RemoveDrone(const int port) {

src/unreal_simulator.cpp

@@ -225,8 +225,12 @@ class UnrealSimulator : public nodelet::Nodelet {
 
   std::mutex mutex_ueds_;
 
+  ueds_connector::Coordinates ueds_world_origin_;
+
   std::vector<ueds_connector::Coordinates> ueds_world_origins_;
 
+  ueds_connector::Coordinates position2ue(const Eigen::Vector3d &pos, const ueds_connector::Coordinates &ueds_world_origin);
+
   void updateUnrealPoses(const bool teleport_without_collision);
 
   void checkForCrash(void);
@@ -393,7 +397,8 @@ void UnrealSimulator::onInit() {
       break;
     }
 
-    ros::Duration(1.0).sleep();
+    //ros::Duration(1.0).sleep();
+    std::this_thread::sleep_for(std::chrono::seconds(1));
   }
 
   // | ------------------ check the API version ----------------- |
@@ -432,12 +437,8 @@ void UnrealSimulator::onInit() {
     ROS_ERROR("[UnrealSimulator] ueds_game_controller_ was not Disconnected succesfully.");
   }
 
-  ROS_WARN("SLEEP START");
-  // ros::Duration(1.0).sleep();
   std::this_thread::sleep_for(std::chrono::seconds(1));
-  ROS_WARN("SLEEP STOP");
 
-  // ueds_game_controller_ = std::make_unique<ueds_connector::GameModeController>(LOCALHOST, 8000);
   while (true) {
     bool connect_result = ueds_game_controller_->Connect();
     if (connect_result != 1) {
@@ -478,12 +479,27 @@ void UnrealSimulator::onInit() {
 
   // | --------------------- Spawn the UAVs --------------------- |
 
+  const auto [result, world_origin] = ueds_game_controller_->GetWorldOrigin();
+
+  if (!result) {
+    ROS_ERROR("[UnrealSimulator]: GameError: getting world origin");
+    ros::shutdown();
+  } else {
+    ueds_world_origin_ = world_origin;
+  }
+
   for (size_t i = 0; i < uav_names.size(); i++) {
 
     const std::string uav_name = uav_names[i];
 
-    ROS_INFO("[UnrealSimulator]: %s spawning .......", uav_name.c_str());
-    const auto [resSpawn, port] = ueds_game_controller_->SpawnDrone();
+    mrs_multirotor_simulator::MultirotorModel::State uav_state = uavs_[i]->getState();
+
+    ueds_connector::Coordinates pos = position2ue(uav_state.x, ueds_world_origin_);
+
+    ROS_INFO("[UnrealSimulator]: %s spawning at [%.2lf, %.2lf, %.2lf] ...", uav_name.c_str(), uav_state.x.x(), uav_state.x.y(), uav_state.x.z());
+
+    auto [resSpawn, port] = ueds_game_controller_->SpawnDroneAtLocation(pos);
+    //auto [resSpawn, port] = ueds_game_controller_->SpawnDrone();
 
     if (!resSpawn) {
       ROS_ERROR("[UnrealSimulator]: failed to spawn %s", uav_names[i].c_str());
@@ -496,26 +512,31 @@ void UnrealSimulator::onInit() {
 
     ueds_connectors_.push_back(ueds_connector);
 
+
     auto connect_result = ueds_connector->Connect();
 
     if (connect_result != 1) {
 
       ROS_ERROR("[UnrealSimulator]: %s - Error connecting to drone controller, connect_result was %d", uav_name.c_str(), connect_result);
       ros::shutdown();
 
-    } else {
+    } 
+    else {
 
       ROS_INFO("[UnrealSimulator]: %s - Connection succeed: %d", uav_name.c_str(), connect_result);
-
-      std::this_thread::sleep_for(std::chrono::seconds(3));
-      const auto [res, location] = ueds_connector->GetLocation();
-
-      if (!res) {
-        ROS_ERROR("[UnrealSimulator]: %s - DroneError: getting location", uav_name.c_str());
-        ros::shutdown();
-      } else {
-        ueds_world_origins_.push_back(location);
-      }
+
+      // ROS_INFO("[UnrealSimulator]: wait until UAV fall on the ground ... && uptade their world origin");
+
+      // std::this_thread::sleep_for(std::chrono::seconds(3));
+
+      // const auto [res, location] = ueds_connector->GetLocation();
+
+      // if (!res) {
+      //   ROS_ERROR("[UnrealSimulator]: %s - DroneError: getting location", uav_name.c_str());
+      //   ros::shutdown();
+      // } else {
+      //   ueds_world_origins_.push_back(location);
+      // }
     }
 
     ph_lidars_.push_back(mrs_lib::PublisherHandler<sensor_msgs::PointCloud2>(nh_, "/" + uav_name + "/lidar/points", 10));
@@ -610,9 +631,9 @@ void UnrealSimulator::onInit() {
     }
   }
 
-  ROS_INFO("[UnrealSimulator]: teleporting the UAVs to their spawn positions");
+  // ROS_INFO("[UnrealSimulator]: teleporting the UAVs to their spawn positions");
 
-  updateUnrealPoses(true);
+  // updateUnrealPoses(true);
 
   ROS_INFO("[UnrealSimulator]: Unreal UAVs are initialized");
 
@@ -902,7 +923,7 @@ void UnrealSimulator::timerLidar([[maybe_unused]] const ros::TimerEvent& event)
 
   for (size_t i = 0; i < uavs_.size(); i++) {
 
-    mrs_multirotor_simulator::MultirotorModel::State state = uavs_[i]->getState();
+    //mrs_multirotor_simulator::MultirotorModel::State state = uavs_[i]->getState();
 
     bool                                   res;
     std::vector<ueds_connector::LidarData> lidarData;
@@ -995,7 +1016,7 @@ void UnrealSimulator::timerSegLidar([[maybe_unused]] const ros::TimerEvent& even
 
   for (size_t i = 0; i < uavs_.size(); i++) {
 
-    mrs_multirotor_simulator::MultirotorModel::State state = uavs_[i]->getState();
+    //mrs_multirotor_simulator::MultirotorModel::State state = uavs_[i]->getState();
 
     bool                                      res;
     std::vector<ueds_connector::LidarSegData> lidarSegData;
@@ -1428,9 +1449,10 @@ void UnrealSimulator::updateUnrealPoses(const bool teleport_without_collision) {
 
       ueds_connector::Coordinates pos;
 
-      pos.x = ueds_world_origins_[i].x + state.x.x() * 100.0;
-      pos.y = ueds_world_origins_[i].y - state.x.y() * 100.0;
-      pos.z = ueds_world_origins_[i].z + state.x.z() * 100.0;
+      pos = position2ue(state.x, ueds_world_origin_);
+      // pos.x = ueds_world_origins_[i].x + state.x.x() * 100.0;
+      // pos.y = ueds_world_origins_[i].y - state.x.y() * 100.0;
+      // pos.z = ueds_world_origins_[i].z + state.x.z() * 100.0;
 
       ueds_connector::Rotation rot;
       rot.pitch = 180.0 * (-pitch / M_PI);
@@ -1442,6 +1464,16 @@ void UnrealSimulator::updateUnrealPoses(const bool teleport_without_collision) {
   }
 }
 
+ueds_connector::Coordinates UnrealSimulator::position2ue(const Eigen::Vector3d &pos, const ueds_connector::Coordinates &ueds_world_origin){
+  ueds_connector::Coordinates pos_ue;
+
+  pos_ue.x = ueds_world_origin.x + pos.x() * 100.0;
+  pos_ue.y = ueds_world_origin.y - pos.y() * 100.0;
+  pos_ue.z = ueds_world_origin.z + pos.z() * 100.0;
+
+  return pos_ue;
+}
+
 //}
 
 /* checkForCrash() //{ */

tmux/two_drones/config/simulator.yaml

@@ -1,3 +1,4 @@
+ueds_world_level_name_enum: 0
 frames:
   world:
     name: "simulator_origin"
@@ -30,8 +31,8 @@ uav2:
 
 sensors:
   lidar:
-    enabled: true
+    enabled: false
   rgb:
-    enabled: true
+    enabled: false
   stereo:
-    enabled: true
+    enabled: false