From f2f4b229d79f68a1e9823f5f14f1c0f0b0762526 Mon Sep 17 00:00:00 2001 From: doterop Date: Tue, 31 Mar 2020 17:21:06 +0200 Subject: [PATCH] Updated set_autopilot to get tm as a parameter --- LibCarla/source/carla/client/Vehicle.cpp | 7 ++++--- LibCarla/source/carla/client/Vehicle.h | 8 +++++++- LibCarla/source/carla/rpc/Command.h | 11 ++++++++++- PythonAPI/carla/source/libcarla/Actor.cpp | 4 +++- PythonAPI/carla/source/libcarla/Client.cpp | 12 +++++++++--- PythonAPI/carla/source/libcarla/Commands.cpp | 4 +++- PythonAPI/examples/manual_control.py | 17 ++++++++++------- 7 files changed, 46 insertions(+), 17 deletions(-) diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 74a5c5754..73a8b6de7 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/LibCarla/source/carla/client/Vehicle.cpp @@ -15,9 +15,11 @@ #include "carla/trafficmanager/TrafficManager.h" namespace carla { + +using TM = traffic_manager::TrafficManager; + namespace client { - using TM = traffic_manager::TrafficManager; template static bool GetControlIsSticky(const AttributesT &attributes) { @@ -33,8 +35,7 @@ namespace client { : Actor(std::move(init)), _is_control_sticky(GetControlIsSticky(GetAttributes())) {} - void Vehicle::SetAutopilot(bool enabled) { - TM tm(GetEpisode()); + void Vehicle::SetAutopilot(TM& tm, bool enabled) { if (enabled) { tm.RegisterVehicles({shared_from_this()}); } else { diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index 73765f231..7377e5b27 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/LibCarla/source/carla/client/Vehicle.h @@ -13,6 +13,11 @@ #include "carla/rpc/VehiclePhysicsControl.h" namespace carla { + +namespace traffic_manager { + class TrafficManager; +} + namespace client { class TrafficLight; @@ -23,13 +28,14 @@ namespace client { using Control = rpc::VehicleControl; using PhysicsControl = rpc::VehiclePhysicsControl; using LightState = rpc::VehicleLightState::LightState; + using TM = traffic_manager::TrafficManager; explicit Vehicle(ActorInitializer init); using ActorState::GetBoundingBox; /// Switch on/off this vehicle's autopilot. - void SetAutopilot(bool enabled = true); + void SetAutopilot(TM& tm, bool enabled = true); /// Apply @a control to this vehicle. void ApplyControl(const Control &control); diff --git a/LibCarla/source/carla/rpc/Command.h b/LibCarla/source/carla/rpc/Command.h index 57b8f3df6..f923672f0 100644 --- a/LibCarla/source/carla/rpc/Command.h +++ b/LibCarla/source/carla/rpc/Command.h @@ -17,6 +17,11 @@ #include namespace carla { + +namespace traffic_manager { + class TrafficManager; +} + namespace rpc { class Command { @@ -135,11 +140,15 @@ namespace rpc { }; struct SetAutopilot : CommandBase { + using TM = traffic_manager::TrafficManager; + SetAutopilot() = default; - SetAutopilot(ActorId id, bool value) + SetAutopilot(ActorId id, /* TM tm, */ bool value) : actor(id), + // tm(tm), enabled(value) {} ActorId actor; + // boost::optional tm; bool enabled; MSGPACK_DEFINE_ARRAY(actor, enabled); }; diff --git a/PythonAPI/carla/source/libcarla/Actor.cpp b/PythonAPI/carla/source/libcarla/Actor.cpp index 03beb9749..c06e00e43 100644 --- a/PythonAPI/carla/source/libcarla/Actor.cpp +++ b/PythonAPI/carla/source/libcarla/Actor.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include @@ -53,6 +54,7 @@ void export_actor() { using namespace boost::python; namespace cc = carla::client; namespace cr = carla::rpc; + namespace ctm = carla::traffic_manager; class_>("vector_of_ints") .def(vector_indexing_suite>()) @@ -114,7 +116,7 @@ void export_actor() { .def("get_light_state", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetLightState)) .def("apply_physics_control", &cc::Vehicle::ApplyPhysicsControl, (arg("physics_control"))) .def("get_physics_control", CONST_CALL_WITHOUT_GIL(cc::Vehicle, GetPhysicsControl)) - .def("set_autopilot", &cc::Vehicle::SetAutopilot, (arg("enabled") = true)) + .def("set_autopilot", &cc::Vehicle::SetAutopilot, (arg("TM"), arg("enabled") = true)) .def("get_speed_limit", &cc::Vehicle::GetSpeedLimit) .def("get_traffic_light_state", &cc::Vehicle::GetTrafficLightState) .def("is_at_traffic_light", &cc::Vehicle::IsAtTrafficLight) diff --git a/PythonAPI/carla/source/libcarla/Client.cpp b/PythonAPI/carla/source/libcarla/Client.cpp index 2addbbe0c..5e0dfd7b9 100644 --- a/PythonAPI/carla/source/libcarla/Client.cpp +++ b/PythonAPI/carla/source/libcarla/Client.cpp @@ -41,6 +41,9 @@ static auto ApplyBatchCommandsSync( const carla::client::Client &self, const boost::python::object &commands, bool do_tick) { + + carla::log_warning("ApplyBatchCommandsSync"); + using CommandType = carla::rpc::Command; std::vector cmds { boost::python::stl_input_iterator(commands), @@ -57,6 +60,7 @@ static auto ApplyBatchCommandsSync( std::vector vehicles_to_enable(cmds.size(), nullptr); std::vector vehicles_to_disable(cmds.size(), nullptr); carla::client::World world = self.GetWorld(); + boost::optional tm; std::atomic vehicles_to_enable_index; std::atomic vehicles_to_disable_index; @@ -65,6 +69,7 @@ static auto ApplyBatchCommandsSync( vehicles_to_disable_index.store(0); auto ProcessCommand = [&](size_t min_index, size_t max_index) { + carla::log_warning("ApplyBatchCommandsSync ProcessCommand"); for (size_t i = min_index; i < max_index; ++i) { if (!responses[i].HasError()) { @@ -88,6 +93,7 @@ static auto ApplyBatchCommandsSync( // check SetAutopilot command else if (cmd_type_info == typeid(carla::rpc::Command::SetAutopilot)) { autopilotValue = boost::get(cmd_type).enabled; + // tm = boost::get(cmd_type).tm; isAutopilot = true; } @@ -141,9 +147,9 @@ static auto ApplyBatchCommandsSync( vehicles_to_disable.shrink_to_fit(); // check if any autopilot command was sent - if ((vehicles_to_enable.size() || vehicles_to_disable.size())) { - self.GetInstanceTM().RegisterVehicles(vehicles_to_enable); - self.GetInstanceTM().UnregisterVehicles(vehicles_to_disable); + if (tm && (vehicles_to_enable.size() || vehicles_to_disable.size())) { + tm->RegisterVehicles(vehicles_to_enable); + tm->UnregisterVehicles(vehicles_to_disable); } return result; diff --git a/PythonAPI/carla/source/libcarla/Commands.cpp b/PythonAPI/carla/source/libcarla/Commands.cpp index 9fb98f17a..568852260 100644 --- a/PythonAPI/carla/source/libcarla/Commands.cpp +++ b/PythonAPI/carla/source/libcarla/Commands.cpp @@ -47,6 +47,7 @@ void export_commands() { namespace cc = carla::client; namespace cg = carla::geom; namespace cr = carla::rpc; + namespace ctm = carla::traffic_manager; using ActorPtr = boost::shared_ptr; @@ -151,8 +152,9 @@ void export_commands() { class_("SetAutopilot") .def("__init__", &command_impl::CustomInit, (arg("actor"), arg("enabled"))) - .def(init((arg("actor_id"), arg("enabled")))) + .def(init((arg("actor_id"), /* arg("tm"), */ arg("enabled")))) .def_readwrite("actor_id", &cr::Command::SetAutopilot::actor) + //.def_readwrite("tm", &cr::Command::SetAutopilot::tm) .def_readwrite("enabled", &cr::Command::SetAutopilot::enabled) ; diff --git a/PythonAPI/examples/manual_control.py b/PythonAPI/examples/manual_control.py index b9761b53c..65377803e 100755 --- a/PythonAPI/examples/manual_control.py +++ b/PythonAPI/examples/manual_control.py @@ -155,7 +155,7 @@ def get_actor_display_name(actor, truncate=250): class World(object): - def __init__(self, carla_world, hud, args): + def __init__(self, carla_world, tm, hud, args): self.world = carla_world self.actor_role_name = args.rolename try: @@ -165,6 +165,7 @@ class World(object): print(' The server could not send the OpenDRIVE (.xodr) file:') print(' Make sure it exists, has the same name of your town, and is correct.') sys.exit(1) + self.tm = tm self.hud = hud self.player = None self.collision_sensor = None @@ -285,7 +286,7 @@ class KeyboardControl(object): if isinstance(world.player, carla.Vehicle): self._control = carla.VehicleControl() self._lights = carla.VehicleLightState.NONE - world.player.set_autopilot(self._autopilot_enabled) + world.player.set_autopilot(world.tm, self._autopilot_enabled) world.player.set_light_state(self._lights) elif isinstance(world.player, carla.Walker): self._control = carla.WalkerControl() @@ -307,9 +308,9 @@ class KeyboardControl(object): return True elif event.key == K_BACKSPACE: if self._autopilot_enabled: - world.player.set_autopilot(False) + world.player.set_autopilot(world.tm, False) world.restart() - world.player.set_autopilot(True) + world.player.set_autopilot(world.tm, True) else: world.restart() elif event.key == K_F1: @@ -350,7 +351,7 @@ class KeyboardControl(object): world.destroy_sensors() # disable autopilot self._autopilot_enabled = False - world.player.set_autopilot(self._autopilot_enabled) + world.player.set_autopilot(world.tm, self._autopilot_enabled) world.hud.notification("Replaying file 'manual_recording.rec'") # replayer client.replay_file("manual_recording.rec", world.recording_start, 0, 0) @@ -381,7 +382,7 @@ class KeyboardControl(object): self._control.gear = self._control.gear + 1 elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL: self._autopilot_enabled = not self._autopilot_enabled - world.player.set_autopilot(self._autopilot_enabled) + world.player.set_autopilot(world.tm, self._autopilot_enabled) world.hud.notification( 'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off')) elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL: @@ -1011,8 +1012,10 @@ def game_loop(args): (args.width, args.height), pygame.HWSURFACE | pygame.DOUBLEBUF) + # TODO: args.tm_port + tm = client.get_trafficmanager(8000) hud = HUD(args.width, args.height) - world = World(client.get_world(), hud, args) + world = World(client.get_world(), tm, hud, args) controller = KeyboardControl(world, args.autopilot) clock = pygame.time.Clock()