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)