def __init__(self): # Get the vehicle name, which comes in as HOSTNAME self.vehicle = os.getenv('HOSTNAME') # Get the hostname to conect to server host = os.getenv("DUCKIETOWN_SERVER", "localhost") # Create ZMQ connection with Server self.sim = RemoteRobot(host, silent=False) # Subscribes to the output of the lane_controller_node and IK node self.action_sub = rospy.Subscriber('/{}/lane_controller_node/car_cmd'.format( self.vehicle), Twist2DStamped, self._action_cb) self.ik_action_sub = rospy.Subscriber('/{}/wheels_driver_node/wheels_cmd'.format( self.vehicle), WheelsCmdStamped, self._ik_action_cb) # Place holder for the action self.action = np.array([0, 0]) # Publishes onto the corrected image topic # since image out of simulator is currently rectified self.cam_pub = rospy.Publisher('/{}/corrected_image/compressed'.format( self.vehicle), CompressedImage, queue_size=10) # Publisher for camera info - needed for the ground_projection self.cam_info_pub = rospy.Publisher('/{}/camera_node/camera_info'.format( self.vehicle), CameraInfo, queue_size=1) # Initializes the node rospy.init_node('ROSAgent') # 10Hz ROS Cycle - TODO: What is this number? self.r = rospy.Rate(10)
def __init__( self, host, gain=-1.0, trim=0.0, radius=0.0318, k=27.0, limit=1.0 ): RemoteRobot.__init__(self, host=host, silent=True) self.action_space = spaces.Box( low=np.array([-1, -1]), high=np.array([1, 1]), dtype=np.float32 ) # Should be adjusted so that the effective speed of the robot is 0.2 m/s self.gain = gain # Directional trim adjustment self.trim = trim # Wheel radius self.radius = radius # Motor constant self.k = k # Wheel velocity limit self.limit = limit
def __init__(self): print("Hello!") host = os.getenv("DUCKIETOWN_SERVER", "localhost") # Create ZMQ connection self.sim = RemoteRobot(host, silent=False) self.rosmaster = os.getenv('HOSTNAME') self.action_sub = rospy.Subscriber( '/{}/lane_controller_node/car_cmd'.format(self.rosmaster), Twist2DStamped, self._action_cb) self.cam_pub = rospy.Publisher('/{}/corrected_image/compressed'.format( self.rosmaster), CompressedImage, queue_size=10) self.action = np.array([0, 0]) self.cam_info_pub = rospy.Publisher( '/{}/camera_node/camera_info'.format(self.rosmaster), CameraInfo, queue_size=1) rospy.init_node('RemoteRobotRos') rospy.set_param('{}/verbose'.format(os.getenv('HOSTNAME')), "true") self.r = rospy.Rate(10)
def __init__(self, silent=False): # Produce the occasional print self.silent = silent # In the docker container this will be set to point to the # hostname of the `gym-duckietown-server` container, but in # the local test environment this will just map to localhost host = os.getenv("DUCKIETOWN_SERVER", "localhost") self.action_sub = rospy.Subscriber('/cmd', String, self._action_cb) self.reset_sub = rospy.Subscriber('/2', Bool, self._reset_cb) self.cam_pub = rospy.Publisher('/img/compressed', CompressedImage, queue_size=10) self.action_debug_pub = rospy.Publisher('/1', String, queue_size=10) self.action = [0., 0.] # Create ZMQ connection self.sim = RemoteRobot(host, silent=self.silent) # Tuple of velocity and steering angle, each in the range # [-1, 1] for full speed ahead, full speed backward, full # left turn, and full right turn respectively self.action_space = spaces.Box(low=-1, high=1, shape=(2, ), dtype=np.float32) # We observe an RGB image with pixels in [0, 255] # Note: the pixels are in uint8 format because this is more compact # than float32 if sent over the network or stored in a dataset self.observation_space = spaces.Box(low=0, high=255, shape=(CAMERA_HEIGHT, CAMERA_WIDTH, 3), dtype=np.uint8) # Create a black image buffer for the last observation self.last_obs = np.zeros((CAMERA_HEIGHT, CAMERA_WIDTH, 3), np.uint8) self.reward_range = (-1000, 1000) self._windows_exists = False # Initialize the state self.seed() self.reset( ) # FIXME: I'm quite sure this has to be called by the agent by gym convention rospy.init_node('RemoteRobotRos')
def step(self, action, with_observation=True): vel, angle = action # Distance between the wheels baseline = 0.13 # assuming same motor constants k for both motors k_r = self.k k_l = self.k # adjusting k by gain and trim k_r_inv = (self.gain + self.trim) / k_r k_l_inv = (self.gain - self.trim) / k_l omega_r = (vel + 0.5 * angle * baseline) / self.radius omega_l = (vel - 0.5 * angle * baseline) / self.radius # conversion from motor rotation rate to duty cycle u_r = omega_r * k_r_inv u_l = omega_l * k_l_inv # limiting output to limit, which is 1.0 for the duckiebot u_r_limited = max(min(u_r, self.limit), -self.limit) u_l_limited = max(min(u_l, self.limit), -self.limit) vels = np.array([u_l_limited, u_r_limited]) return RemoteRobot.step(self, vels)
def __init__(self, debug=False): # Produce graphical output self.debug = debug # CURRENTLY UNUSED # In the docker container this will be set to point to the # hostname of the `gym-duckietown-server` container, but in # the local test environment this will just map to localhost host = os.getenv("DUCKIETOWN_SERVER", "localhost") # Create ZMQ connection self.sim = RemoteRobot(host) # Tuple of velocity and steering angle, each in the range # [-1, 1] for full speed ahead, full speed backward, full # left turn, and full right turn respectively self.action_space = spaces.Box( low=-1, high=1, shape=(2,), dtype=np.float32 ) # We observe an RGB image with pixels in [0, 255] # Note: the pixels are in uint8 format because this is more compact # than float32 if sent over the network or stored in a dataset self.observation_space = spaces.Box( low=0, high=255, shape=(CAMERA_HEIGHT, CAMERA_WIDTH, 3), dtype=np.uint8 ) # Create a black image buffer for the last observation self.last_obs = np.zeros((CAMERA_HEIGHT, CAMERA_WIDTH, 3), np.uint8) self.reward_range = (-1000, 1000) self._windows_exists = False self.sim_ready = False # Initialize the state self.seed() self.reset() # FIXME: I'm quite sure this has to be called by the agent by gym convention
class ROSBridge(object): def __init__(self): print("Hello!") host = os.getenv("DUCKIETOWN_SERVER", "localhost") # Create ZMQ connection self.sim = RemoteRobot(host, silent=False) self.rosmaster = os.getenv('HOSTNAME') self.action_sub = rospy.Subscriber( '/{}/lane_controller_node/car_cmd'.format(self.rosmaster), Twist2DStamped, self._action_cb) self.cam_pub = rospy.Publisher('/{}/corrected_image/compressed'.format( self.rosmaster), CompressedImage, queue_size=10) self.action = np.array([0, 0]) self.cam_info_pub = rospy.Publisher( '/{}/camera_node/camera_info'.format(self.rosmaster), CameraInfo, queue_size=1) rospy.init_node('RemoteRobotRos') rospy.set_param('{}/verbose'.format(os.getenv('HOSTNAME')), "true") self.r = rospy.Rate(10) def _action_cb(self, msg): v = msg.v omega = msg.omega self.action = np.array([v, omega]) def _publish_info(self): self.cam_info_pub.publish(CameraInfo()) def _publish_img(self, obs): img_msg = CompressedImage() time = rospy.get_rostime() img_msg.header.stamp.secs = time.secs img_msg.header.stamp.nsecs = time.nsecs img_msg.format = "jpeg" contig = cv2.cvtColor(np.ascontiguousarray(obs), cv2.COLOR_BGR2RGB) img_msg.data = np.array(cv2.imencode('.jpg', contig)[1]).tostring() self.cam_pub.publish(img_msg) def spin(self): while not rospy.is_shutdown(): img, r, d, _ = self.sim.step(self.action) self._publish_img(img) self._publish_info() self.r.sleep()
class SimpleSimAgentEnv(gym.Env): """ Remote client for the simple Duckietown road simulator. Draws a road with turns using OpenGL, and simulates basic differential-drive dynamics. """ metadata = { 'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 30 # TODO: do we need this on the client? } def __init__(self, silent=False): # Produce the occasional print self.silent = silent # In the docker container this will be set to point to the # hostname of the `gym-duckietown-server` container, but in # the local test environment this will just map to localhost host = os.getenv("DUCKIETOWN_SERVER", "localhost") # Create ZMQ connection self.sim = RemoteRobot(host, silent=self.silent) # Tuple of velocity and steering angle, each in the range # [-1, 1] for full speed ahead, full speed backward, full # left turn, and full right turn respectively self.action_space = spaces.Box( low=-1, high=1, shape=(2,), dtype=np.float32 ) # We observe an RGB image with pixels in [0, 255] # Note: the pixels are in uint8 format because this is more compact # than float32 if sent over the network or stored in a dataset self.observation_space = spaces.Box( low=0, high=255, shape=(CAMERA_HEIGHT, CAMERA_WIDTH, 3), dtype=np.uint8 ) # Create a black image buffer for the last observation self.last_obs = np.zeros((CAMERA_HEIGHT, CAMERA_WIDTH, 3), np.uint8) self.reward_range = (-1000, 1000) self._windows_exists = False # Initialize the state self.seed() #self.reset() # FIXME: I'm quite sure this has to be called by the agent by gym convention def reset(self): """ Reset the simulation at the start of a new episode This also randomizes many environment parameters (domain randomization) """ return self.sim.reset() def close(self): """ Doesn't do anything, but should be used to end the simulation by gym convention """ pass def seed(self, seed=None): # TODO: for now this function doesn't do anything. # The seed is on the side of the server. Therefore we # must transmit the seed to the server. But what if the # server is already running and seeded? # self.np_random, _ = seeding.np_random(seed) return [seed] def step(self, action): """ Steps the simulation. Run action and get observation and reward. :param action: tuple|list|ndarray of exactly 2 floating point values each in range [-1,1] indicating the forward/backward speed and the steering angle respectively. :return: tuple of observation (image as ndarray), reward (float scalar), done (bool), misc (empty dict) """ assert len(action) == 2 action = np.array(action) obs, rew, done, misc = self.sim.step(action, with_observation=True) return obs, rew, done, misc def _create_window(self): """ Create a new matplotlib window if none exists to render observations. :return: """ ## This following bit is important because duckietown_slimremote ## uses the Agg backend natively to prevent errors on headless ## servers. So if we want to use matplotlib we have to switch ## the backend import matplotlib from matplotlib import pyplot as plt gui_env = ['TKAgg', 'GTKAgg', 'Qt4Agg', 'WXAgg'] for gui in gui_env: try: plt.switch_backend(gui) break except: continue if not self.silent: print("Using Matplotlib backend:", matplotlib.get_backend()) ## actually create the render window plt.ion() img = np.zeros((CAMERA_HEIGHT, CAMERA_WIDTH, 3)) self._plt_img = plt.imshow(img, interpolation='none', animated=True, label="Duckiebot Camera") self._plt_ax = plt.gca() def _draw_window(self, obs): """ Update the matplotlib observation window. :param obs: :return: """ if obs is not None: self._plt_img.set_data(obs) self._plt_ax.plot([0]) plt.pause(0.001) # I found this necessary - otherwise no visible img def render(self, mode='human', close=False): """ Either create a matplotlib window of the last observation or return the last observation as ndarray. :param mode: string, possible values are "human" for rendering the observations in a matplotlib window and "rgb_array" to return the last observation as ndarray. :param close: :return: """ obs, _, _, _ = self.sim.observe() if mode == "rgb_array": return obs else: if not self._windows_exists: self._create_window() self._windows_exists = True self._draw_window(obs)
def render_obs(self): return RemoteRobot.observe(self)[0]
class ROSAgent(object): def __init__(self): # Get the vehicle name, which comes in as HOSTNAME self.vehicle = os.getenv('HOSTNAME') # Get the hostname to conect to server host = os.getenv("DUCKIETOWN_SERVER", "localhost") # Create ZMQ connection with Server self.sim = RemoteRobot(host, silent=False) # Subscribes to the output of the lane_controller_node and IK node self.action_sub = rospy.Subscriber('/{}/lane_controller_node/car_cmd'.format( self.vehicle), Twist2DStamped, self._action_cb) self.ik_action_sub = rospy.Subscriber('/{}/wheels_driver_node/wheels_cmd'.format( self.vehicle), WheelsCmdStamped, self._ik_action_cb) # Place holder for the action self.action = np.array([0, 0]) # Publishes onto the corrected image topic # since image out of simulator is currently rectified self.cam_pub = rospy.Publisher('/{}/corrected_image/compressed'.format( self.vehicle), CompressedImage, queue_size=10) # Publisher for camera info - needed for the ground_projection self.cam_info_pub = rospy.Publisher('/{}/camera_node/camera_info'.format( self.vehicle), CameraInfo, queue_size=1) # Initializes the node rospy.init_node('ROSAgent') # 10Hz ROS Cycle - TODO: What is this number? self.r = rospy.Rate(10) def _action_cb(self, msg): """ Callback to listen to last outputted action from lane_controller_node Stores it and sustains same action until new message published on topic """ v = msg.v omega = msg.omega def _ik_action_cb(self, msg): """ Callback to listen to last outputted action from lane_controller_node Stores it and sustains same action until new message published on topic """ vl = msg.vel_left vr = msg.vel_right self.action = np.array([vl, vr]) def _publish_info(self): """ Publishes a default CameraInfo - TODO: Fix after distortion applied in simulator """ self.cam_info_pub.publish(CameraInfo()) def _publish_img(self, obs): """ Publishes the image to the compressed_image topic, which triggers the lane following loop """ img_msg = CompressedImage() time = rospy.get_rostime() img_msg.header.stamp.secs = time.secs img_msg.header.stamp.nsecs = time.nsecs img_msg.format = "jpeg" contig = cv2.cvtColor(np.ascontiguousarray(obs), cv2.COLOR_BGR2RGB) img_msg.data = np.array(cv2.imencode('.jpg', contig)[1]).tostring() self.cam_pub.publish(img_msg) def spin(self): """ Main loop Steps the sim with the last action at rate of 10Hz """ while not rospy.is_shutdown(): img, r , d, _ = self.sim.step(self.action) self._publish_img(img) self._publish_info() self.r.sleep()
from duckietown_slimremote.pc.robot import RemoteRobot robot = RemoteRobot( "10.204.6.223") # currently in the lab, can't use zeroconf/avahi # for i in range(5): # robot.step(action=[0, 0, 1, 0, 0]) # time.sleep(0.5) # robot.step(action=[0, 0, 0, 1, 0]) # time.sleep(0.5) # robot.step(action=[0, 0, 0, 0, 1]) # time.sleep(0.5) robot.step([0] * 5)