tz = 0
running = True
while robot.step(timestep) != -1:
t = translation.getSFVec3f()
if running:
percent = 1 - abs(0.25 - t[2]) / 0.25
if percent < 0:
percent = 0
if t[2] > 0.01 and abs(
tz - t[2]
) < 0.0001: # away from starting position and not moving any more
message = "stop"
running = False
else:
message = "percent"
message += ":" + str(percent)
robot.wwiSendText(message)
tz = t[2]
else: # wait for record message
message = robot.wwiReceiveText()
if message:
if message.startswith("record:"):
record = robotbenchmarkRecord(message, "robot_programming",
percent)
robot.wwiSendText(record)
break
elif message == "exit":
break
robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
# Detect status of inverted pendulum
position = positionField.getSFFloat()
if position < -1.58 or position > 1.58:
# stop
run = False
robot.wwiSendText("stop")
else:
if forceStep <= 0:
forceStep = 800 + random.randint(0, 400)
force = force + 0.02
toSend = "%.2lf %d" % (force, seed)
if sys.version_info.major > 2:
toSend = bytes(toSend, "utf-8")
emitter.send(toSend)
else:
forceStep = forceStep - 1
else:
# wait for record message
message = robot.wwiReceiveText()
if message:
if message.startswith("record:"):
record = robotbenchmarkRecord(message, "inverted_pendulum",
time)
robot.wwiSendText(record)
break
elif message == "exit":
break
robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
running = True
stopMessageSent = False
while robot.step(timestep) != -1:
if running:
time = robot.getTime()
# If some of the maze blocks have been moved immediately terminate the benchmark
for i in range(0, mazeBlocksListCount):
item = mazeBlocksList[i]
if isPositionChanged(item['initialPosition'],
item['node'].getPosition()):
time = 60
robot.wwiSendText("time:%-24.3f" % time)
break
if time < 60 and not isMazeEndReached(thymio2.getPosition()):
robot.wwiSendText("time:%-24.3f" % time)
else:
running = False
timestep = int(timestep / 4)
else: # Wait for record message.
if not stopMessageSent:
robot.wwiSendText("stop")
stopMessageSent = True
else:
message = robot.wwiReceiveText()
if message and message.startswith("record:"):
record = robotbenchmarkRecord(message, "maze_runner", -time)
robot.wwiSendText(record)
break
enable_sensors_visualization(supervisor, False)
elif message.endswith("true"):
enable_sensors_visualization(supervisor, True)
supervisor.wwiSendText("stop")
if inEmergencyLane:
supervisor.setLabel(0,
"The vehicle should not enter in the emergency lane.",
0.01, 0.85, 0.09, 0xFF0000, 0, "Lucida Console")
elif collided:
supervisor.setLabel(0, "The vehicle should not collide.", 0.01, 0.85, 0.09,
0xFF0000, 0, "Lucida Console")
elif sumoFailure:
supervisor.setLabel(
0,
"Problem with traffic generation using SUMO, we can't take this performance into account.",
0.01, 0.85, 0.09, 0xFF0000, 0, "Lucida Console")
position = vehicleNode.getPosition()
distance = round(
roadPath.project(Point((position[0], position[2]))) - initialDistance, 3)
while supervisor.step(timestep) != -1:
# wait for record message
message = supervisor.wwiReceiveText()
if message and message.startswith("record:"):
record = robotbenchmarkRecord(message, "highway_driving", distance)
supervisor.wwiSendText(record)
break
if i < cubeObstaclesCount:
randomlyPlaceObject(cubeObstacles[i])
i += 1
if running:
time = robot.getTime()
# If the robot has collided with something that isn't the ground or has
# reached the goal or has even run out of time, record final time and
# terminate simulation.
numberofContactPoints = thymio2.getNumberOfContactPoints()
for x in range(0, numberofContactPoints):
contactPoint = thymio2.getContactPoint(x)
if contactPoint[1] > 0.02 or thymio2.getPosition(
)[0] < -3.3 or time >= 80:
if contactPoint[1] > 0.02:
time = 80
running = False
break
robot.wwiSendText("time:%-24.3f" % time)
else: # Wait for record message.
if not stopMessageSent:
robot.wwiSendText("stop")
stopMessageSent = True
else:
message = robot.wwiReceiveText()
if message and message.startswith("record:"):
record = robotbenchmarkRecord(message, "obstacle_avoidance",
-time)
robot.wwiSendText(record)
break
distance = round(math.sqrt(math.pow(targetPosition[0] - position[0], 2) +
math.pow(targetPosition[2] - position[2], 2)), 4)
time = robot.getTime()
robot.wwiSendText("update: " + str(time) + " {0:.4f}".format(distance))
if boxPicked and distance < 0.036 and position[1] < 0.156:
if distance == previousDistance:
notMovingStepCount += 1
if notMovingStepCount > 10:
break
else:
notMovingStepCount = 0
previousDistance = distance
elif not boxPicked and position[1] > 0.21:
boxPicked = True
robot.wwiSendText("stop")
while robot.step(timestep) != -1:
# wait for record message
message = robot.wwiReceiveText()
if message:
if message.startswith("record:"):
record = robotbenchmarkRecord(message, "pick_and_place", -time)
robot.wwiSendText(record)
break
elif message == "exit":
break
robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
hitsCount += target.hit(robotHead.getPosition(), [-R[2], -R[5], -R[8]],
hitError)
stepsCount += 1
robot.wwiSendText("hits:%d/%d" % (hitsCount, stepsCount))
# Update target object position:
# return if the whole trajectory has been completed.
isRunning = target.move(timestep)
# Compute final grade and exit
if stepsCount == 0:
hitRate = 0.0
else:
hitRate = float(hitsCount) / stepsCount
robot.wwiSendText("stop")
# Wait for record message.
timestep = int(robot.getBasicTimeStep())
while robot.step(timestep) != -1:
message = robot.wwiReceiveText()
if message:
if message.startswith("record:"):
record = robotbenchmarkRecord(message, "visual_tracking", hitRate)
robot.wwiSendText(record)
break
elif message == "exit":
break
robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
# Main loop starts here.
while (supervisor.step(timestep) != -1 and not metric.isBenchmarkOver()):
# Recovers current time and position/orientation of the robot.
pos = pioneer.getPosition()
pos2d = [pos[0], pos[2]]
orientation = pioneer.getOrientation()
time = supervisor.getTime()
metric.update(pos2d, orientation, time)
if ALLOW_LABELS:
metric.updateLabels(0x0000ff, supervisor, time)
supervisor.wwiSendText('update:' + metric.getWebMetricUpdate())
for pointMessage in metric.getWebNewPoints():
supervisor.wwiSendText(pointMessage)
supervisor.wwiSendText('stop')
# Wait for credentials sent by the robot window.
while supervisor.step(timestep) != -1:
message = supervisor.wwiReceiveText()
if message and message.startswith("record:"):
performance = metric.getPerformance()
record = robotbenchmarkRecord(message, "square_path", performance)
supervisor.wwiSendText(record)
break
