PYTHON   48

TurnToClosestCube

Guest on 2nd August 2022 12:46:15 AM

  1. from cozmo_fsm import *
  2. from math import *
  3.  
  4. #Runs the fsm to look at each cub and then turn towards the closest one
  5. class TurnToClosestCube(StateMachineProgram):
  6.     $setup{
  7.         #Turns Cozmo 360-degrees so he is able to get the position of all cubes
  8.         #He needs to move very slowly in order to consistently see every cube
  9.         scanForCubes: DriveTurn(360, 5)
  10.         scanForCubes =C=> getClosest
  11.         getClosest: FindClosestCube() =D=> turnTo
  12.         #Goes back to StateNode at the end to prevent issues upon relaunching
  13.         turnTo: TurnToFace() =C=> StateNode()
  14.     }
  15.  
  16. #Takes the position of each cube into account and posts the closest one
  17. class FindClosestCube(StateNode):
  18.     def start(self, event=None):
  19.         if self.running:return
  20.         super().start(event)
  21.        
  22.         #Finds the distance to cube one
  23.         x = cube1.pose.position.y- robot.pose.position.y
  24.         y = cube1.pose.position.x- robot.pose.position.x
  25.         hyp1 = sqrt(x**2 + y**2)
  26.         #Sets to an arbitrary (far) distance if the cube has been moved
  27.         if cube1.pose.origin_id != robot.pose.origin_id:
  28.                 hyp1 = 1*10**9
  29.  
  30.         #Finds the distance to cube 2
  31.         x = cube2.pose.position.y- robot.pose.position.y
  32.         y = cube2.pose.position.x- robot.pose.position.x
  33.         hyp2 = sqrt(x**2 + y**2)
  34.         #Sets to an arbitrary (far) distance if the cube has been moved
  35.         if cube2.pose.origin_id != robot.pose.origin_id:
  36.                 hyp2 = 1*10**9
  37.                
  38.         #Finds the distance to cube 3
  39.         x = cube3.pose.position.y- robot.pose.position.y
  40.         y = cube3.pose.position.x- robot.pose.position.x
  41.         hyp3 = sqrt(x**2 + y**2)
  42.         #Sets to an arbitrary (far) distance if the cube has been moved
  43.         if cube3.pose.origin_id != robot.pose.origin_id:
  44.                 hyp3 = 1*10**9
  45.  
  46.         #sets closest to the closest cube (or 0 if all of the cubes are...)
  47.         if hyp1<hyp2 and hyp1<hyp3:
  48.             closest = 1
  49.         elif hyp2<hyp1 and hyp2<hyp3:
  50.             closest = 2
  51.         elif hyp3<hyp1 and hyp3<hyp2:
  52.             closest=3
  53.         elif hyp1==hyp2 and hyp2==hyp3 and hyp1==1*10**9:
  54.             closest=0
  55.         else:
  56.             closest=2
  57.  
  58.         self.post_data(closest)
  59.  
  60. #Turns Cozmo to face the closest cube
  61. class TurnToFace(Turn):
  62.     # try it with only one of the super().start(event)
  63.     def start(self, event=None):
  64.         if self.running:return
  65.  
  66.         #Finds the anflr that cozmo needs to turn in order to face the closest cube
  67.         if isinstance(event, DataEvent):
  68.             closest = event.data
  69.             if closest ==1:
  70.                 x = cube1.pose.position.y- robot.pose.position.y
  71.                 y = cube1.pose.position.x- robot.pose.position.x
  72.                 angle = atan2(x,y)-robot.pose.rotation.angle_z.radians
  73.             elif closest ==2:
  74.                 x = cube2.pose.position.y- robot.pose.position.y
  75.                 y = cube2.pose.position.x- robot.pose.position.x
  76.                 angle = atan2(x,y)-robot.pose.rotation.angle_z.radians
  77.             elif closest==3:
  78.                 x = cube3.pose.position.y- robot.pose.position.y
  79.                 y = cube3.pose.position.x- robot.pose.position.x
  80.                 angle = atan2(x,y)-robot.pose.rotation.angle_z.radians
  81.             else:
  82.                 angle=0
  83.  
  84.             #Turns cozmo to the desired angle
  85.             self.angle = Angle(angle)
  86.            
  87.             super().start(event)

Raw Paste


Login or Register to edit or fork this paste. It's free.