Beispiel #1
0
    def step(self, action):
        vel, angle = action

        # Distance between the wheels
        baseline = self.unwrapped.wheel_dist

        # 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])

        obs, reward, done, info = Simulator.step(self, vels)
        mine = {}
        mine['k'] = self.k
        mine['gain'] = self.gain
        mine['train'] = self.trim
        mine['radius'] = self.radius
        mine['omega_r'] = omega_r
        mine['omega_l'] = omega_l
        info['DuckietownEnv'] = mine
        return obs, reward, done, info
action_dim = env.action_space.shape[0]
max_action = float(env.action_space.high[0])

# Initialize policy
policy = DDPG(state_dim, action_dim, max_action, net_type="dense")
policy.load("model", directory="./models", for_inference=True)

with torch.no_grad():
    while True:
        env.reset()
        obs = env.get_features()
        env.render()
        rewards = []
        while True:
            action = policy.predict(np.array(obs))
            _, rew, done, misc = env.step(action)
            obs = env.get_features()
            rewards.append(rew)
            env.render()

            try:
                lp = env.get_lane_pos2(env.cur_pos, env.cur_angle)
                dist = abs(lp.dist)
            except:
                dist = 0

            print(f"Reward: {rew:.2f}",
                  f"\t| Action: [{action[0]:.3f}, {action[1]:.3f}]",
                  f"\t| Speed: {env.speed:.2f}", f"\t| Dist.: {dist:.3f}")

            cv2.imshow("obs", obs)
Beispiel #3
0
        img_msg.data = np.array(cv2.imencode('.jpg', image)[1]).tostring()

        self.pub_img.publish(img_msg)


if __name__ == '__main__':
    wrapper = RosWrapperNode(node_name='ros_wrapper_node')
    env = Simulator(
        seed=123,  # random seed
        map_name="loop_empty",
        max_steps=500001,  # we don't want the gym to reset itself
        domain_rand=0,
        camera_width=640,
        camera_height=480,
        accept_start_angle_deg=4,  # start close to straight
        full_transparency=True,
        distortion=True,
    )
    obs = env.reset()
    wrapper.publish_img(obs)

    while not rospy.is_shutdown():
        action = [wrapper.action_l, wrapper.action_r]
        obs, reward, done, _ = env.step(action)

        if done:
            obs = env.reset()

        wrapper.publish_img(obs)
        rospy.Rate(15).sleep()
#!/usr/bin/env python3
import gym_duckietown
from gym_duckietown.simulator import Simulator
env = Simulator(
    seed=123,  # random seed
    map_name="loop_empty",
    max_steps=500001,  # we don't want the gym to reset itself
    domain_rand=0,
    camera_width=640,
    camera_height=480,
    accept_start_angle_deg=4,  # start close to straight
    full_transparency=True,
    distortion=True,
)
while True:
    action = [0.1, 0.1]
    observation, reward, done, misc = env.step(action)
    #print(observation)
    env.render()
    if done:
        env.reset()
class ROSWrapper(DTROS):
    '''
    Class that creates a wrapper between the duckietown simulator and the ROS
    interface of a fake robot simulated in the host. The node serves as interface
    for the following parts:
        - Subscribes to the wheels command topic and transforms it into actions in simulation
        - Publishes the rendered observations in the simulator to the camera image topic
    '''
    def __init__(self, node_name):
        # initialize the DTROS parent class
        super(ROSWrapper, self).__init__(node_name=node_name,
                                         node_type=NodeType.GENERIC)

        # construct rendered observation publisher
        self.camera_pub = rospy.Publisher('~/' + VEHICLE_NAME +
                                          '/camera_node/image/compressed',
                                          CompressedImage,
                                          queue_size=1)

        # Construct subscriber for wheel_cmd topic
        self.wheels_cmd_sub = rospy.Subscriber(
            '~/' + VEHICLE_NAME + '/wheels_driver_node/wheels_cmd',
            WheelsCmdStamped,
            self.wheels_cmd_cb,
            queue_size=1)

        # Initialize the simulator
        self.env = Simulator(
            seed=123,  # random seed
            map_name="loop_empty",
            max_steps=500001,  # we don't want the gym to reset itself
            domain_rand=0,
            camera_width=640,
            camera_height=480,
            accept_start_angle_deg=4,  # start close to straight
            full_transparency=True,
            distortion=True,
        )
        # Create action variable
        self.action = [0.0, 0.0]

    def wheels_cmd_cb(self, msg):
        '''
        Callback that maps wheel commands into actions in the simulator.

            Creates the simulated robot actions based on the received command
            through the main topic in the duckietown infrastructure.

            Args:
                msg (WheelsCmdStamped): Velocity command
        '''

        vel_left = msg.vel_left
        vel_right = msg.vel_right

        self.action = [vel_left, vel_right]

    def run(self):
        '''
        Continuously run the wrapper to map ROS interface with simulator.

            This method runs the node, and begins an iterative process where the
            image is retrieved from the simulator and published in the camera image topic,
            and the wheels command received by subscribing to the wheels_cmd topic is translated
            into actions executed in the simulator. The process iterates until the node is terminated

        '''

        while not rospy.is_shutdown():

            # Update the simulator
            # the action message is updated every time the wheels_cmd_cb is called
            # that is, every time a message arrives.
            observation, reward, done, misc = self.env.step(self.action)
            # Render the new state
            self.env.render()

            # Set again action to stop, in case no more messages are being received
            self.action = [0.0, 0.0]

            if done:
                # Reset the simulator when the robot goes out of the road.
                self.env.reset()

            # Transalte the observation into a CompressedImage message to publish it
            bgr_obs = cv2.cvtColor(observation, cv2.COLOR_RGB2BGR)
            bridge = CvBridge()
            img_msg = bridge.cv2_to_compressed_imgmsg(cvim=bgr_obs,
                                                      dst_format="jpg")

            # Publish the image in the /image/compressed topic
            self.camera_pub.publish(img_msg)
    def move(self, velocities):
        obs, reward, done, info = Simulator.step(self, np.array(velocities))

        return obs  # , reward, done
Beispiel #7
0
class MyROSWrapperNode(DTROS):
    def __init__(self, node_name):
        # initialize the DTROS parent class
        #color = rospy.get_param('~color')
        super(MyROSWrapperNode, self).__init__(node_name=node_name,
                                               node_type=NodeType.GENERIC)
        # construct Publisher and Subscriber
        self.pub = rospy.Publisher('/fakebot/camera_node/image/compressed',
                                   CompressedImage,
                                   queue_size=10)
        self.sub = rospy.Subscriber('/fakebot/wheels_driver_node/wheels_cmd',
                                    WheelsCmdStamped, self.callback)
        #self.sub = rospy.Subscriber('chatter', String, self.callback)
        self.iter = 0

        # create bag files:
        # self.bag = bag

        self.env = Simulator(
            seed=123,  # random seed
            map_name="loop_empty",
            max_steps=500001,  # we don't want the gym to reset itself
            domain_rand=0,
            camera_width=640,
            camera_height=480,
            accept_start_angle_deg=4,  # start close to straight
            full_transparency=True,
            distortion=True,
        )

        self.br = CvBridge()
        self.right_wheel = 0
        self.left_wheel = 0

    def callback(self, data):
        if data:
            # update wheel command to action
            self.right_wheel = data.vel_right
            self.left_wheel = data.vel_left

            action = [self.left_wheel, self.right_wheel]
            print("I got command:", action)
            observation, reward, done, misc = self.env.step(action)
            # env.render()

            # print("observation.shape:",observation.shape )
            # observation.shape: (480, 640, 3)
            # print("observation.type:", type(observation))
            # observation.type: <class 'numpy.ndarray'>

            # publish image to viewer
            # np_arr = np.fromstring(data.data, np.uint8)
            # img = cv2.imdecode(observation, cv2.IMREAD_COLOR)

            img = cv2.cvtColor(np.asarray(observation), cv2.COLOR_RGB2BGR)
            compressed_img_msg = self.br.cv2_to_compressed_imgmsg(
                img, dst_format='jpg')
            self.pub.publish(compressed_img_msg)

            if done:
                self.env.reset()
        else:
            rospy.loginfo("waiting for Publisher")
Beispiel #8
0
class DuckiegymRosWrapperNode(DTROS):
    """
    Run a gym-duckietown simulator, redirect the simulator's image to a ros topic, redirect the wheel commands
    to simulator actions.
    """
    def __init__(self, node_name):
        super(DuckiegymRosWrapperNode,
              self).__init__(node_name=node_name, node_type=NodeType.GENERIC)

        self.action = [0, 0]  # wheel commands for simulator
        self.camera_info = self.load_camera_info(
            "/data/config/calibrations/camera_intrinsic/default.yaml")

        self.bridge = CvBridge()

        vehicle_name = os.environ.get("VEHICLE_NAME")
        image_topic = "/" + vehicle_name + "/camera_node/image/compressed"
        self.pub_img = rospy.Publisher(image_topic, CompressedImage)
        camera_info_topic = "/" + vehicle_name + "/camera_node/camera_info"
        self.pub_camera_info = rospy.Publisher(camera_info_topic, CameraInfo)
        wheels_cmd_topic = "/" + vehicle_name + "/wheels_driver_node/wheels_cmd"
        self.sub_wheels_cmd = rospy.Subscriber(wheels_cmd_topic,
                                               WheelsCmdStamped,
                                               self.callback_wheels_cmd,
                                               queue_size=1)

        self.sim = Simulator(
            seed=123,  # random seed
            map_name="loop_empty",
            max_steps=500001,  # we don't want the gym to reset itself
            domain_rand=0,
            camera_width=640,
            camera_height=480,
            accept_start_angle_deg=4,  # start close to straight
            full_transparency=True,
            distortion=False,
        )

    def run_simulator(self):
        """
        Main loop of this node, runs simulation loop and camera publishers.
        """
        while not rospy.is_shutdown():
            observation, reward, done, misc = self.sim.step(self.action)
            self.publish_camera_info()
            self.publish_camera_image(observation)
            self.sim.render()
            if done:
                self.sim.reset()

    def callback_wheels_cmd(self, msg):
        """
        Update action variable (parameter for simulator) on callback.
        """
        self.action = [msg.vel_left, msg.vel_right]

    def publish_camera_image(self, observation):
        """
        Publish simulated camera frame as CompressedImage message.
        """
        img = self.bridge.cv2_to_compressed_imgmsg(cv2.cvtColor(
            observation, cv2.COLOR_RGB2BGR),
                                                   dst_format="jpg")
        img.header.stamp = rospy.Time.now()
        self.pub_img.publish(img)

    def publish_camera_info(self):
        """
        Publish calibration info about camera.
        """
        self.camera_info.header.stamp = rospy.Time.now()
        self.pub_camera_info.publish(self.camera_info)

    def load_camera_info(self, filename):
        """
        Load calibration info about camera from config file.
        """
        with open(filename, 'r') as stream:
            calib_data = yaml.load(stream)
        cam_info = CameraInfo()
        cam_info.width = calib_data['image_width']
        cam_info.height = calib_data['image_height']
        cam_info.K = calib_data['camera_matrix']['data']
        cam_info.D = calib_data['distortion_coefficients']['data']
        cam_info.R = calib_data['rectification_matrix']['data']
        cam_info.P = calib_data['projection_matrix']['data']
        cam_info.distortion_model = calib_data['distortion_model']
        return cam_info