Is there a way to manual control the ego-vehicle (with an option to shift between steer/autopilot mode)?

[ChenSunMac]

I have tried to develop a data_recorder_sync.py for CARLA 0.9.X

I read the examples and I can get the synchronous version running with autopilot by spawn actors and put sensors on one of them. However, Now I wish to add controller onto the selected ego-vehicle such that I can manually perform some driving behaviors.

I referred the manual_control.py and manual_control_steeringwheel.py and created a version of DualControl() with no hud like the following:

class DualControl(object):
    def __init__(self, world, start_in_autopilot):
        self._autopilot_enabled = start_in_autopilot
        self.player = world.ego_vehicle
        if isinstance(self.player, carla.Vehicle):
            self._control = carla.VehicleControl()
            world.ego_vehicle.set_autopilot(self._autopilot_enabled)
        else:
            raise NotImplementedError("Actor type not wanted")
        self._steer_cache = 0.0

        # initialize steering wheel
        pygame.joystick.init()
        joystick_count = pygame.joystick.get_count()
        if joystick_count > 1:
            raise ValueError("Please Connect Just One Joystick")

        self._joystick = pygame.joystick.Joystick(0)
        self._joystick.init()

        self._parser = ConfigParser()
        self._parser.read('wheel_config.ini')
        self._steer_idx = int(
            self._parser.get('G29 Racing Wheel', 'steering_wheel'))
        self._throttle_idx = int(
            self._parser.get('G29 Racing Wheel', 'throttle'))
        self._brake_idx = int(self._parser.get('G29 Racing Wheel', 'brake'))
        self._reverse_idx = int(self._parser.get('G29 Racing Wheel', 'reverse'))
        self._handbrake_idx = int(
            self._parser.get('G29 Racing Wheel', 'handbrake'))

    def parse_events(self, world, clock):
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                return True
            elif event.type == pygame.JOYBUTTONDOWN:
                if event.button == 0:
                    world.restart()
                elif event.button == 3:
                    world.next_weather() # tbd
                elif event.button == self._reverse_idx:
                    self._control.gear = 1 if self._control.reverse else -1
                elif event.button == 10: ## wheel - R3
                    self._autopilot_enabled = not self._autopilot_enabled
                    world.ego_vehicle.set_autopilot(self._autopilot_enabled)
                    print("press R3")

            elif event.type == pygame.KEYUP:
                if self._is_quit_shortcut(event.key):
                    return True
                elif event.key == K_BACKSPACE:
                    world.restart() #TBD

                if isinstance(self._control, carla.VehicleControl):
                    if event.key == K_q:
                        self._control.gear = 1 if self._control.reverse else -1
                    elif event.key == K_m:
                        self._control.manual_gear_shift = not self._control.manual_gear_shift
                        self._control.gear = world.ego_vehicle.get_control().gear

                    elif self._control.manual_gear_shift and event.key == K_COMMA:
                        self._control.gear = max(-1, self._control.gear - 1)
                    elif self._control.manual_gear_shift and event.key == K_PERIOD:
                        self._control.gear = self._control.gear + 1
                    elif event.key == K_p:
                        self._autopilot_enabled = not self._autopilot_enabled
                        world.ego_vehicle.set_autopilot(self._autopilot_enabled)

        if not self._autopilot_enabled:
            if isinstance(self._control, carla.VehicleControl):
                self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time())
                self._parse_vehicle_wheel()
                self._control.reverse = self._control.gear < 0
            elif isinstance(self._control, carla.WalkerControl):
                self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time())
            world.ego_vehicle.apply_control(self._control)

    def _parse_vehicle_keys(self, keys, milliseconds):

        self._control.throttle = 1.0 if keys[K_UP] or keys[K_w] else 0.0
        steer_increment = 5e-4 * milliseconds
        if keys[K_LEFT] or keys[K_a]:
            self._steer_cache -= steer_increment
        elif keys[K_RIGHT] or keys[K_d]:
            self._steer_cache += steer_increment
        else:
            self._steer_cache = 0.0
        self._steer_cache = min(0.7, max(-0.7, self._steer_cache))
        self._control.steer = round(self._steer_cache, 1)
        self._control.brake = 1.0 if keys[K_DOWN] or keys[K_s] else 0.0
        self._control.hand_brake = keys[K_SPACE]

    def _parse_vehicle_wheel(self):
        numAxes = self._joystick.get_numaxes()
        jsInputs = [float(self._joystick.get_axis(i)) for i in range(numAxes)]
        # print (jsInputs)
        jsButtons = [float(self._joystick.get_button(i)) for i in
                     range(self._joystick.get_numbuttons())]

        # Custom function to map range of inputs [1, -1] to outputs [0, 1] i.e 1 from inputs means nothing is pressed
        # For the steering, it seems fine as it is
        K1 = 1.0  # 0.55
        steerCmd = K1 * math.tan(1.1 * jsInputs[self._steer_idx])

        K2 = 1.6  # 1.6
        throttleCmd = K2 + (2.05 * math.log10(
            -0.7 * jsInputs[self._throttle_idx] + 1.4) - 1.2) / 0.92
        if throttleCmd <= 0:
            throttleCmd = 0
        elif throttleCmd > 1:
            throttleCmd = 1

        brakeCmd = 1.6 + (2.05 * math.log10(
            -0.7 * jsInputs[self._brake_idx] + 1.4) - 1.2) / 0.92
        if brakeCmd <= 0:
            brakeCmd = 0
        elif brakeCmd > 1:
            brakeCmd = 1

        self._control.steer = steerCmd
        self._control.brake = brakeCmd
        self._control.throttle = throttleCmd
        #toggle = jsButtons[self._reverse_idx]
        self._control.hand_brake = bool(jsButtons[self._handbrake_idx])

    @staticmethod
    def _is_quit_shortcut(key):
        return (key == K_ESCAPE) or (key == K_q and pygame.key.get_mods() & KMOD_CTRL)

