SHARE
TWEET

Untitled

a guest Jan 12th, 2019 58 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import time
  2. import cozmo
  3. import _thread as thread
  4. import sys
  5. import networking
  6. import schedule
  7. import extended_command
  8. import PIL
  9. from PIL import Image, ImageDraw, ImageFont
  10.  
  11. coz = None
  12. video_port = ""
  13. camera_id = 0
  14. infoServer = None
  15. annotated = False
  16. flipped = 0
  17. colour = False
  18.  
  19. def set_colour(command, args):
  20.     global colour
  21.     if extended_command.is_authed(args['name']) == 2:
  22.         colour = not colour
  23.         coz.camera.color_image_enabled = colour
  24.  
  25.  
  26. def set_annotated(command, args):
  27.     global annotated
  28.     if extended_command.is_authed(args['name']) == 2:
  29.         annotated = not annotated
  30.  
  31. def set_flipped(command, args):
  32.     global flipped
  33.     if extended_command.is_authed(args['name']) == 2:
  34.         flipped = not flipped
  35.  
  36. def setup(robot_config):
  37.     global camera_id
  38.     global infoServer
  39.     global video_port
  40.     global annotated
  41.     global colour
  42.    
  43.     camera_id = robot_config.get('robot', 'camera_id')
  44.     infoServer = robot_config.get('misc', 'info_server')
  45.     video_port = getVideoPort()
  46.     cozmo.setup_basic_logging()
  47.     cozmo.robot.Robot.drive_off_charger_on_connect = False
  48.    
  49.     extended_command.add_command('.annotate', set_annotated)
  50.     extended_command.add_command('.color', set_colour)
  51.     extended_command.add_command('.colour', set_colour)
  52.  
  53.     try:
  54.         thread.start_new_thread(cozmo.connect, (run,))
  55.     except KeyboardInterrupt as e:
  56.         pass        
  57.     except cozmo.ConnectionError as e:
  58.         sys.exit("A connection error occurred: %s" % e)
  59.  
  60.     while not coz:
  61.         try:
  62.            time.sleep(0.5)
  63.            print("not coz")
  64.         except (KeyboardInterrupt, SystemExit):
  65.            sys.exit()
  66.  
  67.     if robot_config.has_section('cozmo'):
  68.         send_online_status = robot_config.getboolean('cozmo', 'send_online_status')
  69.         annotated = robot_config.getboolean('cozmo', 'annotated')
  70.         colour = robot_config.getboolean('cozmo', 'colour')
  71.     else:
  72.         send_online_status = True
  73.    
  74.     if send_online_status:
  75.         print("Enabling online status")
  76.         schedule.repeat_task(10, updateServer);
  77.  
  78. def getVideoPort():
  79.     import robot_util
  80.     import json
  81.     url = 'https://%s/get_video_port/%s' % (infoServer, camera_id)
  82.     response = robot_util.getWithRetry(url)
  83.     return(json.loads(response)['mpeg_stream_port'])
  84.  
  85. # Tell the server we are online
  86. def updateServer():
  87.     print("Updating Server")
  88.     try:
  89.         networking.appServerSocketIO.emit('send_video_status', {'send_video_process_exists': True,
  90.                                'ffmpeg_process_exists': True,
  91.                                'camera_id':camera_id})
  92.     except AttributeError:
  93.         print("Error: No appServerSocketIO");
  94.  
  95. def getCozmo():
  96.     return coz
  97.  
  98. class RobotStateDisplay(cozmo.annotate.Annotator):
  99.     def apply(self, image, scale):
  100.         d = ImageDraw.Draw(image)
  101.  
  102.         bounds = [10, 5, image.width, image.height]
  103.         bounds_shadow = [10, 6, image.width, image.height]
  104.         font = ImageFont.truetype('static/fonts/LiberationSans-Bold.ttf', 15)
  105.         def print_line(text_line):
  106.             shadow = cozmo.annotate.ImageText(text_line, position=cozmo.annotate.TOP_LEFT, color='#000000', font=font)
  107.             shadow.render(d, bounds_shadow)
  108.             text = cozmo.annotate.ImageText(text_line, position=cozmo.annotate.TOP_LEFT, color='#ffffff', font=font)
  109.             text.render(d, bounds)
  110.             TEXT_HEIGHT = 15
  111.             bounds[1] += TEXT_HEIGHT
  112.             bounds_shadow[1] += TEXT_HEIGHT
  113.  
  114.         # Display the Pose info for the robot
  115.  
  116.         pose = coz.pose
  117.         print_line('Pose: Pos = <%.1f, %.1f, %.1f>' % pose.position.x_y_z)
  118.         print_line('Pose: Rot quat = <%.1f, %.1f, %.1f, %.1f>' % pose.rotation.q0_q1_q2_q3)
  119.         print_line('Pose: angle_z = %.1f' % pose.rotation.angle_z.degrees)
  120.         print_line('Pose: origin_id: %s' % pose.origin_id)
  121.  
  122.         # Display the Accelerometer and Gyro data for the robot
  123.  
  124.         print_line('Accelmtr: <%.1f, %.1f, %.1f>' % coz.accelerometer.x_y_z)
  125.         print_line('Gyro: <%.1f, %.1f, %.1f>' % coz.gyro.x_y_z)
  126.  
  127. def run(coz_conn):
  128.     global coz
  129.     coz = coz_conn.wait_for_robot()
  130.     coz.enable_stop_on_cliff(True)
  131.  
  132.     # Turn on image receiving by the camera
  133.     coz.camera.image_stream_enabled = True
  134.  
  135.     coz.say_text( "hey everyone, lets robot!", in_parallel=True)
  136.  
  137.     while True:
  138.         time.sleep(0.25)
  139.  
  140.         from subprocess import Popen, PIPE
  141.         from sys import platform
  142.  
  143.         #Frames to file
  144.         #p = Popen(['ffmpeg', '-y', '-f', 'image2pipe', '-vcodec', 'png', '-r', '25', '-i', '-', '-vcodec', 'mpeg1video', '-qscale', '5', '-r', '25', 'outtest.mpg'], stdin=PIPE)
  145.  
  146.         coz.world.image_annotator.add_annotator('robotState', RobotStateDisplay)
  147.         if platform.startswith('linux') or platform == "darwin":
  148.             #MacOS/Linux
  149.             p = Popen(['/usr/local/bin/ffmpeg', '-y', '-f', 'image2pipe', '-vcodec', 'png', '-r', '25', '-i', '-', '-vcodec', 'mpeg1video', '-r', '25', "-f","mpegts","http://letsrobot.tv:"+str(video_port)+"/hello/320/240/"], stdin=PIPE)
  150.         elif platform.startswith('win'):
  151.             #Windows
  152.             import os
  153.             if not os.path.isfile('c:/ffmpeg/bin/ffmpeg.exe'):
  154.                print("Error: cannot find c:\\ffmpeg\\bin\\ffmpeg.exe check ffmpeg is installed. Terminating controller")
  155.                thread.interrupt_main()
  156.                thread.exit()
  157.  
  158.             p = Popen(['c:/ffmpeg/bin/ffmpeg.exe', '-nostats', '-y', '-f', 'image2pipe', '-vcodec', 'png', '-r', '25', '-i', '-', '-vcodec', 'mpeg1video', '-r', '25','-b:v', '400k', "-f","mpegts","http://letsrobot.tv:"+str(video_port)+"/hello/320/240/"], stdin=PIPE)
  159.        
  160.         try:
  161.             while True:
  162.                 if coz:
  163.                     image = coz.world.latest_image
  164.                     if image:
  165.                         if annotated:
  166.                             image = image.annotate_image()
  167.                         else:
  168.                             image = image.raw_image
  169.                         image.save(p.stdin, 'PNG')
  170.                 else:
  171.                     time.sleep(.1)
  172.             p.stdin.close()
  173.             p.wait()
  174.         except cozmo.exceptions.SDKShutdown:
  175.             p.stdin.close()
  176.             p.wait()
  177.             pass              
  178.  
  179. def say(*args):
  180.     global coz
  181.     message = args[0]
  182.  
  183.     try:
  184.         coz.say_text(message, duration_scalar=0.75)
  185.     except cozmo.exceptions.RobotBusy:
  186.         return False
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top