From c82c015d144c9f461fe48149f6858e8006d7ed0b Mon Sep 17 00:00:00 2001 From: nsubiron Date: Tue, 26 Sep 2017 18:20:50 +0200 Subject: [PATCH] Change protocol: remove autopilot option, always send AI control and let the client decide --- .../Carla/AI/WheeledVehicleAIController.cpp | 16 ++-- Source/Carla/AI/WheeledVehicleAIController.h | 16 ++++ Source/Carla/Game/CarlaPlayerState.cpp | 10 ++- Source/Carla/Game/CarlaPlayerState.h | 88 +++++++++++++++++-- Source/Carla/Game/CarlaServer.cpp | 53 +++++------ Source/Carla/Game/CarlaVehicleController.cpp | 5 ++ Util/CarlaServer/include/carla/carla_server.h | 3 +- .../source/carla/server/CarlaEncoder.cpp | 11 ++- Util/Proto/carla_server.proto | 3 +- Util/PythonClient/carla/client.py | 1 - Util/PythonClient/carla/console.py | 1 - Util/PythonClient/client_test.py | 17 ++-- Util/PythonClient/test/Basic.py | 28 +++--- 13 files changed, 189 insertions(+), 63 deletions(-) diff --git a/Source/Carla/AI/WheeledVehicleAIController.cpp b/Source/Carla/AI/WheeledVehicleAIController.cpp index 8e85c95cc..d8eb510e9 100644 --- a/Source/Carla/AI/WheeledVehicleAIController.cpp +++ b/Source/Carla/AI/WheeledVehicleAIController.cpp @@ -91,8 +91,12 @@ void AWheeledVehicleAIController::Tick(const float DeltaTime) { Super::Tick(DeltaTime); + TickAutopilotController(); + if (bAutopilotEnabled) { - TickAutopilotController(); + Vehicle->SetThrottleInput(AutopilotControl.Throttle); + Vehicle->SetSteeringInput(AutopilotControl.Steer); + Vehicle->SetBrakeInput(AutopilotControl.Brake); } } @@ -164,13 +168,13 @@ void AWheeledVehicleAIController::TickAutopilotController() } if (Throttle < 0.001f) { - Vehicle->SetBrakeInput(1.0f); - Vehicle->SetThrottleInput(0.0f); + AutopilotControl.Brake = 1.0f; + AutopilotControl.Throttle = 0.0f; } else { - Vehicle->SetBrakeInput(0.0f); - Vehicle->SetThrottleInput(Throttle); + AutopilotControl.Brake = 0.0f; + AutopilotControl.Throttle = Throttle; } - Vehicle->SetSteeringInput(Steering); + AutopilotControl.Steer = Steering; } float AWheeledVehicleAIController::GoToNextTargetLocation(FVector &Direction) diff --git a/Source/Carla/AI/WheeledVehicleAIController.h b/Source/Carla/AI/WheeledVehicleAIController.h index 554f20c15..f702d1601 100644 --- a/Source/Carla/AI/WheeledVehicleAIController.h +++ b/Source/Carla/AI/WheeledVehicleAIController.h @@ -180,6 +180,20 @@ public: /// @name AI // =========================================================================== /// @{ +protected: + + struct FAutopilotControl { + float Throttle = 0.0f; + float Steer = 0.0f; + float Brake = 0.0f; + float HandBrake = false; + }; + + const FAutopilotControl &GetAutopilotControl() const + { + return AutopilotControl; + } + private: void TickAutopilotController(); @@ -223,5 +237,7 @@ private: UPROPERTY(VisibleAnywhere) float MaximumSteerAngle = -1.0f; + FAutopilotControl AutopilotControl; + std::queue TargetLocations; }; diff --git a/Source/Carla/Game/CarlaPlayerState.cpp b/Source/Carla/Game/CarlaPlayerState.cpp index a4a1a5f10..41a6acf75 100644 --- a/Source/Carla/Game/CarlaPlayerState.cpp +++ b/Source/Carla/Game/CarlaPlayerState.cpp @@ -22,11 +22,19 @@ void ACarlaPlayerState::CopyProperties(APlayerState *PlayerState) ACarlaPlayerState *Other = Cast(PlayerState); if (Other != nullptr) { + FramesPerSecond = Other->FramesPerSecond; PlatformTimeStamp = Other->PlatformTimeStamp; GameTimeStamp = Other->GameTimeStamp; Transform = Other->Transform; - Acceleration = Other->Acceleration; ForwardSpeed = Other->ForwardSpeed; + Acceleration = Other->Acceleration; + Throttle = Other->Throttle; + Steer = Other->Steer; + Brake = Other->Brake; + bHandBrake = Other->bHandBrake; + CurrentGear = Other->CurrentGear; + SpeedLimit = Other->SpeedLimit; + TrafficLightState = Other->TrafficLightState; CollisionIntensityCars = Other->CollisionIntensityCars; CollisionIntensityPedestrians = Other->CollisionIntensityPedestrians; CollisionIntensityOther = Other->CollisionIntensityOther; diff --git a/Source/Carla/Game/CarlaPlayerState.h b/Source/Carla/Game/CarlaPlayerState.h index c85127e13..ff3b6a90d 100644 --- a/Source/Carla/Game/CarlaPlayerState.h +++ b/Source/Carla/Game/CarlaPlayerState.h @@ -30,6 +30,11 @@ public: // =========================================================================== public: + // =========================================================================== + /// @name Timing + // =========================================================================== + /// @{ + UFUNCTION(BlueprintCallable) float GetFramesPerSecond() const { @@ -48,6 +53,12 @@ public: return GameTimeStamp; } + /// @} + // =========================================================================== + /// @name Transform and dynamics + // =========================================================================== + /// @{ + UFUNCTION(BlueprintCallable) const FTransform &GetTransform() const { @@ -66,24 +77,54 @@ public: return Transform.GetRotation().GetForwardVector(); } + UFUNCTION(BlueprintCallable) + float GetForwardSpeed() const + { + return ForwardSpeed; + } + UFUNCTION(BlueprintCallable) const FVector &GetAcceleration() const { return Acceleration; } + /// @} + // =========================================================================== + /// @name Vehicle control + // =========================================================================== + /// @{ + + UFUNCTION(BlueprintCallable) + float GetThrottle() const + { + return Throttle; + } + + UFUNCTION(BlueprintCallable) + float GetSteer() const + { + return Steer; + } + + UFUNCTION(BlueprintCallable) + float GetBrake() const + { + return Brake; + } + + UFUNCTION(BlueprintCallable) + bool GetHandBrake() const + { + return bHandBrake; + } + UFUNCTION(BlueprintCallable) int32 GetCurrentGear() const { return CurrentGear; } - UFUNCTION(BlueprintCallable) - float GetForwardSpeed() const - { - return ForwardSpeed; - } - UFUNCTION(BlueprintCallable) float GetSpeedLimit() const { @@ -96,6 +137,12 @@ public: return TrafficLightState; } + /// @} + // =========================================================================== + /// @name Collision + // =========================================================================== + /// @{ + UFUNCTION(BlueprintCallable) float GetCollisionIntensityCars() const { @@ -114,6 +161,12 @@ public: return CollisionIntensityOther; } + /// @} + // =========================================================================== + /// @name Road intersection + // =========================================================================== + /// @{ + UFUNCTION(BlueprintCallable) float GetOtherLaneIntersectionFactor() const { @@ -126,6 +179,12 @@ public: return OffRoadIntersectionFactor; } + /// @} + // =========================================================================== + /// @name Images + // =========================================================================== + /// @{ + UFUNCTION(BlueprintCallable) int32 GetNumberOfImages() const { @@ -143,6 +202,7 @@ public: return Images; } + /// @} // =========================================================================== // -- Modifiers -------------------------------------------------------------- // =========================================================================== @@ -178,14 +238,26 @@ private: UPROPERTY(VisibleAnywhere) FTransform Transform; + UPROPERTY(VisibleAnywhere) + float ForwardSpeed = 0.0f; + UPROPERTY(VisibleAnywhere) FVector Acceleration; UPROPERTY(VisibleAnywhere) - int32 CurrentGear; + float Throttle = 0.0f; UPROPERTY(VisibleAnywhere) - float ForwardSpeed = 0.0f; + float Steer = 0.0f; + + UPROPERTY(VisibleAnywhere) + float Brake = 0.0f; + + UPROPERTY(VisibleAnywhere) + bool bHandBrake = false; + + UPROPERTY(VisibleAnywhere) + int32 CurrentGear; UPROPERTY(VisibleAnywhere) float SpeedLimit = -1.0f; diff --git a/Source/Carla/Game/CarlaServer.cpp b/Source/Carla/Game/CarlaServer.cpp index 937c1f79a..45c194bf0 100644 --- a/Source/Carla/Game/CarlaServer.cpp +++ b/Source/Carla/Game/CarlaServer.cpp @@ -37,6 +37,11 @@ static int32 GetTimeOut(uint32 TimeOut, const bool bBlocking) // -- Set functions ------------------------------------------------------------ // ============================================================================= +static inline void Set(bool &lhs, bool rhs) +{ + lhs = rhs; +} + static inline void Set(float &lhs, float rhs) { lhs = rhs; @@ -154,34 +159,25 @@ CarlaServer::ErrorCode CarlaServer::ReadControl(ACarlaVehicleController &Player, carla_control values; auto ec = ParseErrorCode(carla_read_control(Server, values, GetTimeOut(TimeOut, bBlocking))); if (Success == ec) { - Player.SetAutopilot(values.autopilot); - if (!values.autopilot) { - check(Player.IsPossessingAVehicle()); - auto Vehicle = Player.GetPossessedVehicle(); - Vehicle->SetSteeringInput(values.steer); - Vehicle->SetThrottleInput(values.throttle); - Vehicle->SetBrakeInput(values.brake); - Vehicle->SetHandbrakeInput(values.hand_brake); - Vehicle->SetReverse(values.reverse); + check(Player.IsPossessingAVehicle()); + auto Vehicle = Player.GetPossessedVehicle(); + Vehicle->SetSteeringInput(values.steer); + Vehicle->SetThrottleInput(values.throttle); + Vehicle->SetBrakeInput(values.brake); + Vehicle->SetHandbrakeInput(values.hand_brake); + Vehicle->SetReverse(values.reverse); #ifdef CARLA_SERVER_EXTRA_LOG - UE_LOG( - LogCarlaServer, - Log, - TEXT("Read control (%s): { Steer = %f, Throttle = %f, Brake = %f, Handbrake = %s, Reverse = %s }"), - (bBlocking ? TEXT("Sync") : TEXT("Async")), - values.steer, - values.throttle, - values.brake, - (values.hand_brake ? TEXT("True") : TEXT("False")), - (values.reverse ? TEXT("True") : TEXT("False"))); - } else { - UE_LOG( - LogCarlaServer, - Log, - TEXT("Read control (%s): { Autopilot = On }"), - (bBlocking ? TEXT("Sync") : TEXT("Async"))); + UE_LOG( + LogCarlaServer, + Log, + TEXT("Read control (%s): { Steer = %f, Throttle = %f, Brake = %f, Handbrake = %s, Reverse = %s }"), + (bBlocking ? TEXT("Sync") : TEXT("Async")), + values.steer, + values.throttle, + values.brake, + (values.hand_brake ? TEXT("True") : TEXT("False")), + (values.reverse ? TEXT("True") : TEXT("False"))); #endif // CARLA_SERVER_EXTRA_LOG - } } else if ((!bBlocking) && (TryAgain == ec)) { UE_LOG(LogCarlaServer, Warning, TEXT("No control received from the client this frame!")); } @@ -311,6 +307,11 @@ CarlaServer::ErrorCode CarlaServer::SendMeasurements( Set(player.collision_other, PlayerState.GetCollisionIntensityOther()); Set(player.intersection_otherlane, PlayerState.GetOtherLaneIntersectionFactor()); Set(player.intersection_offroad, PlayerState.GetOffRoadIntersectionFactor()); + Set(player.ai_control.steer, PlayerState.GetSteer()); + Set(player.ai_control.throttle, PlayerState.GetThrottle()); + Set(player.ai_control.brake, PlayerState.GetBrake()); + Set(player.ai_control.hand_brake, PlayerState.GetHandBrake()); + Set(player.ai_control.reverse, PlayerState.GetCurrentGear() < 0); TArray Agents; if (bSendNonPlayerAgentsInfo) { diff --git a/Source/Carla/Game/CarlaVehicleController.cpp b/Source/Carla/Game/CarlaVehicleController.cpp index c9e99771d..9dddfdcb4 100644 --- a/Source/Carla/Game/CarlaVehicleController.cpp +++ b/Source/Carla/Game/CarlaVehicleController.cpp @@ -84,6 +84,11 @@ void ACarlaVehicleController::Tick(float DeltaTime) CarlaPlayerState->ForwardSpeed = Vehicle->GetVehicleForwardSpeed(); const FVector CurrentSpeed = CarlaPlayerState->ForwardSpeed * CarlaPlayerState->GetOrientation(); CarlaPlayerState->Acceleration = (CurrentSpeed - PreviousSpeed) / DeltaTime; + const auto &AutopilotControl = GetAutopilotControl(); + CarlaPlayerState->Steer = AutopilotControl.Steer; + CarlaPlayerState->Throttle = AutopilotControl.Throttle; + CarlaPlayerState->Brake = AutopilotControl.Brake; + CarlaPlayerState->bHandBrake = AutopilotControl.HandBrake; CarlaPlayerState->CurrentGear = Vehicle->GetVehicleCurrentGear(); CarlaPlayerState->SpeedLimit = GetSpeedLimit(); CarlaPlayerState->TrafficLightState = GetTrafficLightState(); diff --git a/Util/CarlaServer/include/carla/carla_server.h b/Util/CarlaServer/include/carla/carla_server.h index aaa25756b..1986e051d 100644 --- a/Util/CarlaServer/include/carla/carla_server.h +++ b/Util/CarlaServer/include/carla/carla_server.h @@ -110,7 +110,6 @@ extern "C" { float brake; bool hand_brake; bool reverse; - bool autopilot; }; /* ======================================================================== */ @@ -134,6 +133,8 @@ extern "C" { float intersection_otherlane; /** Percentage of the car off-road. */ float intersection_offroad; + /** Vehicle's AI control that would apply this frame. */ + struct carla_control ai_control; }; /* ======================================================================== */ diff --git a/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp b/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp index 87ad0cbe3..5e1d464e2 100644 --- a/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp +++ b/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp @@ -36,6 +36,15 @@ namespace server { Set(lhs->mutable_orientation(), rhs.orientation); } + static void Set(cs::Control *lhs, const carla_control &rhs) { + DEBUG_ASSERT(lhs != nullptr); + lhs->set_steer(rhs.steer); + lhs->set_throttle(rhs.throttle); + lhs->set_brake(rhs.brake); + lhs->set_hand_brake(rhs.hand_brake); + lhs->set_reverse(rhs.reverse); + } + static void SetVehicle(cs::Vehicle *lhs, const carla_agent &rhs) { DEBUG_ASSERT(lhs != nullptr); Set(lhs->mutable_transform(), rhs.transform); @@ -113,6 +122,7 @@ namespace server { 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); + Set(player->mutable_ai_control(), values.player_measurements.ai_control); // Non-player agents. message->clear_non_player_agents(); // we need to clear as we cache the message. for (auto &agent : agents(values)) { @@ -162,7 +172,6 @@ namespace server { values.brake = message->brake(); values.hand_brake = message->hand_brake(); values.reverse = message->reverse(); - values.autopilot = message->autopilot(); return true; } else { log_error("invalid protobuf message: control"); diff --git a/Util/Proto/carla_server.proto b/Util/Proto/carla_server.proto index 99ec61dcd..24d386a26 100644 --- a/Util/Proto/carla_server.proto +++ b/Util/Proto/carla_server.proto @@ -89,7 +89,6 @@ message Control { float brake = 3; bool hand_brake = 4; bool reverse = 5; - bool autopilot = 6; } message Measurements { @@ -105,6 +104,8 @@ message Measurements { float intersection_otherlane = 8; float intersection_offroad = 9; + + Control ai_control = 10; } uint32 platform_timestamp = 1; diff --git a/Util/PythonClient/carla/client.py b/Util/PythonClient/carla/client.py index 2bfcb93f5..b84e95e6a 100644 --- a/Util/PythonClient/carla/client.py +++ b/Util/PythonClient/carla/client.py @@ -95,7 +95,6 @@ class CarlaClient(object): pb_message.brake = kwargs.get('brake', 0.0) pb_message.hand_brake = kwargs.get('hand_brake', False) pb_message.reverse = kwargs.get('reverse', False) - pb_message.autopilot = kwargs.get('autopilot', False) self._control_client.write(pb_message.SerializeToString()) diff --git a/Util/PythonClient/carla/console.py b/Util/PythonClient/carla/console.py index 0545cde38..478826f46 100644 --- a/Util/PythonClient/carla/console.py +++ b/Util/PythonClient/carla/console.py @@ -22,7 +22,6 @@ class _Control(object): self.c_brake = 0.0 self.c_hand_brake = False self.c_reverse = False - self.c_autopilot = False def action_list(self): return [x[2:] for x in dir(self) if x.startswith('c_')] diff --git a/Util/PythonClient/client_test.py b/Util/PythonClient/client_test.py index f8e0bcc2b..1a4c00959 100755 --- a/Util/PythonClient/client_test.py +++ b/Util/PythonClient/client_test.py @@ -48,7 +48,7 @@ def run_carla_server(args): client.start_episode(player_start) - autopilot = (random.random() < 0.5) + use_ai_control = (random.random() < 0.5) reverse = (random.random() < 0.2) for frame in range(0, frames_per_episode): @@ -63,11 +63,18 @@ def run_carla_server(args): images[0].save_to_disk(filename.format(episode, frame)) logging.debug('sending control...') + control = measurements.player_measurements.ai_control + if not use_ai_control: + control.steer = random.uniform(-1.0, 1.0) + control.throttle = 0.3 + control.hand_brake = False + control.reverse = reverse client.send_control( - steer=random.uniform(-1.0, 1.0), - throttle=0.3, - reverse=reverse, - autopilot=autopilot) + steer=control.steer, + throttle=control.throttle, + brake=control.brake, + hand_brake=control.hand_brake, + reverse=control.reverse) def main(): diff --git a/Util/PythonClient/test/Basic.py b/Util/PythonClient/test/Basic.py index 23755f669..7ce467f72 100644 --- a/Util/PythonClient/test/Basic.py +++ b/Util/PythonClient/test/Basic.py @@ -11,7 +11,7 @@ from carla.util import make_connection class _BasicTestBase(test.CarlaServerTest): - def run_carla_client(self, carla_settings, number_of_episodes, number_of_frames, control=None): + def run_carla_client(self, carla_settings, number_of_episodes, number_of_frames, use_ai_control=None): with make_connection(CarlaClient, self.args.host, self.args.port, timeout=15) as client: logging.info('CarlaClient connected, running %d episodes', number_of_episodes) for _ in range(0, number_of_episodes): @@ -28,26 +28,30 @@ class _BasicTestBase(test.CarlaServerTest): number_of_player_starts, number_of_frames) client.start_episode(player_start) - autopilot = (random.random() < 0.5) + if use_ai_control is None: + use_ai_control = (random.random() < 0.5) reverse = (random.random() < 0.2) for _ in range(0, number_of_frames): logging.debug('reading measurements...') measurements, images = client.read_measurements() number_of_agents = len(measurements.non_player_agents) - expected_number_of_agents = carla_settings.get_number_of_agents() logging.debug('received data of %d agents', number_of_agents) logging.debug('received %d images', len(images)) if len(images) != len(carla_settings._cameras): raise RuntimeError('received %d images, expected %d' % (len(images), len(carla_settings._cameras))) logging.debug('sending control...') - if control is not None: - client.send_control(**control) - else: - client.send_control( - steer=random.uniform(-1.0, 1.0), - throttle=0.3, - reverse=reverse, - autopilot=autopilot) + control = measurements.player_measurements.ai_control + if not use_ai_control: + control.steer = random.uniform(-1.0, 1.0) + control.throttle = 0.3 + control.hand_brake = False + control.reverse = reverse + client.send_control( + steer=control.steer, + throttle=control.throttle, + brake=control.brake, + hand_brake=control.hand_brake, + reverse=control.reverse) class UseCase(_BasicTestBase): @@ -97,4 +101,4 @@ class LongEpisode(_BasicTestBase): def run(self): settings = CarlaSettings() settings.add_camera(Camera('DefaultCamera')) - self.run_carla_client(settings, 1, 2000, {'autopilot': True}) + self.run_carla_client(settings, 1, 2000, use_ai_control=True)