From 26d42b6dc2d433a465cb514d0c132f5a64fdfd07 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Thu, 27 Jul 2017 16:47:02 +0200 Subject: [PATCH] Changes in the protocol, use repeated and add option for non-player agents --- Source/Carla/Game/CarlaPlayerState.cpp | 3 +- Source/Carla/Game/CarlaPlayerState.h | 19 ++--- Source/Carla/Game/CarlaServer.cpp | 31 ++++---- Source/Carla/Game/CarlaVehicleController.cpp | 16 +++-- Source/Carla/Game/CarlaVehicleController.h | 3 + Util/CarlaServer/include/carla/carla_server.h | 37 ++++++++-- Util/CarlaServer/source/carla/ArrayView.h | 14 ++++ .../source/carla/server/CarlaEncoder.cpp | 71 +++++++++++++++---- .../source/test/Test_CarlaServer.cpp | 10 +-- Util/Proto/carla_server.proto | 55 ++++++++++++-- 10 files changed, 197 insertions(+), 62 deletions(-) diff --git a/Source/Carla/Game/CarlaPlayerState.cpp b/Source/Carla/Game/CarlaPlayerState.cpp index c282cb9dd..a4a1a5f10 100644 --- a/Source/Carla/Game/CarlaPlayerState.cpp +++ b/Source/Carla/Game/CarlaPlayerState.cpp @@ -24,8 +24,7 @@ void ACarlaPlayerState::CopyProperties(APlayerState *PlayerState) { PlatformTimeStamp = Other->PlatformTimeStamp; GameTimeStamp = Other->GameTimeStamp; - Location = Other->Location; - Orientation = Other->Orientation; + Transform = Other->Transform; Acceleration = Other->Acceleration; ForwardSpeed = Other->ForwardSpeed; CollisionIntensityCars = Other->CollisionIntensityCars; diff --git a/Source/Carla/Game/CarlaPlayerState.h b/Source/Carla/Game/CarlaPlayerState.h index 17c8a74e8..a78044a97 100644 --- a/Source/Carla/Game/CarlaPlayerState.h +++ b/Source/Carla/Game/CarlaPlayerState.h @@ -54,15 +54,21 @@ public: } UFUNCTION() - const FVector &GetLocation() const + const FTransform &GetTransform() const { - return Location; + return Transform; } UFUNCTION() - const FVector &GetOrientation() const + FVector GetLocation() const { - return Orientation; + return Transform.GetLocation(); + } + + UFUNCTION() + FVector GetOrientation() const + { + return Transform.GetRotation().GetForwardVector(); } UFUNCTION() @@ -164,10 +170,7 @@ private: int32 GameTimeStamp = 0.0f; UPROPERTY(VisibleAnywhere) - FVector Location; - - UPROPERTY(VisibleAnywhere) - FVector Orientation; + FTransform Transform; UPROPERTY(VisibleAnywhere) FVector Acceleration; diff --git a/Source/Carla/Game/CarlaServer.cpp b/Source/Carla/Game/CarlaServer.cpp index 2a9e37166..dad29a70c 100644 --- a/Source/Carla/Game/CarlaServer.cpp +++ b/Source/Carla/Game/CarlaServer.cpp @@ -41,9 +41,15 @@ static inline void Set(float &lhs, float rhs) lhs = rhs; } -static inline void Set(carla_vector3d &cVector, const FVector &uVector) +static inline void Set(carla_vector3d &lhs, const FVector &rhs) { - cVector = {uVector.X, uVector.Y, uVector.Z}; + lhs = {rhs.X, rhs.Y, rhs.Z}; +} + +static inline void Set(carla_transform &lhs, const FTransform &rhs) +{ + Set(lhs.location, rhs.GetLocation()); + Set(lhs.orientation, rhs.GetRotation().GetForwardVector()); } static void Set(carla_image &cImage, const FCapturedImage &uImage) @@ -92,7 +98,7 @@ CarlaServer::~CarlaServer() CarlaServer::ErrorCode CarlaServer::Connect() { - UE_LOG(LogCarlaServer, Log, TEXT("Waiting for client to connect...")); + UE_LOG(LogCarlaServer, Log, TEXT("Waiting for the client to connect...")); return ParseErrorCode(carla_server_connect(Server, WorldPort, TimeOut)); } @@ -115,17 +121,17 @@ CarlaServer::ErrorCode CarlaServer::SendSceneDescription( const TArray &AvailableStartSpots, const bool bBlocking) { - const int32 NumberOfStartPositions = AvailableStartSpots.Num(); - auto Positions = MakeUnique(NumberOfStartPositions); + const int32 NumberOfStartSpots = AvailableStartSpots.Num(); + auto StartSpots = MakeUnique(NumberOfStartSpots); - for (auto i = 0u; i < NumberOfStartPositions; ++i) { - Set(Positions[i], AvailableStartSpots[i]->GetActorLocation()); + for (auto i = 0u; i < NumberOfStartSpots; ++i) { + Set(StartSpots[i], AvailableStartSpots[i]->GetActorTransform()); } - UE_LOG(LogCarlaServer, Log, TEXT("Sending %d available start positions"), NumberOfStartPositions); + UE_LOG(LogCarlaServer, Log, TEXT("Sending %d available start positions"), NumberOfStartSpots); carla_scene_description scene; - scene.player_start_locations = Positions.Get(); - scene.number_of_player_start_locations = NumberOfStartPositions; + scene.player_start_spots = StartSpots.Get(); + scene.number_of_player_start_spots = NumberOfStartSpots; return ParseErrorCode(carla_write_scene_description(Server, scene, GetTimeOut(TimeOut, bBlocking))); } @@ -135,7 +141,7 @@ CarlaServer::ErrorCode CarlaServer::ReadEpisodeStart(uint32 &StartPositionIndex, carla_episode_start values; auto ec = ParseErrorCode(carla_read_episode_start(Server, values, GetTimeOut(TimeOut, bBlocking))); if (Success == ec) { - StartPositionIndex = values.player_start_location_index; + StartPositionIndex = values.player_start_spot_index; UE_LOG(LogCarlaServer, Log, TEXT("Episode start received: { StartIndex = %d }"), StartPositionIndex); } return ec; @@ -182,8 +188,7 @@ CarlaServer::ErrorCode CarlaServer::SendMeasurements(const ACarlaPlayerState &Pl values.platform_timestamp = PlayerState.GetPlatformTimeStamp(); values.game_timestamp = PlayerState.GetGameTimeStamp(); auto &player = values.player_measurements; - Set(player.location, PlayerState.GetLocation()); - Set(player.orientation, PlayerState.GetOrientation()); + Set(player.transform, PlayerState.GetTransform()); Set(player.acceleration, PlayerState.GetAcceleration()); Set(player.forward_speed, PlayerState.GetForwardSpeed()); Set(player.collision_vehicles, PlayerState.GetCollisionIntensityCars()); diff --git a/Source/Carla/Game/CarlaVehicleController.cpp b/Source/Carla/Game/CarlaVehicleController.cpp index e5886b48f..18c69ac8f 100644 --- a/Source/Carla/Game/CarlaVehicleController.cpp +++ b/Source/Carla/Game/CarlaVehicleController.cpp @@ -107,11 +107,10 @@ void ACarlaVehicleController::Tick(float DeltaTime) if (IsPossessingAVehicle()) { CarlaPlayerState->UpdateTimeStamp(DeltaTime); - CarlaPlayerState->Location = GetVehicleLocation(); - const FVector PreviousSpeed = CarlaPlayerState->ForwardSpeed * CarlaPlayerState->Orientation; - CarlaPlayerState->Orientation = GetVehicleOrientation(); + const FVector PreviousSpeed = CarlaPlayerState->ForwardSpeed * CarlaPlayerState->GetOrientation(); + CarlaPlayerState->Transform = GetVehicleTransform(); CarlaPlayerState->ForwardSpeed = GetVehicleForwardSpeed(); - const FVector CurrentSpeed = CarlaPlayerState->ForwardSpeed * CarlaPlayerState->Orientation; + const FVector CurrentSpeed = CarlaPlayerState->ForwardSpeed * CarlaPlayerState->GetOrientation(); CarlaPlayerState->Acceleration = (CurrentSpeed - PreviousSpeed) / DeltaTime; CarlaPlayerState->CurrentGear = GetVehicleCurrentGear(); IntersectPlayerWithRoadMap(); @@ -130,6 +129,12 @@ void ACarlaVehicleController::Tick(float DeltaTime) // -- Vehicle pawn info -------------------------------------------------------- // ============================================================================= +FTransform ACarlaVehicleController::GetVehicleTransform() const +{ + check(GetPawn() != nullptr); + return GetPawn()->GetActorTransform(); +} + FVector ACarlaVehicleController::GetVehicleLocation() const { check(GetPawn() != nullptr); @@ -144,8 +149,7 @@ float ACarlaVehicleController::GetVehicleForwardSpeed() const FVector ACarlaVehicleController::GetVehicleOrientation() const { - check(GetPawn() != nullptr); - return GetPawn()->GetTransform().GetRotation().GetForwardVector(); + return GetVehicleTransform().GetRotation().GetForwardVector(); } int32 ACarlaVehicleController::GetVehicleCurrentGear() const diff --git a/Source/Carla/Game/CarlaVehicleController.h b/Source/Carla/Game/CarlaVehicleController.h index c2497d726..0289d3e67 100644 --- a/Source/Carla/Game/CarlaVehicleController.h +++ b/Source/Carla/Game/CarlaVehicleController.h @@ -58,6 +58,9 @@ public: return MovementComponent != nullptr; } + /// World transform of the vehicle. + FTransform GetVehicleTransform() const; + /// World location of the vehicle. FVector GetVehicleLocation() const; diff --git a/Util/CarlaServer/include/carla/carla_server.h b/Util/CarlaServer/include/carla/carla_server.h index c7dd018e7..9f68fc539 100644 --- a/Util/CarlaServer/include/carla/carla_server.h +++ b/Util/CarlaServer/include/carla/carla_server.h @@ -30,6 +30,28 @@ extern "C" { const uint32_t *data; }; + struct carla_transform { + struct carla_vector3d location; + struct carla_vector3d orientation; + }; + + struct carla_bounding_box { + struct carla_transform transform; + struct carla_vector3d extent; + }; + +#define CARLA_SERVER_AGENT_UNKNOWN 0u +#define CARLA_SERVER_AGENT_VEHICLE 1u +#define CARLA_SERVER_AGENT_PEDESTRIAN 2u + + struct carla_agent { + uint32_t id; + uint32_t type; + struct carla_transform transform; + struct carla_bounding_box bounding_box; + float forward_speed; + }; + /* ======================================================================== */ /* -- carla_request_new_episode ------------------------------------------- */ /* ======================================================================== */ @@ -52,8 +74,8 @@ extern "C" { struct carla_scene_description { /** Collection of the initial player start locations. */ - const struct carla_vector3d *player_start_locations; - uint32_t number_of_player_start_locations; + const struct carla_transform *player_start_spots; + uint32_t number_of_player_start_spots; }; /* ======================================================================== */ @@ -61,7 +83,7 @@ extern "C" { /* ======================================================================== */ struct carla_episode_start { - uint32_t player_start_location_index; + uint32_t player_start_spot_index; }; /* ======================================================================== */ @@ -89,10 +111,8 @@ extern "C" { /* ======================================================================== */ struct carla_player_measurements { - /** World location of the player. */ - struct carla_vector3d location; - /** Orientation of the player. */ - struct carla_vector3d orientation; + /** World transform of the player. */ + struct carla_transform transform; /** Current acceleration of the player. */ struct carla_vector3d acceleration; /** Forward speed in km/h. */ @@ -120,6 +140,9 @@ extern "C" { uint32_t game_timestamp; /** Player measurements. */ struct carla_player_measurements player_measurements; + /** Non-player agents. */ + const struct carla_agent *non_player_agents; + uint32_t number_of_non_player_agents; }; /* ======================================================================== */ diff --git a/Util/CarlaServer/source/carla/ArrayView.h b/Util/CarlaServer/source/carla/ArrayView.h index 6981ec964..3373002f2 100644 --- a/Util/CarlaServer/source/carla/ArrayView.h +++ b/Util/CarlaServer/source/carla/ArrayView.h @@ -91,4 +91,18 @@ namespace detail { template using const_array_view = detail::ArrayView>; +namespace array_view { + + template > + static inline auto make_mutable(T *data, typename V::size_type size) { + return V(data, size); + } + + template > + static inline auto make_const(const T *data, typename V::size_type size) { + return V(data, size); + } + +} // namespace array_view + } // namespace carla diff --git a/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp b/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp index 4fccbeb68..8120d087a 100644 --- a/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp +++ b/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp @@ -4,6 +4,7 @@ #include +#include "carla/ArrayView.h" #include "carla/Debug.h" #include "carla/Logging.h" @@ -14,12 +15,56 @@ namespace cs = carla_server; namespace carla { namespace server { + static auto start_spots(const carla_scene_description &values) { + return array_view::make_const(values.player_start_spots, values.number_of_player_start_spots); + } + + static auto agents(const carla_measurements &values) { + return array_view::make_const(values.non_player_agents, values.number_of_non_player_agents); + } + + static void Set(cs::Vector3D *lhs, const carla_vector3d &rhs) { + DEBUG_ASSERT(lhs != nullptr); + lhs->set_x(rhs.x); + lhs->set_y(rhs.y); + lhs->set_z(rhs.z); + } + + static void Set(cs::Transform *lhs, const carla_transform &rhs) { + DEBUG_ASSERT(lhs != nullptr); + Set(lhs->mutable_location(), rhs.location); + Set(lhs->mutable_orientation(), rhs.orientation); + } + + static void Set(cs::BoundingBox *lhs, const carla_bounding_box &rhs) { + DEBUG_ASSERT(lhs != nullptr); + Set(lhs->mutable_transform(), rhs.transform); + Set(lhs->mutable_extent(), rhs.extent); + } + + static cs::Agent::Type GetAgentType(const uint32_t type) { + switch (type) { + case CARLA_SERVER_AGENT_VEHICLE: return cs::Agent::VEHICLE; + case CARLA_SERVER_AGENT_PEDESTRIAN: return cs::Agent::PEDESTRIAN; + default: return cs::Agent::UNKNOWN; + } + } + + static void Set(cs::Agent *lhs, const carla_agent &rhs) { + DEBUG_ASSERT(lhs != nullptr); + lhs->set_id(rhs.id); + lhs->set_type(GetAgentType(rhs.type)); + Set(lhs->mutable_transform(), rhs.transform); + Set(lhs->mutable_bounding_box(), rhs.bounding_box); + lhs->set_forward_speed(rhs.forward_speed); + } + std::string CarlaEncoder::Encode(const carla_scene_description &values) { auto *message = _protobuf.CreateMessage(); DEBUG_ASSERT(message != nullptr); - message->set_player_start_locations( - values.player_start_locations, - sizeof(carla_vector3d) * values.number_of_player_start_locations); + for (auto &spot : start_spots(values)) { + Set(message->add_player_start_spots(), spot); + } return Protobuf::Encode(*message); } @@ -30,29 +75,27 @@ namespace server { return Protobuf::Encode(*message); } - static void SetVector3D(cs::Vector3D *lhs, const carla_vector3d &rhs) { - DEBUG_ASSERT(lhs != nullptr); - lhs->set_x(rhs.x); - lhs->set_y(rhs.y); - lhs->set_z(rhs.z); - } - std::string CarlaEncoder::Encode(const carla_measurements &values) { static thread_local auto *message = _protobuf.CreateMessage(); DEBUG_ASSERT(message != nullptr); message->set_platform_timestamp(values.platform_timestamp); message->set_game_timestamp(values.game_timestamp); + // Player measurements. auto *player = message->mutable_player_measurements(); DEBUG_ASSERT(player != nullptr); - SetVector3D(player->mutable_location(), values.player_measurements.location); - SetVector3D(player->mutable_orientation(), values.player_measurements.orientation); - SetVector3D(player->mutable_acceleration(), values.player_measurements.acceleration); + Set(player->mutable_transform(), values.player_measurements.transform); + Set(player->mutable_acceleration(), values.player_measurements.acceleration); player->set_forward_speed(values.player_measurements.forward_speed); player->set_collision_vehicles(values.player_measurements.collision_vehicles); player->set_collision_pedestrians(values.player_measurements.collision_pedestrians); player->set_collision_other(values.player_measurements.collision_other); player->set_intersection_otherlane(values.player_measurements.intersection_otherlane); player->set_intersection_offroad(values.player_measurements.intersection_offroad); + // Non-player agents. + message->clear_non_player_agents(); // we need to clear as we cache the message. + for (auto &agent : agents(values)) { + Set(message->add_non_player_agents(), agent); + } return Protobuf::Encode(*message); } @@ -79,7 +122,7 @@ namespace server { DEBUG_ASSERT(message != nullptr); message->ParseFromString(str); if (message->IsInitialized()) { - values.player_start_location_index = message->player_start_location_index(); + values.player_start_spot_index = message->player_start_spot_index(); return true; } else { log_error("invalid protobuf message: episode start"); diff --git a/Util/CarlaServer/source/test/Test_CarlaServer.cpp b/Util/CarlaServer/source/test/Test_CarlaServer.cpp index abb0ef8a7..bd2f5ef21 100644 --- a/Util/CarlaServer/source/test/Test_CarlaServer.cpp +++ b/Util/CarlaServer/source/test/Test_CarlaServer.cpp @@ -56,11 +56,11 @@ TEST(CarlaServerAPI, SimBlocking) { {2u, 3u, 1u, image1} }; - const carla_vector3d start_locations[] = { - {0.0f, 0.0f, 0.0f}, - {1.0f, 0.0f, 0.0f}, - {0.0f, 1.0f, 0.0f}, - {1.0f, 1.0f, 0.0f} + const carla_transform start_locations[] = { + {carla_vector3d{0.0f, 0.0f, 0.0f}, carla_vector3d{0.0f, 0.0f, 0.0f}}, + {carla_vector3d{1.0f, 0.0f, 0.0f}, carla_vector3d{1.0f, 0.0f, 0.0f}}, + {carla_vector3d{0.0f, 1.0f, 0.0f}, carla_vector3d{0.0f, 1.0f, 0.0f}}, + {carla_vector3d{1.0f, 1.0f, 0.0f}, carla_vector3d{1.0f, 1.0f, 0.0f}} }; const auto S = CARLA_SERVER_SUCCESS; diff --git a/Util/Proto/carla_server.proto b/Util/Proto/carla_server.proto index ffebac9a2..27b396dec 100644 --- a/Util/Proto/carla_server.proto +++ b/Util/Proto/carla_server.proto @@ -4,28 +4,67 @@ package carla_server; option cc_enable_arenas = true; +// ============================================================================= +// -- Basic types -------------------------------------------------------------- +// ============================================================================= + message Vector3D { float x = 1; float y = 2; float z = 3; } +message Transform { + Vector3D location = 1; + // For now, orientation as the cartesian coordinates X, Y, Z. But we are + // probably better off using Roll, Pitch, Yaw in the future as it gives more + // info. + Vector3D orientation = 2; +} + +message BoundingBox { + Transform transform = 1; + Vector3D extent = 2; +} + +message Agent { + enum Type { + UNKNOWN = 0; + VEHICLE = 1; + PEDESTRIAN = 2; + } + + uint32 id = 1; + Type type = 2; + Transform transform = 3; + BoundingBox bounding_box = 4; + float forward_speed = 5; +} + +// ============================================================================= +// -- World Server Messages ---------------------------------------------------- +// ============================================================================= + message RequestNewEpisode { - bytes ini_file = 1; + string ini_file = 1; } message SceneDescription { - bytes player_start_locations = 1; + repeated Transform player_start_spots = 1; } message EpisodeStart { - int32 player_start_location_index = 1; + uint32 player_start_spot_index = 1; } message EpisodeReady { bool ready = 1; } +// ============================================================================= +// -- Agent Server Messages ---------------------------------------------------- +// ============================================================================= + message Control { float steer = 1; float throttle = 2; @@ -36,8 +75,7 @@ message Control { message Measurements { message PlayerMeasurements { - Vector3D location = 1; - Vector3D orientation = 2; + Transform transform = 1; Vector3D acceleration = 3; float forward_speed = 4; @@ -50,8 +88,11 @@ message Measurements { float intersection_offroad = 9; } - int32 platform_timestamp = 1; - int32 game_timestamp = 2; + uint32 platform_timestamp = 1; + + uint32 game_timestamp = 2; PlayerMeasurements player_measurements = 3; + + repeated Agent non_player_agents = 4; }