pr2_interactive_manipulation_frontend: adjust_rviz

Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import sys
00004 import yaml
00005 #from stgit.git import switch
00006 
00007 def print_usage():
00008     print "usage:\nadjust_rviz_config.py in_file out_file stereo nav_enabled nav_local sim\n(where stereo, nav_enabled, nav_local, and sim are true or false)\n"
00009 
00010 def str2bool(name,v):
00011     if v not in ['true', 'false']:
00012         print name + "was not true or false!"
00013         print_usage()
00014         sys.exit(1)
00015         
00016     vb = v.lower() == 'true'
00017     print name, " = ", vb
00018     return vb
00019   
00020 
00021 if len(sys.argv) < 7:
00022     print_usage()
00023     sys.exit(1)
00024 
00025 # parse cmdline params
00026 
00027 stereo = str2bool( 'stereo', sys.argv[3] )
00028 nav_enabled = str2bool( 'nav_enabled', sys.argv[4] )
00029 nav_local = str2bool( 'nav_local', sys.argv[5] )
00030 sim = str2bool( 'sim', sys.argv[6] )
00031 track_people = str2bool( 'track_people', sys.argv[7] )
00032 
00033 # set derived params
00034 
00035 if nav_enabled and not nav_local:
00036     fixed_frame = '/map'
00037     map_enabled = True
00038 else:
00039     fixed_frame = '/odom_combined'
00040     map_enabled = False
00041     
00042 if sim:
00043     kinect_color_topic = '/head_mount_kinect/rgb/image_raw'
00044     kinect_color_hint = 'raw'
00045     kinect_depth_topic = '/head_mount_kinect_rgb/depth/image_raw'
00046     kinect_depth_hint = 'raw'
00047 else:
00048     kinect_color_topic = '/head_mount_kinect/rgb/image_color'
00049     kinect_color_hint ='compressed'
00050     kinect_depth_topic = '/head_mount_kinect/depth_registered/image'
00051     kinect_depth_hint = 'compressedDepth'
00052 
00053 # read yaml and modify properties 
00054 
00055 in_stream = open(sys.argv[1], 'r')
00056 rviz_yaml = yaml.load(in_stream)
00057 
00058 rviz_yaml['Visualization Manager']['Global Options']['Fixed Frame'] = fixed_frame
00059 
00060 for display in rviz_yaml['Visualization Manager']['Displays']:
00061     
00062     if display['Name'] == 'People Tracking':
00063         display['Enabled'] = track_people
00064 
00065     if display['Name'] == 'Navigation':
00066         for nav_display in display['Displays']:
00067 
00068             if nav_display['Name'] == 'Map':
00069                 nav_display['Enabled'] = map_enabled
00070 
00071             if nav_display['Name'] in ['Nav Global Path','Nav Obstacles']:
00072                 nav_display['Enabled'] = nav_enabled
00073                 if nav_local:
00074                     nav_display['Topic'] = nav_display['Topic'].replace('move_base_node','move_base_local_node')
00075 
00076     if display['Name'] == 'Kinect Stream':
00077         display['Color Image Topic'] = kinect_color_topic
00078         display['Color Transport Hint'] = kinect_color_hint
00079         display['Depth Map Topic'] = kinect_depth_topic
00080         display['Depth Map Transport Hint'] = kinect_depth_hint
00081 
00082     if display['Name'] == 'Head Camera' and not stereo:
00083         display['Image Topic'] = kinect_color_topic
00084         display['Transport Hint'] = kinect_color_hint
00085 
00086 # dump yaml to output file
00087 
00088 out_stream = open(sys.argv[2], 'w')
00089 yaml.dump(rviz_yaml, out_stream)