where by pushing R3 on the G29 Steerwheel or Key-P on keyboard, the autopilot-mode should be able to toggle and run the manual driving. I got it work on the asynchronous mode (revised on the manual_control_steeringwheel.py)

However, with the synchronous mode, (implemented as following):

        ## TEST the HUD
        pygame.init()

        display = pygame.display.set_mode(
            (400, 300),
            pygame.HWSURFACE | pygame.DOUBLEBUF)
        font = get_font()
        clock = pygame.time.Clock()        

        with CarlaSyncMode(self.world, self.front_rgb, fps=30) as sync_mode:
            # Skip initial frames where the car is being put on the ambient
            # TBD

            while True:
                # if should_quit():
                #     return
                # clock.tick()          

                snapshot, front_rgb = sync_mode.tick(timeout=10.0)  # If needed, self.frame can be obtained too
                # Processing raw data
                ego_speed = self.ego_vehicle.get_velocity()
                print(ego_speed)

                # # Processing raw data
               ...
                # store data 
                self.total_recorded_frames += 1

                ## Controller
                controller = DualControl(self, True)

                clock.tick_busy_loop(30)
                if controller.parse_events(world, clock):
                    return               

## follow up with recordings and draw the display...

It can run without pushing any key aka the statement

controller.parse_events(world, clock)

is not true. Whenever I push the key to enter this if statement, I got the error message like

[jackbart94]

Hi,

In order to help you, we must know what version of CARLA you are using. There are changes to the autopilot system in 0.9.7. and that might be causing this error.

[ChenSunMac]

Hi,

In order to help you, we must know what version of CARLA you are using. There are changes to the autopilot system in 0.9.7. and that might be causing this error.

I am using 0.9.7

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[ChenSunMac]

repackaged the program and works fine