class TransformationFetcher: def __init__(self, broker_uri=None): self.channel = Channel() if broker_uri is None else Channel(broker_uri) self.subscription = Subscription(self.channel) self.transformations = {} def get_transformation(self, _from, _to): if _from in self.transformations and _to in self.transformations[_from]: return self.transformations[_from][_to] if self._request_transformation(_from, _to): return self.transformations[_from][_to] return None def _request_transformation(self, _from, _to): topic = 'FrameTransformation.{}.{}'.format(_from, _to) self.subscription.subscribe(topic) try: msg = self.channel.consume(timeout=5.0) self.subscription.unsubscribe(topic) except: self.subscription.unsubscribe(topic) return False transformation = msg.unpack(FrameTransformation) if _from not in self.transformations: self.transformations[_from] = {} self.transformations[_from][_to] = to_np(transformation.tf) return True
def navigate(goalX, goalY, robotArUco, worldFrame, mapFile, step, robot_size, N_KNN, MAX_EDGE_LEN, show_path): exporter = ZipkinExporter( service_name="Robot.Controller", host_name="10.10.2.13", port="30200", transport=AsyncTransport, ) # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.13:30000") # Create a subscription subscription = Subscription(channel) # Subscribe to the following topics: # - To get the robot's current position in relation where it was turned on, i.e., dead reckoning odometry # - To get the position of the ArUco marker attached to the robot's back #robotArUco = 8 # ArUco marker ID attached to the robot's back #worldFrame = 1000 # Number of the world reference frame in the HPN Intelligent Space awarenessOff(channel) time.sleep(6) # to wait some time to be sure the awareness if off topicGetRobotOdometry = "FrameTransformation.2000.2001" topicGetArUcoRobotBack = "FrameTransformation." + str( robotArUco + 100) + "." + str(worldFrame) subscription.subscribe(topicGetRobotOdometry) subscription.subscribe(topicGetArUcoRobotBack) # Initialise transformation matrices used for storing odometry and correction pepperPose = np.identity(4) robotOriginToWorld = np.identity(4) lastOdometry = np.identity(4) # Load the frame transformation between the ArUco marker on the robot's back and the robot's base frame. fileName = "frameArUcoRobot.dat" arUcoToRobot = read_frameTransformation(fileName) robotToArUco = inv(arUcoToRobot) print("Goal: ", goalX, " ", goalY) # Localize the robot before start path planning robotLocalized = False notSeen = 0 count = 0 while not robotLocalized: # Source must be the current position of the robot in the world frame message = channel.consume() if (message.topic == topicGetRobotOdometry): print("reading odometry") # get the transformation matrix corresponding to the current rotation and position of the robot lastOdometry = unpackFrameTransformation(message) notSeen = notSeen + 1 print(notSeen) if (message.topic == topicGetArUcoRobotBack): print("correcting odometry") # get the frame transformation betweeb the ArUco marker on the robot's back and the world arUcoToWorld = unpackFrameTransformation(message) # calculates the robot pose in the world frame pepperPose = arUcoToWorld * robotToArUco # calculate the frame transformation needed to correct robot's odometry while the ArUco marker is not seen by the intelligent space robotOriginToWorld = pepperPose * inv(lastOdometry) sourceX = pepperPose[0, 3] sourceY = pepperPose[1, 3] print("x= ", sourceX, " y= ", sourceY) robotLocalized = True notSeen = 0 if notSeen > 30: notSeen = 0 count = count + 1 # unsubscribe to not accumulate messages subscription.unsubscribe(topicGetRobotOdometry) subscription.unsubscribe(topicGetArUcoRobotBack) if count > 4: print("I can't get localized by the Intelligent Space.") print( "Please take me to a spot where the marker on my back can be seen by one of the cameras." ) sys.exit(0) makeTurn(0, 0.3, channel) subscription.subscribe(topicGetRobotOdometry) subscription.subscribe(topicGetArUcoRobotBack) # unsubscribe to not accumulate messages subscription.unsubscribe(topicGetRobotOdometry) subscription.unsubscribe(topicGetArUcoRobotBack) #sys.exit(0) # Create obstacles ox, oy = create_virtualObstacles() # Call path planning # rx, ry contains the positions in the path rx, ry = PRM_planning(mapFile, sourceX, sourceY, goalX, goalY, ox, oy, robot_size, step, N_KNN, MAX_EDGE_LEN) # Check if a path was found/home/raquel/ProgrammingIS/learningISBristol// #assert len(rx) != 0, 'Cannot found path' if len(rx) == 0: print('Cannot find path') raise SystemError #sys.exit(0) # Plot map points and obstacles if show_path: map_x, map_y = read_map(mapFile, step) plt.plot(ox, oy, ".k") plt.plot(sourceX, sourceY, "^r") plt.plot(goalX, goalY, "^c") plt.plot(map_x, map_y, ".b") plt.grid(True) plt.axis("equal") plt.plot(rx, ry, "-r") plt.plot(rx, ry, "og") plt.show() # Reverse the order of the path (list) returned by the path-planning algorithm # The original list contains the goal at the beginning and the source at the end. We need the reverse rx = rx[::-1] ry = ry[::-1] print(rx) print(ry) #sys.exit(0) # Subscribe to the previous topics subscription.subscribe(topicGetRobotOdometry) subscription.subscribe(topicGetArUcoRobotBack) i = 0 k = 0 stuck = 0 dist = 100.0 threshold = 0.2 try: while True: try: # listen the channelprint(frameArUcoRobot) message = channel.consume(timeout=0.5) spanr = message.extract_tracing() tracer = Tracer(exporter, spanr) tracer.start_span(name="Navigate") # Check if the message received is the robot's odometry - FrameTransformation type if (message.topic == topicGetRobotOdometry): # get the transformation matrix corresponding to the current rotation and position of the robot lastOdometry = unpackFrameTransformation(message) pepperPose = robotOriginToWorld * lastOdometry #print(pepperPose) elif (message.topic == topicGetArUcoRobotBack): print("Odometry Corrected") # get the transformation matrix corresponding to the current pose of the robot corrected when # it sees an ArUco marker arUcoToWorld = unpackFrameTransformation(message) pepperPose = arUcoToWorld * robotToArUco robotOriginToWorld = pepperPose * inv(lastOdometry) except socket.timeout: print("Time out") # matrix Inverse toPepperFrame = inv(pepperPose) # transform the goal from the world frame to the robot frame posX, posY, posZ = changeRefFrame(rx[i], ry[i], 0, toPepperFrame) distPrevious = dist dist = math.sqrt(posX**2 + posY**2) #print(dist) # If distance to current goal is less than the threshold, pick the next point in the path to be the next goal if dist < threshold: i = i + 1 stuck = 0 print(dist) print("Path index: ", i, " of ", len(rx)) if i == (len(rx) - 1): threshold = 0.5 # If that was the last point in the path, finish navigation. Goal achieved. if i >= len(rx): print("Goal achieved") break # If not the last point in the path, get the next point and converte it to robot frame print("Next point in the path") posX, posY, posZ = changeRefFrame(rx[i], ry[i], 0, toPepperFrame) # Command robot to move commandMoveTo(posX, posY, channel) #commandNavigateTo(posX,posY,channel) # If distance to current goal is greater than the threshold, check if robot is stopped # To check if robot is stopped, calculate the difference between current distance and previous one elif abs(dist - distPrevious ) < 0.005: # if difference is less than 0.5 cm k = k + 1 # accumulate if k == 20: #see if situation remains for 20 times # Robot is stuck. Let's send a move command again print(dist) k = 0 print( "Ooops.... I got stuck.... I will try to move. Stuck = ", stuck) posX, posY, posZ = changeRefFrame(rx[i], ry[i], 0, toPepperFrame) stuck = stuck + 1 if stuck == 4: goBack(-10, 0, channel) stuck = 0 posX, posY, posZ = changeRefFrame( rx[i], ry[i], 0, toPepperFrame) # Command robot to move commandMoveTo(posX, posY, channel) else: commandMoveTo(posX, posY, channel) tracer.end_span() except KeyboardInterrupt: commandMoveTo(0, 0, channel) #awarenessOff(channel) awarenessOn(channel) time.sleep(6)
def call_via_aruco(arucoID): # parameters for navigation mapFile = "../lesson09_mapping_with_ArUco/map3011.dat" robotArUco = 8 worldFrame = 1000 step = 2 robotRadius = .8 N_KNN = 40 # number of edge from one sampled point MAX_EDGE_LEN = 2.5 # [m] Maximum edge length show_path = False # Create a channel to connect to the broker channel = Channel("amqp://10.10.2.23:30000") # Create a subscription subscription = Subscription(channel) # Subscribe to the following topic: # - To get the position of the ArUco marker used as a calling signal for the robot topicGetArUcoLocation = "FrameTransformation."+str(arucoID+100)+"."+str(worldFrame) subscription.subscribe(topicGetArUcoLocation) # Localise the marker for calling the robot markerLocalized = False notSeen = 0 count = 0 while not markerLocalized: # Source must be the current position of the robot in the world frame message = channel.consume() notSeen = notSeen + 1 if (message.topic == topicGetArUcoLocation): print("Found ArUco") # get the frame transformation betweeb the ArUco marker on the robot's back and the world arUcoToWorld = unpackFrameTransformation (message) goalX = arUcoToWorld[0,3] goalY = arUcoToWorld[1,3] print("x= ",goalX," y= ",goalY) markerLocalized = True notSeen = 0 if notSeen > 30: notSeen = 0 count = count + 1 print("Try to turn the marker or show to other camera.") if count > 4: print("I can't localize the marker in the Intelligent Space.") sys.exit(0) # unsubscribe to not accumulate messages subscription.unsubscribe(topicGetArUcoLocation) # call the robot navigate(goalX,goalY,robotArUco, worldFrame, mapFile,step,robotRadius,N_KNN,MAX_EDGE_LEN,show_path)