fixed drone spawning at any coordinates

ctu-mrs · Jun 22, 2024 · 7cd569c · 7cd569c
1 parent cb8f09a
commit 7cd569c
Show file tree

Hide file tree

Showing 5 changed files with 40 additions and 21 deletions.
diff --git a/include/ueds_connector/serialization/serializable_shared.h b/include/ueds_connector/serialization/serializable_shared.h
@@ -457,13 +457,16 @@ struct Request : public Common::NetworkRequest
   double x;
   double y;
   double z;
+
   double pitch;
   double yaw;
   double roll;
 
+  bool should_collide;
+
   template <class Archive>
   void serialize(Archive& archive) {
-    archive(cereal::base_class<Common::NetworkRequest>(this), x, y, z, pitch, yaw, roll);
+    archive(cereal::base_class<Common::NetworkRequest>(this), x, y, z, pitch, yaw, roll, should_collide);
   }
 };
 
@@ -509,13 +512,16 @@ struct Request : public Common::NetworkRequest
   double x;
   double y;
   double z;
+
   double pitch;
   double yaw;
   double roll;
 
+  bool should_collide;
+
   template <class Archive>
   void serialize(Archive& archive) {
-    archive(cereal::base_class<Common::NetworkRequest>(this), x, y, z, pitch, yaw, roll);
+    archive(cereal::base_class<Common::NetworkRequest>(this), x, y, z, pitch, yaw, roll, should_collide);
   }
 };
 

diff --git a/include/ueds_connector/ueds_connector.h b/include/ueds_connector/ueds_connector.h
@@ -37,9 +37,9 @@ class UedsConnector : public SocketClient {
 
   std::tuple<bool, Rotation, bool, Coordinates> SetRotation(const Rotation& rotation);
 
-  std::tuple<bool, Coordinates, Rotation, bool, Coordinates> SetLocationAndRotation(const Coordinates& coordinate, const Rotation& rotation);
+  std::tuple<bool, Coordinates, Rotation, bool, Coordinates> SetLocationAndRotation(const Coordinates& coordinate, const Rotation& rotation, const bool should_collide);
 
-  std::tuple<bool> SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation);
+  std::tuple<bool> SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation, const bool should_collide);
 
   std::tuple<bool, std::vector<LidarData>, Coordinates> GetLidarData();
 

diff --git a/src/ueds_connector/ueds_connector.cpp b/src/ueds_connector/ueds_connector.cpp
@@ -191,16 +191,21 @@ std::tuple<bool, Rotation, bool, Coordinates> UedsConnector::SetRotation(const R
 
 /* setLocationAndRotation() //{ */
 
-std::tuple<bool, Coordinates, Rotation, bool, Coordinates> UedsConnector::SetLocationAndRotation(const Coordinates& coordinate, const Rotation& rotation) {
+std::tuple<bool, Coordinates, Rotation, bool, Coordinates> UedsConnector::SetLocationAndRotation(const Coordinates& coordinate, const Rotation& rotation,
+                                                                                                 const bool should_collide) {
 
   Serializable::Drone::SetLocationAndRotation::Request request{};
-  request.x     = coordinate.x;
-  request.y     = coordinate.y;
-  request.z     = coordinate.z;
+
+  request.x = coordinate.x;
+  request.y = coordinate.y;
+  request.z = coordinate.z;
+
   request.pitch = rotation.pitch;
   request.yaw   = rotation.yaw;
   request.roll  = rotation.roll;
 
+  request.should_collide = should_collide;
+
   Serializable::Drone::SetLocationAndRotation::Response response{};
   const auto                                            status  = Request(request, response);
   const auto                                            success = status && response.status;
@@ -230,19 +235,23 @@ std::tuple<bool, Coordinates, Rotation, bool, Coordinates> UedsConnector::SetLoc
 
 /* setLocationAndRotationAsync() //{ */
 
-std::tuple<bool> UedsConnector::SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation) {
+std::tuple<bool> UedsConnector::SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation, const bool should_collide) {
 
   Serializable::Drone::SetLocationAndRotationAsync::Request request{};
-  request.x     = coordinate.x;
-  request.y     = coordinate.y;
-  request.z     = coordinate.z;
+
+  request.x = coordinate.x;
+  request.y = coordinate.y;
+  request.z = coordinate.z;
+
   request.pitch = rotation.pitch;
   request.yaw   = rotation.yaw;
   request.roll  = rotation.roll;
 
-  Serializable::Drone::SetLocationAndRotation::Response response{};
-  const auto                                            status  = Request(request, response);
-  const auto                                            success = status && response.status;
+  request.should_collide = should_collide;
+
+  Serializable::Drone::SetLocationAndRotationAsync::Response response{};
+  const auto                                                 status  = Request(request, response);
+  const auto                                                 success = status && response.status;
 
   return std::make_tuple(success);
 }

diff --git a/src/unreal_simulator.cpp b/src/unreal_simulator.cpp
@@ -48,7 +48,7 @@
 
 /* defines //{ */
 
-#define API_VERSION 1
+#define API_VERSION 2
 
 //}
 
@@ -221,7 +221,7 @@ class UnrealSimulator : public nodelet::Nodelet {
 
   std::vector<ueds_connector::Coordinates> ueds_world_origins_;
 
-  void updateUnrealPoses(void);
+  void updateUnrealPoses(const bool teleport_without_collision);
 
   void checkForCrash(void);
 
@@ -532,6 +532,10 @@ void UnrealSimulator::onInit() {
     }
   }
 
+  ROS_INFO("[UnrealSimulator]: teleporting the UAVs to their spawn positions");
+
+  updateUnrealPoses(true);
+
   ROS_INFO("[UnrealSimulator]: Unreal UAVs are initialized");
 
   // | --------------- dynamic reconfigure server --------------- |
@@ -727,7 +731,7 @@ void UnrealSimulator::timerUnrealSync([[maybe_unused]] const ros::TimerEvent& ev
     return;
   }
 
-  updateUnrealPoses();
+  updateUnrealPoses(false);
 }
 
 //}
@@ -1306,7 +1310,7 @@ void UnrealSimulator::publishPoses(void) {
 
 /* updateUnrealPoses() //{ */
 
-void UnrealSimulator::updateUnrealPoses(void) {
+void UnrealSimulator::updateUnrealPoses(const bool teleport_without_collision) {
 
   // | ------------ set each UAV's position in unreal ----------- |
 
@@ -1330,7 +1334,7 @@ void UnrealSimulator::updateUnrealPoses(void) {
       rot.roll  = 180.0 * (roll / M_PI);
       rot.yaw   = 180.0 * (-yaw / M_PI);
 
-      ueds_connectors_[i]->SetLocationAndRotationAsync(pos, rot);
+      ueds_connectors_[i]->SetLocationAndRotationAsync(pos, rot, !teleport_without_collision && _collisions_);
     }
   }
 }

diff --git a/tmux/one_drone/config/simulator.yaml b/tmux/one_drone/config/simulator.yaml
@@ -8,7 +8,7 @@ uav1:
     x: 0.0
     y: 0.0
     z: 0.0
-    heading: 0
+    heading: 0.0
 
 sensors:
   lidar: