From cd1db62a97fc6fd310d338c5af443236e649e614 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Tue, 5 Sep 2017 11:43:22 +0100 Subject: [PATCH] Add autopilot mode to protocol --- Source/Carla/Game/CarlaGameController.cpp | 3 +- Source/Carla/Game/CarlaServer.cpp | 49 +++++++++---------- Source/Carla/Game/CarlaServer.h | 4 +- Util/CarlaServer/include/carla/carla_server.h | 1 + .../source/carla/server/CarlaEncoder.cpp | 1 + Util/Proto/carla_server.proto | 1 + Util/PythonClient/carla_client.py | 14 ++++-- 7 files changed, 40 insertions(+), 33 deletions(-) diff --git a/Source/Carla/Game/CarlaGameController.cpp b/Source/Carla/Game/CarlaGameController.cpp index 220ca169e..954f9edec 100644 --- a/Source/Carla/Game/CarlaGameController.cpp +++ b/Source/Carla/Game/CarlaGameController.cpp @@ -123,8 +123,7 @@ void CarlaGameController::Tick(float DeltaSeconds) // Read control, block if the settings say so. if (Server != nullptr) { const bool bShouldBlock = CarlaSettings->bSynchronousMode; - check(Player->IsPossessingAVehicle()); - if (Errc::Error == Server->ReadControl(*Player->GetPossessedVehicle(), bShouldBlock)) { + if (Errc::Error == Server->ReadControl(*Player, bShouldBlock)) { Server = nullptr; } } diff --git a/Source/Carla/Game/CarlaServer.cpp b/Source/Carla/Game/CarlaServer.cpp index f75834818..3f51e30e1 100644 --- a/Source/Carla/Game/CarlaServer.cpp +++ b/Source/Carla/Game/CarlaServer.cpp @@ -6,6 +6,7 @@ #include "GameFramework/PlayerStart.h" #include "CarlaPlayerState.h" +#include "CarlaVehicleController.h" #include "CarlaWheeledVehicle.h" #include "SceneCaptureCamera.h" #include "Settings/CarlaSettings.h" @@ -62,17 +63,8 @@ static void Set(carla_image &cImage, const FCapturedImage &uImage) #ifdef CARLA_SERVER_EXTRA_LOG { - auto GetImageType = [](carla::ImageType Type) { - switch (Type) { - case carla::IMAGE: return TEXT("IMAGE"); - case carla::SCENE_FINAL: return TEXT("SCENE_FINAL"); - case carla::DEPTH: return TEXT("DEPTH"); - case carla::SEMANTIC_SEG: return TEXT("SEMANTIC_SEG"); - default: return TEXT("ERROR!"); - } - }; const auto Size = uImage.BitMap.Num(); - UE_LOG(LogCarlaServer, Log, TEXT("Sending image %dx%d (%d) %s"), cImage.width, cImage.height, Size, GetImageType(cImage.type())); + UE_LOG(LogCarlaServer, Log, TEXT("Sending image %dx%d (%d) type %d"), cImage.width, cImage.height, Size, cImage.type); } } else { UE_LOG(LogCarlaServer, Warning, TEXT("Sending empty image")); @@ -154,27 +146,34 @@ CarlaServer::ErrorCode CarlaServer::SendEpisodeReady(const bool bBlocking) return ParseErrorCode(carla_write_episode_ready(Server, values, GetTimeOut(TimeOut, bBlocking))); } -CarlaServer::ErrorCode CarlaServer::ReadControl(ACarlaWheeledVehicle &Player, const bool bBlocking) +CarlaServer::ErrorCode CarlaServer::ReadControl(ACarlaVehicleController &Player, const bool bBlocking) { carla_control values; auto ec = ParseErrorCode(carla_read_control(Server, values, GetTimeOut(TimeOut, bBlocking))); if (Success == ec) { - Player.SetSteeringInput(values.steer); - Player.SetThrottleInput(values.throttle); - Player.SetBrakeInput(values.brake); - Player.SetHandbrakeInput(values.hand_brake); - Player.SetReverse(values.reverse); + 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); #ifdef CARLA_SERVER_EXTRA_LOG - UE_LOG( - LogCarlaServer, - Log, - TEXT("Read control: { Steer = %f, Throttle = %f, Brake = %f, Handbrake = %s, Reverse = %s }"), - values.steer, - values.throttle, - values.brake, - (values.hand_brake ? TEXT("True") : TEXT("False")), - (values.reverse ? TEXT("True") : TEXT("False"))); + UE_LOG( + LogCarlaServer, + Log, + TEXT("Read control: { Steer = %f, Throttle = %f, Brake = %f, Handbrake = %s, Reverse = %s }"), + 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: { Autopilot = On }")); #endif // CARLA_SERVER_EXTRA_LOG + } } else if ((!bBlocking) && (TryAgain == ec)) { UE_LOG(LogCarlaServer, Warning, TEXT("No control received from the client this frame!")); } diff --git a/Source/Carla/Game/CarlaServer.h b/Source/Carla/Game/CarlaServer.h index 57dfd7962..a14960c60 100644 --- a/Source/Carla/Game/CarlaServer.h +++ b/Source/Carla/Game/CarlaServer.h @@ -3,7 +3,7 @@ #pragma once class ACarlaGameState; -class ACarlaWheeledVehicle; +class ACarlaVehicleController; class APlayerStart; class UCarlaSettings; @@ -36,7 +36,7 @@ public: ErrorCode SendEpisodeReady(bool bBlocking); - ErrorCode ReadControl(ACarlaWheeledVehicle &Player, bool bBlocking); + ErrorCode ReadControl(ACarlaVehicleController &Player, bool bBlocking); ErrorCode SendMeasurements( const ACarlaGameState &GameState, diff --git a/Util/CarlaServer/include/carla/carla_server.h b/Util/CarlaServer/include/carla/carla_server.h index 37d268533..b6ea33186 100644 --- a/Util/CarlaServer/include/carla/carla_server.h +++ b/Util/CarlaServer/include/carla/carla_server.h @@ -99,6 +99,7 @@ extern "C" { float brake; bool hand_brake; bool reverse; + bool autopilot; }; /* ======================================================================== */ diff --git a/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp b/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp index f1b27856f..ab9fe008e 100644 --- a/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp +++ b/Util/CarlaServer/source/carla/server/CarlaEncoder.cpp @@ -134,6 +134,7 @@ 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 aaf182d51..797dc5df9 100644 --- a/Util/Proto/carla_server.proto +++ b/Util/Proto/carla_server.proto @@ -66,6 +66,7 @@ message Control { float brake = 3; bool hand_brake = 4; bool reverse = 5; + bool autopilot = 6; } message Measurements { diff --git a/Util/PythonClient/carla_client.py b/Util/PythonClient/carla_client.py index 4039d18bb..73d7d56ae 100755 --- a/Util/PythonClient/carla_client.py +++ b/Util/PythonClient/carla_client.py @@ -115,6 +115,7 @@ 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()) @@ -175,8 +176,8 @@ def test_carla_client(): data = client.read_scene_description() number_of_start_spots = len(data.player_start_spots) logging.info('received %d player start locations', number_of_start_spots) - for spot in data.player_start_spots: - logging.info(spot) + # for spot in data.player_start_spots: + # logging.info(spot) logging.info('sending episode start') client.write_episode_start(random.randint(0, max(0, number_of_start_spots - 1))) @@ -191,6 +192,8 @@ def test_carla_client(): logging.info('connecting secondary clients') client.connect_secondary_clients() + autopilot = random.choice([True, False]) + for x in xrange(0, 1000): logging.info('waiting for measurements') data = client.read_measurements() @@ -207,12 +210,15 @@ def test_carla_client(): data = client.read_images() logging.info('received %d bytes of images', len(data) if data is not None else 0) - # if (x+1) % 10 == 0: + # if (x+1) % 100 == 0: # logging.info('Taking a nap...') # time.sleep(2) logging.info('sending control') - client.write_control(steer=random.uniform(-1.0, 1.0), throttle=0.5, reverse=False) + if autopilot: + client.write_control(autopilot=True) + else: + client.write_control(steer=random.uniform(-1.0, 1.0), throttle=0.3, reverse=False) if os.path.isfile(args.ini_file): logging.info('sending file %s', args.ini_file)