def move_robot(motion):
"""
Move robot with various speed (minimal, nominal and maximal)
"""
parameters_direction = tools.read_section("config.cfg",
"DirectionParameters")
distance = int(parameters_direction["distance"][0])
parameters_speed = tools.read_section("config.cfg", "Speed")
min = int(parameters_speed["min_speed"][0])
nom = int(parameters_speed["nom_speed"][0])
max = int(parameters_speed["max_speed"][0])
question_continue('start')
if min == 1:
print 'minimal velocity'
motion.moveToward(0.05, 0.0, 0.0)
time.sleep(6.0)
motion.stopMove()
if nom == 1:
question_continue('nominal velocity')
print 'nomimal velocity'
motion.moveToward(0.3, 0.0, 0.0)
time.sleep(6.0)
motion.stopMove()
if max == 1:
question_continue('maximal velocity')
print 'maximal velocity'
motion.moveToward(1.0, 0.0, 0.0)
time.sleep(5.0)
motion.stopMove()
motion.rest()
def check_wheel_temperatures(dcm, leds, list_wheel_temperature,
list_wheel_speed, list_wheel_stiffness):
"""
Pauses the robot if the wheels are too hot
"""
parameters_cables = tools.read_section("config.cfg",
"CablesRoutingParameters")
Temp_max = int(parameters_cables["Temp_max"][0])
Temp_min = int(parameters_cables["Temp_min"][0])
if list_wheel_temperature[0].value >= Temp_max or \
list_wheel_temperature[1].value >= Temp_max or \
list_wheel_temperature[2].value >= Temp_max:
print("\nWheels too hot --> Waiting")
leds.fadeListRGB("shoulder_group", [
0x00FF00FF,
], [
0.,
])
stop_robot(list_wheel_speed)
unstiff_wheels(list_wheel_stiffness)
# Wait
while list_wheel_temperature[0].value >= Temp_min or \
list_wheel_temperature[1].value >= Temp_min or \
list_wheel_temperature[2].value >= Temp_min:
tools.wait(dcm, 20000)
leds.reset("shoulder_group")
stiff_wheels(list_wheel_stiffness)
def zero_pos():
"""
Fixture which retrieves zero joints position from a configuration file.
"""
return tools.read_section(
PATH + "/global_test_configuration/"
"juliette_positions.cfg", "zero")
def run(self):
"""Log cables crossing"""
gyro_x = subdevice.InertialSensorBase(self.dcm, self.mem, "GyroscopeX")
gyro_y = subdevice.InertialSensorBase(self.dcm, self.mem, "GyroscopeY")
parameters = tools.read_section("config.cfg",
"CablesRoutingParameters")
# If file already exists, reading to know the previous state
if os.path.exists(parameters["Log_file_name"][0]):
data = open(parameters["Log_file_name"][0], 'r')
csv_reader = csv.reader(data)
variables_list = []
row_num = 0
for row in csv_reader:
data_num = 0
if row_num == 0:
for value in row:
variables_list.append([value])
else:
for value in row:
variables_list[data_num].append(value)
data_num += 1
row_num += 1
self._cables_crossing = int(variables_list[0][row_num - 1])
previous_time = float(variables_list[1][row_num - 1])
data.close()
data = open(parameters["Log_file_name"][0], 'a')
# Else initialize to 0
else:
self._cables_crossing = 0
previous_time = 0.
data = open(parameters["Log_file_name"][0], 'w')
data.write("Cable Crossing,Time\n0,0\n")
data.flush()
time_init = time.time()
while not self._stop.is_set():
if gyro_x.value < \
float(parameters["Minimum_CableDetection"][0]) or \
gyro_x.value > \
float(parameters["Maximum_CableDetection"][0]) or\
gyro_y.value < \
float(parameters["Minimum_CableDetection"][0]) or \
gyro_y.value > \
float(parameters["Maximum_CableDetection"][0]):
self._cables_crossing += 1
data.write(str(self._cables_crossing) + "," +\
str(time.time() - time_init + previous_time) + "\n")
print("\nnb of cables crossing: " + str(self._cables_crossing))
data.flush()
time.sleep(02)
time.sleep(0.01)
def run(self):
bumper_right = subdevice.Bumper(self.dcm, self.mem, "FrontRight")
bumper_left = subdevice.Bumper(self.dcm, self.mem, "FrontLeft")
bumper_back = subdevice.Bumper(self.dcm, self.mem, "Back")
list_bumpers = [bumper_right, bumper_left, bumper_back]
parameters = tools.read_section("config.cfg",
"BumpersActivationsParameters")
# If file already exists, reading to know the previous state
if os.path.exists(parameters["Log_file_name"][0]):
data = open(parameters["Log_file_name"][0], 'r')
csv_reader = csv.reader(data)
variables_list = []
row_num = 0
for row in csv_reader:
data_num = 0
if row_num == 0:
for value in row:
variables_list.append([value])
else:
for value in row:
variables_list[data_num].append(value)
data_num += 1
row_num += 1
self._bumpers_activations = int(variables_list[0][row_num - 1])
previous_time = float(variables_list[1][row_num - 1])
data.close()
data = open(parameters["Log_file_name"][0], 'a')
# Else initialize to 0
else:
self._bumpers_activations = 0
previous_time = 0.
data = open(parameters["Log_file_name"][0], 'w')
data.write("Bumpers activations,Time\n0,0\n")
data.flush()
time_init = time.time()
# Loop
while not self._stop.is_set():
for bumper in list_bumpers:
if bumper.value == 1:
self._bumpers_activations += 1
data.write(str(self._bumpers_activations) + "," + \
str(time.time() - time_init + previous_time) + "\n")
print("nb of bumpers activations: " +
str(self._bumpers_activations) + "\n")
data.flush()
tools.wait(self.dcm, 2 * self.wait_time_bumpers)
def move_robot(motion):
parameters_rotation = tools.read_section("config.cfg", "Rotation")
nb_tour = int(parameters_rotation['nb_tour'][0])
nb_test = int(parameters_rotation['nb_test'][0])
question_continue('start')
for test in range(0, nb_test):
motion.moveTo(0, 0, math.pi * (nb_tour * 2))
question_continue('reverse')
motion.moveTo(0, 0, -math.pi * (nb_tour * 2))
test += 1
question_continue('restart')
motion.rest()
def move_robot(motion, mem):
"""
Move Robot and stop when a fall id detected
"""
parameters_slope = tools.read_section("config.cfg", "Stop_Slope")
distance = float(parameters_slope["distance"][0])
dir_x = int(parameters_slope["dir_X"][0])
dir_y = int(parameters_slope["dir_Y"][0])
dir_diag_l = int(parameters_slope["dir_diagL"][0])
dir_diag_r = int(parameters_slope["dir_diagR"][0])
stiffness = int(parameters_slope["stiff_ON"][0])
wait = int(parameters_slope["wait_time"][0])
question_continue('start')
if dir_x == 1:
question_continue('forward')
motion.moveTo(distance, 0, 0)
slope_detection(motion, stiffness, mem, wait)
question_continue('behind')
motion.moveTo(-distance, 0, 0)
slope_detection(motion, stiffness, mem, wait)
if dir_y == 1:
question_continue('left')
motion.moveTo(0, distance, 0)
slope_detection(motion, stiffness, mem, wait)
question_continue('rigth')
motion.moveTo(0, -distance, 0)
slope_detection(motion, stiffness, mem, wait)
if dir_diag_l == 1:
question_continue('diagonale front left')
motion.moveTo(distance, distance, 0)
slope_detection(motion, stiffness, mem, wait)
question_continue('diagonale back left')
motion.moveTo(-distance, -distance, 0)
slope_detection(motion, stiffness, mem, wait)
if dir_diag_r == 1:
question_continue('diagonale front right')
motion.moveTo(distance, -distance, 0)
slope_detection(motion, stiffness, mem, wait)
question_continue('diagonale back right')
motion.moveTo(-distance, distance, 0)
slope_detection(motion, stiffness, mem, wait)
motion.rest()
def move_robot(motion):
"""
Move robot on abstacles:- rubber band
- string
- clips
- news paper
- dust
"""
parameters_direction = tools.read_section("config.cfg", "Obstacle")
distance = float(parameters_direction["distance"][0])
nb_passage = int(parameters_direction["nb_passage"][0])
dir_x = int(parameters_direction["dir_X"][0])
dir_y = int(parameters_direction["dir_Y"][0])
dir_diag_l = int(parameters_direction["dir_diagL"][0])
dir_diag_r = int(parameters_direction["dir_diagR"][0])
rotate = int(parameters_direction["rotate"][0])
question_continue('start')
if rotate == 1:
print 'rotate'
motion.moveTo(0, 0, math.pi * 2)
if dir_x == 1:
question_continue('forward')
motion.moveTo(distance, 0, 0)
question_continue('behind')
motion.moveTo(-distance, 0, 0)
if dir_y == 1:
question_continue('left')
motion.moveTo(0, distance, 0)
question_continue('rigth')
motion.moveTo(0, -distance, 0)
if dir_diag_l == 1:
question_continue('diagonale front left')
motion.moveTo(distance, distance, 0)
question_continue('diagonale back left')
motion.moveTo(-distance, -distance, 0)
if dir_diag_r == 1:
question_continue('diagonale front right')
motion.moveTo(distance, -distance, 0)
question_continue('diagonale back right')
motion.moveTo(-distance, distance, 0)
motion.rest()
def move_robot(motion):
"""
Move robot on slope without falling
"""
parameters_direction = tools.read_section("config.cfg", "Slope")
distance = float(parameters_direction["distance"][0])
nb_passage = int(parameters_direction["nb_passage"][0])
dir_x = int(parameters_direction["dir_X"][0])
dir_y = int(parameters_direction["dir_Y"][0])
dir_diag_l = int(parameters_direction["dir_diagL"][0])
dir_diag_r = int(parameters_direction["dir_diagR"][0])
question_continue('start')
if dir_x == 1:
question_continue('forward')
motion.moveTo(distance, 0, 0)
question_continue('behind')
motion.moveTo(-distance, 0, 0)
if dir_y == 1:
question_continue('left')
motion.moveTo(0, distance, 0)
question_continue('rigth')
motion.moveTo(0, -distance, 0)
if dir_diag_l == 1:
question_continue('diagonale front left ')
motion.moveTo(distance, distance, 0)
question_continue('diagonale back left')
motion.moveTo(-distance, -distance, 0)
if dir_diag_r == 1:
question_continue('diagonale front right')
motion.moveTo(distance, -distance, 0)
question_continue('diagonale back right')
motion.moveTo(-distance, distance, 0)
motion.rest()
def test_move_random(dcm, mem, leds, expressiveness, wait_time,
wait_time_bumpers, min_fraction, max_fraction, max_random,
stop_robot, wake_up_pos_brakes_closed, stiff_robot_wheels,
unstiff_joints, log_wheels_speed, log_bumper_pressions):
#---------------------------------------------------------#
#-------------------- Object creations -------------------#
#---------------------------------------------------------#
#------------------------- Wheels ------------------------#
list_wheel_speed_actuator = [
subdevice.WheelSpeedActuator(dcm, mem, "WheelFR"),
subdevice.WheelSpeedActuator(dcm, mem, "WheelFL"),
subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
]
list_wheel_speed_sensor = [
subdevice.WheelSpeedSensor(dcm, mem, "WheelFR"),
subdevice.WheelSpeedSensor(dcm, mem, "WheelFL"),
subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
]
list_wheel_stiffness_actuator = [
subdevice.WheelStiffnessActuator(dcm, mem, "WheelFR"),
subdevice.WheelStiffnessActuator(dcm, mem, "WheelFL"),
subdevice.WheelStiffnessActuator(dcm, mem, "WheelB")
]
list_wheel_temperature = [
subdevice.WheelTemperatureSensor(dcm, mem, "WheelFR"),
subdevice.WheelTemperatureSensor(dcm, mem, "WheelFL"),
subdevice.WheelTemperatureSensor(dcm, mem, "WheelB")
]
#------------------------- Bumpers -----------------------#
list_bumper = [
subdevice.Bumper(dcm, mem, "FrontRight"),
subdevice.Bumper(dcm, mem, "FrontLeft"),
subdevice.Bumper(dcm, mem, "Back")
]
#-------------------------- Leds --------------------------#
# Disable notifications (from disconnected tablet)
expressiveness._setNotificationEnabled(False)
# Switch off eyes and ears leds
leds.off('FaceLeds')
leds.off('EarLeds')
# Switch on shoulder leds
leds.createGroup("shoulder_group", SHOULDER_LEDS)
leds.on("shoulder_group")
# Bumpers activation detection
parameters_bumpers = tools.read_section("config.cfg",
"BumpersActivationsParameters")
# Launch bumper counter (thread)
bumper_detection = mobile_base_utils.BumpersCounting(
dcm, mem, leds, wait_time_bumpers)
bumper_detection.start()
#---------------------------------------------------------#
#--------------------- Initialization --------------------#
#---------------------------------------------------------#
flag_bumpers = False
list_commands = move_robot_random(dcm, mem, wait_time, min_fraction,
max_fraction, max_random)
tools.wait(dcm, 2000)
#---------------------------------------------------------#
#----------------------- Main Loop -----------------------#
#---------------------------------------------------------#
while flag_bumpers == False:
try:
#-------------------- Wall event -------------------#
if (list_bumper[0].value == 1) or \
(list_bumper[1].value == 1) or \
(list_bumper[2].value == 1):
# Robot hit wall --> Stop it
list_wheel_speed_actuator[0].qvalue = (0.0, 0)
list_wheel_speed_actuator[1].qvalue = (0.0, 0)
list_wheel_speed_actuator[2].qvalue = (0.0, 0)
# Going back
list_wheel_speed_actuator[0].qvalue = [
(-1) * list_commands[0][0][0], wait_time
]
list_wheel_speed_actuator[1].qvalue = [
(-1) * list_commands[1][0][0], wait_time
]
list_wheel_speed_actuator[2].qvalue = [
(-1) * list_commands[2][0][0], wait_time
]
tools.wait(dcm, 4000)
# random again
list_commands = move_robot_random(dcm, mem, wait_time,
min_fraction, max_fraction,
max_random)
#tools.wait(dcm, 2000)
#--------------------- Check temperatures --------------------#
check_wheel_temperatures(dcm, leds, list_wheel_temperature,
list_wheel_speed_actuator,
list_wheel_stiffness_actuator)
#-------------------- Check test variables -------------------#
if bumper_detection.bumpers_activations == \
int(parameters_bumpers["nb_bumper_activations"][0]):
bumper_detection.stop()
flag_bumpers = True
# Exit test if user interrupt
except KeyboardInterrupt:
print "\n******* User interrupt - ending test *******"
bumper_detection.stop()
break
def test_move_random(motion, dcm, mem, leds, expressiveness, wait_time,
wait_time_bumpers, min_fraction, max_fraction, max_random,
stop_robot, wakeup_no_rotation, stiff_robot_wheels,
unstiff_joints, log_wheels_speed, log_bumper_pressions):
#---------------------------------------------------------#
#-------------------- Object creations -------------------#
#---------------------------------------------------------#
#------------------------- Wheels ------------------------#
list_wheel_speed_actuator = [
subdevice.WheelSpeedActuator(dcm, mem, "WheelFR"),
subdevice.WheelSpeedActuator(dcm, mem, "WheelFL"),
subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
]
list_wheel_speed_sensor = [
subdevice.WheelSpeedSensor(dcm, mem, "WheelFR"),
subdevice.WheelSpeedSensor(dcm, mem, "WheelFL"),
subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
]
list_wheel_stiffness_actuator = [
subdevice.WheelStiffnessActuator(dcm, mem, "WheelFR"),
subdevice.WheelStiffnessActuator(dcm, mem, "WheelFL"),
subdevice.WheelStiffnessActuator(dcm, mem, "WheelB")
]
list_wheel_temperature = [
subdevice.WheelTemperatureSensor(dcm, mem, "WheelFR"),
subdevice.WheelTemperatureSensor(dcm, mem, "WheelFL"),
subdevice.WheelTemperatureSensor(dcm, mem, "WheelB")
]
#------------------------- Bumpers -----------------------#
list_bumper = [
subdevice.Bumper(dcm, mem, "FrontRight"),
subdevice.Bumper(dcm, mem, "FrontLeft"),
subdevice.Bumper(dcm, mem, "Back")
]
#-------------------------- Leds --------------------------#
# Disable notifications (from disconnected tablet)
expressiveness._setNotificationEnabled(False)
motion.setExternalCollisionProtectionEnabled('All', False)
# Switch off eyes and ears leds
leds.off('FaceLeds')
leds.off('EarLeds')
# Switch on shoulder leds
leds.createGroup("shoulder_group", SHOULDER_LEDS)
leds.on("shoulder_group")
#----------------------------------------------------------#
#--------------------- Test selection ---------------------#
#----------------------------------------------------------#
# Cables crossing detection
parameters_cables = tools.read_section("config.cfg",
"CablesRoutingParameters")
# Bumpers activation detection
parameters_bumpers = tools.read_section("config.cfg",
"BumpersActivationsParameters")
# Get the desired type of test
flag_test = tools.read_section("config.cfg", "TestChoice")
# Launch bumper counter (thread)
if int(flag_test["test_bumpers"][0]) == 1:
bumper_detection = mobile_base_utils.BumpersCounting(
dcm, mem, leds, wait_time_bumpers)
bumper_detection.start()
# Launch cable counter (thread)
if int(flag_test["test_cables_crossing"][0]) == 1:
cable_detection = mobile_base_utils.CablesCrossing(dcm, mem)
cable_detection.start()
#---------------------------------------------------------#
#--------------------- Initialization --------------------#
#---------------------------------------------------------#
flag_bumpers = False
flag_cables = False
list_wheels_to_use = [1, 1, 0]
#tools.wait(dcm, 30000)
used_wheels = move_robot(dcm, mem, wait_time, list_wheels_to_use)
tools.wait(dcm, 2000)
acc_z = subdevice.InertialSensorBase(dcm, mem, "AccelerometerZ")
#---------------------------------------------------------#
#----------------------- Main Loop -----------------------#
#---------------------------------------------------------#
while flag_bumpers == False and flag_cables == False:
try:
#-------------------- Wall event -------------------#
if (math.fabs(list_wheel_speed_sensor[0].value) < 0.10) and \
(math.fabs(list_wheel_speed_sensor[1].value) < 0.10) and \
(math.fabs(list_wheel_speed_sensor[2].value) < 0.10):
# Robot hit wall --> Stop it
list_wheel_speed_actuator[0].qvalue = (0.0, 0)
list_wheel_speed_actuator[1].qvalue = (0.0, 0)
list_wheel_speed_actuator[2].qvalue = (0.0, 0)
# stop_robot(list_wheel_speed_actuator)
# check temperatures
check_wheel_temperatures(dcm, leds, list_wheel_temperature,
list_wheel_speed_actuator,
list_wheel_stiffness_actuator)
alea = int(random.uniform(0, 10))
if used_wheels[0] == 'fr' and used_wheels[1] == 'fl':
wheel_stiffed_1 = list_wheel_stiffness_actuator[0]
wheel_stiffed_2 = list_wheel_stiffness_actuator[1]
list_wheel_stiffness_actuator[2].qvalue = (1.0, 0)
if alea <= 5:
wheel_stiffed_1.qvalue = (0.0, 0)
list_wheels_to_use = [0, 1, 1]
else:
wheel_stiffed_2.qvalue = (0.0, 0)
list_wheels_to_use = [1, 0, 1]
if used_wheels[0] == 'fl' and used_wheels[1] == 'b':
wheel_stiffed_1 = list_wheel_stiffness_actuator[1]
wheel_stiffed_2 = list_wheel_stiffness_actuator[2]
list_wheel_stiffness_actuator[0].qvalue = (1.0, 0)
if alea <= 5:
wheel_stiffed_1.qvalue = (0.0, 0)
list_wheels_to_use = [1, 0, 1]
else:
wheel_stiffed_2.qvalue = (0.0, 0)
list_wheels_to_use = [1, 1, 0]
if used_wheels[0] == 'b' and used_wheels[1] == 'fr':
wheel_stiffed_1 = list_wheel_stiffness_actuator[2]
wheel_stiffed_2 = list_wheel_stiffness_actuator[0]
list_wheel_stiffness_actuator[1].qvalue = (1.0, 0)
if alea <= 5:
wheel_stiffed_1.qvalue = (0.0, 0)
list_wheels_to_use = [1, 1, 0]
else:
wheel_stiffed_2.qvalue = (0.0, 0)
list_wheels_to_use = [0, 1, 1]
used_wheels = move_robot(dcm, mem, wait_time,
list_wheels_to_use)
tools.wait(dcm, 2000)
#-------------------- Check test variables -------------------#
if int(flag_test["test_bumpers"][0]) == 1 and \
bumper_detection.bumpers_activations == \
int(parameters_bumpers["nb_bumper_activations"][0]):
bumper_detection.stop()
flag_bumpers = True
if int(flag_test["test_cables_crossing"][0]) == 1 and \
cable_detection.cables_crossing == \
int(parameters_cables["Nb_cables_crossing"][0]):
cable_detection.stop()
flag_cables = True
#-------- Robot has fallen ----------#
if (math.fabs(acc_z.value)) < 2.0:
if int(flag_test["test_bumpers"][0]) == 1:
bumper_detection.stop()
if int(flag_test["test_cables_crossing"][0]) == 1:
cable_detection.stop()
break
# Exit test if user interrupt
except KeyboardInterrupt:
print "\n******* User interrupt - ending test *******"
if int(flag_test["test_bumpers"][0]) == 1:
bumper_detection.stop()
if int(flag_test["test_cables_crossing"][0]) == 1:
cable_detection.stop()
break
def wake_up_pos():
"""
Fixture which retrieves wakeUp joints position from a configuration file.
"""
return tools.read_section("juliette_positions.cfg", "wakeUp")
def move_robot(motion):
Move_x = 0
Move_y = 0
diag_1 = 0
diag_2 = 0
parameters_direction = tools.read_section("config.cfg",
"DirectionParameters")
distance = int(parameters_direction["distance"][0])
nb_passage = int(parameters_direction["nb_passage"][0])
dir_x = int(parameters_direction["dir_X"][0])
dir_y = int(parameters_direction["dir_Y"][0])
dir_diag_l = int(parameters_direction["dir_diagL"][0])
dir_diag_r = int(parameters_direction["dir_diagR"][0])
question_continue('start')
if dir_x == 1:
print 'move to X'
for move_x in range(nb_passage):
question_continue('forward')
motion.moveTo(distance, 0, 0)
question_continue('reverse')
motion.moveTo(-distance, 0, 0)
if move_x == nb_passage:
move_x = 0
question_continue('')
pass
if dir_y == 1:
question_continue('to Y')
print 'move to Y'
for move_y in range(nb_passage):
question_continue('left')
motion.moveTo(0, distance, 0)
question_continue('right')
motion.moveTo(0, -distance, 0)
Move_y = +1
if move_y == nb_passage:
move_y = 0
pass
if dir_diag_l == 1:
question_continue('to diagonale')
print 'move to first diagonale'
for diag_1 in range(nb_passage):
question_continue('diagonale front left')
motion.moveTo(distance, distance, 0)
question_continue('diagonale back left')
motion.moveTo(-distance, -distance, 0)
diag_1 = +1
if diag_1 == nb_passage:
diag_1 = 0
pass
if dir_diag_r == 1:
question_continue('to diagonale')
print 'move to second diagonale'
for diag_2 in range(nb_passage):
question_continue('diagonale front right')
motion.moveTo(distance, -distance, 0)
question_continue('diagonale back right')
motion.moveTo(-distance, distance, 0)
diag_2 = +1
if diag_2 == nb_passage:
diag_2 = 0
pass
motion.rest()