From 7cd569c22f925009591af50add2dc39a4f9c63c4 Mon Sep 17 00:00:00 2001 From: tomas Date: Sat, 22 Jun 2024 10:58:53 +0200 Subject: [PATCH] fixed drone spawning at any coordinates --- .../serialization/serializable_shared.h | 10 ++++-- include/ueds_connector/ueds_connector.h | 4 +-- src/ueds_connector/ueds_connector.cpp | 31 ++++++++++++------- src/unreal_simulator.cpp | 14 ++++++--- tmux/one_drone/config/simulator.yaml | 2 +- 5 files changed, 40 insertions(+), 21 deletions(-) diff --git a/include/ueds_connector/serialization/serializable_shared.h b/include/ueds_connector/serialization/serializable_shared.h index 20942a9..4182a8d 100644 --- 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 void serialize(Archive& archive) { - archive(cereal::base_class(this), x, y, z, pitch, yaw, roll); + archive(cereal::base_class(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 void serialize(Archive& archive) { - archive(cereal::base_class(this), x, y, z, pitch, yaw, roll); + archive(cereal::base_class(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 index 6a5f85e..a948a0e 100644 --- a/include/ueds_connector/ueds_connector.h +++ b/include/ueds_connector/ueds_connector.h @@ -37,9 +37,9 @@ class UedsConnector : public SocketClient { std::tuple SetRotation(const Rotation& rotation); - std::tuple SetLocationAndRotation(const Coordinates& coordinate, const Rotation& rotation); + std::tuple SetLocationAndRotation(const Coordinates& coordinate, const Rotation& rotation, const bool should_collide); - std::tuple SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation); + std::tuple SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation, const bool should_collide); std::tuple, Coordinates> GetLidarData(); diff --git a/src/ueds_connector/ueds_connector.cpp b/src/ueds_connector/ueds_connector.cpp index 5b12a23..e840ae1 100644 --- a/src/ueds_connector/ueds_connector.cpp +++ b/src/ueds_connector/ueds_connector.cpp @@ -191,16 +191,21 @@ std::tuple UedsConnector::SetRotation(const R /* setLocationAndRotation() //{ */ -std::tuple UedsConnector::SetLocationAndRotation(const Coordinates& coordinate, const Rotation& rotation) { +std::tuple 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 UedsConnector::SetLoc /* setLocationAndRotationAsync() //{ */ -std::tuple UedsConnector::SetLocationAndRotationAsync(const Coordinates& coordinate, const Rotation& rotation) { +std::tuple 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 index a62d75f..e4f2ccd 100644 --- 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_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 index c5eb338..a048489 100644 --- 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: