def _publish_img(self, obs): """ Publishes the image to the compressed_image topic, which triggers the lane following loop """ logger.info("got image") 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 on_received_get_commands(self, context, data): logger.info("Agent received GetCommand request") while not self.agent.updated: time.sleep(0.01) pwm_left, pwm_right = self.agent.action if self.agent.started: # before starting, we send empty commnands to keep connection self.agent.updated = False rgb = {'r': 0.5, 'g': 0.5, 'b': 0.5} commands = { 'wheels': { 'motor_left': pwm_left, 'motor_right': pwm_right }, 'LEDS': { 'center': rgb, 'front_left': rgb, 'front_right': rgb, 'back_left': rgb, 'back_right': rgb } } context.write('commands', commands)
def __init__(self): logger.info('ROSAgent::__init__ starts') # Get the vehicle name, which comes in as HOSTNAME self.vehicle = os.getenv('HOSTNAME') logger.info('Creating subscriber') # Subscribes to the output of the lane_controller_node 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, 0.0]) self.updated = True # This is set such that we send zero commands when we have not yet received commands # from ros, prevents timeouts for local development self.started = False # Publishes onto the corrected image topic # since image out of simulator is currently rectified logger.info('creating publishers') self.cam_pub = rospy.Publisher( '/{}/camera_node/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 logger.info('Calling init_node') try: rospy.init_node('ROSAgent', log_level=rospy.INFO) logger.info('node initialized') except BaseException as e: logger.info('exception in init_node: %s' % e) raise # 15Hz ROS Cycle - TODO: What is this number? # self.r = rospy.Rate(15) logger.info('ROSAgent::__init__ complete.')
def on_received_observations(self, context, data): logger.info("received observation") jpg_data = data['camera']['jpg_data'] obs = jpg2rgb(jpg_data) self.agent._publish_img(obs) self.agent._publish_info()
def __init__(self): logger.info('started __init__()') # Now, initialize the ROS stuff here: # logger.info('Configuring logging') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # print('configured logging 2') roslaunch_path = os.path.join(os.getcwd(), "lf_slim.launch") logger.info('Creating ROSLaunchParent') self.launch = roslaunch.parent.ROSLaunchParent(uuid, [roslaunch_path]) logger.info('about to call start()') self.launch.start() logger.info('returning from start()') # Start the ROSAgent, which handles publishing images and subscribing to action logger.info('starting ROSAgent()') self.agent = ROSAgent() logger.info('started ROSAgent()') logger.info('completed __init__()')
def on_received_episode_start(self, context, data): logger.info("Starting episode") context.info('Starting episode %s.' % data)
def on_received_seed(self, context, data): logger.info('Received seed from pipes') np.random.seed(data)
def __init__(self, in_sim, launch_file): # Now, initialize the ROS stuff here: vehicle_name = os.getenv('VEHICLE_NAME') # The in_sim switch is used for local development # in that case, we do not start a launch file if not in_sim: # logger.info('Configuring logging') uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) # roslaunch.configure_logging(uuid) # print('configured logging 2') roslaunch_path = os.path.join(os.getcwd(), launch_file) logger.info('Creating ROSLaunchParent') self.launch = roslaunch.parent.ROSLaunchParent( uuid, [roslaunch_path]) logger.info('about to call start()') self.launch.start() logger.info('returning from start()') # Start the ROSAgent, which handles publishing images and subscribing to action logger.info('starting ROSAgent()') self.agent = ROSAgent() logger.info('started ROSAgent()') logger.info('completed __init__()')
data = np.array(im) assert data.ndim == 3 assert data.dtype == np.uint8 return data if __name__ == '__main__': # The following can be set in the environment file LOGLEVEL = os.environ.get('LOGLEVEL', 'INFO').upper() logger.setLevel(LOGLEVEL) logger.warn("Logger set to level: " + str(logger.level)) if logger.level > 20: logger.warn( "Logging is set to {}, info msg will no be shown".format(LOGLEVEL)) logger.info("Started solution2.py") parser = argparse.ArgumentParser() parser.add_argument("-s", "--sim", action="store_true", help="Add this option to start the car interface") parser.add_argument( "--launch_file", default="lf_slim.launch", help="launch file that should be used (default: lf_slim.launch") args = parser.parse_args() agent = ROSBaselineAgent(in_sim=args.sim, launch_file=args.launch_file) logger.info("Created agent in solution2 main") wrap_direct(agent)