def start_nodes():
if not DEVELOP:
# Stop camera triggers
camera_trigger.stop()
# Start camera nodes and wait until they are ready
camera_master.set_camera_launch_param(
frame_rate='camera_driver',
trigger=True
)
camera_master.start_cameras()
while not mct_introspection.camera_nodes_ready(mode='calibration'):
time.sleep(0.2)
# Delay until all camera nodes are ready and start triggers
time.sleep(10)
frame_rates = file_tools.read_frame_rates()
camera_trigger.start(frame_rates['zoom_calibration'])
# Start zoom tool nodes and wait until they are ready
zoom_tool_master.start()
while not mct_introspection.zoom_tool_image_ready():
time.sleep(0.2)
# Start mjpeg servers and throttleing
mjpeg_servers.set_topics(['image_zoom_tool'])
mjpeg_servers.start_servers()
# Turn on active target two led patter
mct_active_target.led_pair()
def start_cameras_and_mjpeg_servers():
"""
Starts the cameras and mjpeg servers
"""
if not DEVELOP:
# Stop camera triggers
camera_trigger.stop()
# Start cameras
camera_master.set_camera_launch_param(
frame_rate='camera_driver',
trigger=True
)
camera_master.start_cameras()
# Wait until the camera nodes are ready and then start the mjpeg servers
while not mct_introspection.camera_nodes_ready(mode='calibration'):
time.sleep(0.2)
# Delay until all camera nodes are ready and start triggers
time.sleep(10)
frame_rates = file_tools.read_frame_rates()
camera_trigger.start(frame_rates['camera_calibration'])
# Start mjpeg servers
mjpeg_servers.set_topics(['image_raw'])
mjpeg_servers.start_servers()
target_info = file_tools.read_target_info(config.camera_calib_target_type)
calibrator_master.start(target_info['size'], target_info['square'])
def __init__(self, framerate_mode):
self.tmp_dir = tempfile.gettempdir()
self.launch_file = os.path.join(self.tmp_dir,
'frame_drop_corrector.launch')
self.corrector_popen = None
framerate_dict = file_tools.read_frame_rates()
self.framerate = float(framerate_dict[framerate_mode])
rospy.on_shutdown(self.clean_up)
rospy.init_node('frame_drop_corrector_master')
self.camera_srv = rospy.Service(
'frame_drop_corrector_master',
CommandString,
self.handle_corrector_srv,
)
def setup_redis_db():
# Create db and add empty camera assignment
db = redis.Redis('localhost', db=config.redis_db)
# Add regions dictionary
regions_dict = file_tools.read_tracking_2d_regions()
redis_tools.set_dict(db, 'regions_dict', regions_dict)
# Add extra video dictionary
extra_video_dict = file_tools.read_logging_extra_video()
redis_tools.set_dict(db, 'extra_video_dict', extra_video_dict)
# Add mjpeg info dictionary
mjpeg_info_dict = mjpeg_servers.get_mjpeg_info_dict()
redis_tools.set_dict(db, 'mjpeg_info_dict', mjpeg_info_dict)
# Add external network interface
machine_def = mct_introspection.get_machine_def()
ip_iface_ext = iface_tools.get_ip_addr(
machine_def['mct_master']['iface-ext'])
redis_tools.set_str(db, 'ip_iface_ext', ip_iface_ext)
# Set operating mode
redis_tools.set_str(db, 'current_mode', 'preview')
## Get frame rates
frame_rate_dict = file_tools.read_frame_rates()
redis_tools.set_dict(db, 'frame_rate_dict', frame_rate_dict)
frame_rate = frame_rate_dict['tracking_2d']
# Get logging directory - add base path to directory, and create if it doesn't exist
logging_params_dict = file_tools.read_logging_params()
logging_params_dict['directory'] = os.path.join(
os.environ['HOME'], logging_params_dict['directory'])
if not os.path.exists(logging_params_dict['directory']):
os.mkdir(logging_params_dict['directory'])
redis_tools.set_dict(db, 'logging_params_dict', logging_params_dict)
# Get lighting params
mightex_params_dict = file_tools.read_mightex_params()
lighting_params_dict = mightex_to_lighting_params(mightex_params_dict)
redis_tools.set_dict(db, 'lighting_params_dict', lighting_params_dict)
return db
def start_nodes():
if not DEVELOP:
# Stop camera triggers
camera_trigger.stop()
file_tools.rsync_camera_calibrations()
# Start camera nodes and wait until they are ready
camera_master.set_camera_launch_param(
frame_rate='camera_driver',
trigger=True
)
camera_master.start_cameras()
while not mct_introspection.camera_nodes_ready(mode='calibration'):
time.sleep(0.2)
# Delay until all camera nodes are ready and start triggers
time.sleep(10)
frame_rates = file_tools.read_frame_rates()
camera_trigger.start(frame_rates['transform_2d_calibration'])
# Start image_proc nodes and wait until they are ready
image_proc_master.start_image_proc()
while not mct_introspection.image_proc_nodes_ready():
time.sleep(0.2)
# Wait for rectified images to be ready - required for launching transform
# calibrators.
while not mct_introspection.image_rect_ready():
time.sleep(0.2)
# Start transform 2d calibrator nodes and wait until ready
transform_2d_calibrator_master.start()
while not mct_introspection.transform_2d_calibrator_nodes_ready():
time.sleep(0.2)
# Start mjpeg servers and throttleing
topic_list = [ 'image_transform_calibration_0', 'image_transform_calibration_1']
mjpeg_servers.set_topics(topic_list)
mjpeg_servers.start_servers()
def start_cameras_and_mjpeg_servers():
"""
Starts the cameras and mjpeg servers
"""
if not DEVELOP:
# Stop camera triggers
camera_trigger.stop()
# Wait until camera inspectors are running and then start cameras
while not mct_introspection.camera_inspectors_ready():
time.sleep(0.2)
camera_inspector_master.start_cameras()
# Delay until all camera nodes are ready and start triggers
time.sleep(10)
frame_rates = file_tools.read_frame_rates()
camera_trigger.start(frame_rates['assignment'])
# Wait until the camera nodes are ready and then start the mjpeg servers
while not mct_introspection.camera_nodes_ready(mode='inspector'):
time.sleep(0.2)
mjpeg_servers.set_topics(['image_raw'])
mjpeg_servers.start_servers()
)
camera_master.start_cameras()
# Wait until the camera nodes are ready and then start the mjpeg servers
while not mct_introspection.camera_nodes_ready(mode='calibration'):
time.sleep(0.2)
print('done')
# Delay until all camera nodes are ready and start triggers
print(' * camera nodes sync delay ... ', end='')
sys.stdout.flush()
time.sleep(10)
print('done')
print(' * starting camera triggers ... ', end='')
sys.stdout.flush()
frame_rates = file_tools.read_frame_rates()
camera_trigger.start(frame_rates['camera_calibration'])
print('done')
print(' * starting mjpeg servers... ',end='')
sys.stdout.flush()
mjpeg_servers.set_topics(['image_raw'])
mjpeg_servers.start_servers()
print('done')
print(' * starting camera calibrators ... ',end='')
sys.stdout.flush()
target_info = file_tools.read_target_info(config.camera_calib_target_type)
calibrator_master.start(target_info['size'], target_info['square'])
print('done')
def get_frame_rates():
"""
Reads the frame_rates.yaml file and returns a dictionary of the allowed frame
rates.
"""
return file_tools.read_frame_rates()
def tracking_2d_node_startup(debug=False):
# Stop camera triggers
debug_print(' * stopping camera triggers ... ', end='', debug=debug)
sys.stdout.flush()
camera_trigger.stop()
debug_print('done', debug=debug)
file_tools.rsync_camera_calibrations(verbose=True)
# Start camera nodes and wait until they are ready
debug_print(' * starting camera nodes ... ', end='', debug=debug)
sys.stdout.flush()
camera_master.set_camera_launch_param(frame_rate='camera_driver',
trigger=True)
camera_master.start_cameras()
while not mct_introspection.camera_nodes_ready(mode='tracking'):
time.sleep(ready_poll_dt)
debug_print('done', debug=debug)
# Delay until all camera nodes are ready
debug_print(' * camera nodes sync delay ... ', end='', debug=debug)
sys.stdout.flush()
time.sleep(camera_startup_t)
debug_print('done', debug=debug)
# Reset rand sync
debug_print(' * resetting rand sync ... ', end='', debug=debug)
sys.stdout.flush()
status = reset_rand_sync()
if not status:
raise RuntimeError, 'unable to reset rand_sync device'
debug_print('done', debug=debug)
# Restart camera triggers
debug_print(' * starting camera triggers ... ', end='', debug=debug)
sys.stdout.flush()
frame_rates = file_tools.read_frame_rates()
camera_trigger.start(frame_rates['tracking_2d'])
debug_print('done', debug=debug)
# Start image_proc nodes and wait until they are ready
debug_print(' * starting image proc nodes ... ', end='', debug=debug)
sys.stdout.flush()
image_proc_master.start_image_proc()
while not mct_introspection.image_proc_nodes_ready():
time.sleep(ready_poll_dt)
debug_print('done', debug=debug)
# Wait for image rect topics
debug_print(' * waiting for image_rect topics ... ', end='', debug=debug)
sys.stdout.flush()
while not mct_introspection.image_rect_ready():
time.sleep(ready_poll_dt)
debug_print('done', debug=debug)
# Start frame drop corrector nodes and wait until they are ready
debug_print(' * starting frame drop correctors ... ', end='', debug=debug)
sys.stdout.flush()
frame_drop_corrector_master.start()
while not mct_introspection.frame_drop_correctors_ready():
time.sleep(ready_poll_dt)
debug_print('done', debug=debug)
# Stop camera triggers again to sync frame drop correctors. Note, It
# appears we do need to stop the camera triggers twice during startup.
# Sync'ing the frame drop correctors requires stopping the camera triggers
# resetting the frame drop correctors and then re-starting the triggers.
debug_print(' * stopping camera triggers ... ', end='', debug=debug)
sys.stdout.flush()
camera_trigger.stop()
debug_print('done', debug=debug)
# Reset frame drop correctors
debug_print(' * frame drop corrector sync delay ... ', end='', debug=debug)
time.sleep(buffer_flush_t)
frame_drop_corrector.reset_all()
debug_print('done', debug=debug)
# --------------------------------------------------------------------
# Need to reset time stamp watchdog ... or change how it functions
#
# Delay working on this until we know what modifcations we are making
# to the watchdog.
# --------------------------------------------------------------------
# Restart camera triggers
debug_print(' * starting camera triggers ... ', end='', debug=debug)
sys.stdout.flush()
frame_rates = file_tools.read_frame_rates()
camera_trigger.start(frame_rates['tracking_2d'])
debug_print('done', debug=debug)
# Start frame skipper nodes and wait unti they are ready
debug_print(' * starting frame skippers ... ', end='', debug=debug)
sys.stdout.flush()
frame_skipper_master.start_frame_skippers()
while not mct_introspection.frame_skippers_ready():
time.sleep(ready_poll_dt)
debug_print('done', debug=debug)
# Start image stitcher nodes and wait until stitched image topics ready
debug_print(' * starting image stitchers ... ', end='', debug=debug)
sys.stdout.flush()
image_stitcher_master.start_image_stitchers()
while not mct_introspection.stitched_images_ready():
time.sleep(ready_poll_dt)
debug_print('done', debug=debug)
# Start three point tracker nodes and wait until they are ready.
debug_print(' * starting three point trackers ... ', end='', debug=debug)
sys.stdout.flush()
three_point_tracker_master.start_three_point_trackers()
while not mct_introspection.three_point_trackers_ready():
time.sleep(ready_poll_dt)
while not mct_introspection.three_point_tracker_synchronizers_ready():
time.sleep(ready_poll_dt)
debug_print('done', debug=debug)
# Start stitched image labeler and wait until stitched images are published.
debug_print(' * starting stitched image lablers ... ', end='', debug=debug)
sys.stdout.flush()
stitched_image_labeler_master.start_stitched_image_labelers()
while not mct_introspection.stitched_image_labelers_ready():
time.sleep(ready_poll_dt)
debug_print('done', debug=debug)
# Start avi writer nodes
debug_print(' * starting avi writers ... ', end='', debug=debug)
sys.stdout.flush()
avi_writer_master.start_avi_writers()
debug_print('done', debug=debug)
# Start tracking pts loggers
debug_print(' * starting tracking pts loggers ... ', end='', debug=debug)
sys.stdout.flush()
tracking_pts_logger_master.start_tracking_pts_loggers()
debug_print('done', debug=debug)
# Start mjpeg servers
debug_print(' * starting mjpeg servers ... ', end='', debug=debug)
sys.stdout.flush()
# Set topics for mjpeg servers
mjpeg_topics = []
# Add time stamp watchdog image
mjpeg_topics.append('/image_frame_drop_watchdog')
# Add the stitched and tracking pts images for all regions
for region in regions_dict:
mjpeg_topics.append('/{0}/image_stitched_labeled'.format(region))
mjpeg_topics.append('/{0}/image_tracking_pts'.format(region))
mjpeg_topics.append('/{0}/image_tracking_info'.format(region))
# Add any extra video images
for v in extra_video_dict.values():
mjpeg_topics.append(v)
mjpeg_servers.set_topics(mjpeg_topics)
mjpeg_servers.start_servers()
debug_print('done', debug=debug)
"""
while not rospy.is_shutdown():
with self.lock:
self.process_camera_stamps()
# Utility functions
# ----------------------------------------------------------------------------
def get_camera_from_topic(topic):
camera = topic.split('/')[2]
return camera
def get_machine_from_topic(topic):
machine = topic.split('/')[1]
return machine
# -----------------------------------------------------------------------------
if __name__ == '__main__':
# Temporary get tracking 2d frame rate. When/if we have more tracking modes
# we will probably want to pass this to the node at launch.
frame_rate_dict = file_tools.read_frame_rates()
frame_rate = frame_rate_dict['tracking_2d']
node = TimeStampWatchdog(frame_rate)
node.run()