demo_controller.py

import time
import sys
import math
import numpy as np
import gym
from gym_duckietown.envs import DuckietownEnv

# Default environment
env = DuckietownEnv(map_name='udem1', domain_rand = False, draw_bbox = False)

# Controller callable object. Default PI regulator
class Controller( object ):
    def __init__(self):
        self.I = 0
        self.K_P = 10
        self.K_I = 2
        self.v = 0.2

    def __call__(self, dist_est, angle_est):
        err = dist_est + angle_est
        P = self.K_P * err
        omega = P + self.I
    
        self.I = self.I + self.K_I*err

        return (self.v, omega)

obs = env.reset()
env.render()

controller = Controller()
total_reward = 0

while True:

    # Get pose of the robot on the lane, distance to road center and angle
    # between robot pose and lane
    lane_pose = env.get_lane_pos(env.cur_pos, env.cur_angle)
    dist_to_road_center = lane_pose.dist
    angle_lane_robot = lane_pose.angle_rad

    # Define speed and regulator

    obs, reward, end, info = env.step(controller(dist_to_road_center, angle_lane_robot))
    total_reward += reward

    print("Step: {}, step_reward: {}, total_reward: {}".format(env.step_count, reward, total_reward))

    env.render()

    if end:
        if reward < 0:
            print("----CRASHED------")
        print("Total reward: {}".format(total_reward))
        break