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)