def __init__(self, dcm, mem, event, file_name):
"""
@dcm : proxy to DCM (object)
@mem : proxy to Motion (object)
@event : event object (object)
@file_name : file to save log (string)
"""
threading.Thread.__init__(self)
self._event = event
self._file_name = file_name
self._robot_on_charging_station = subdevice.ChargingStationSensor(
dcm, mem)
self._battery_current_sensor = subdevice.BatteryChargeSensor(dcm, mem)
self._wheelb_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelB")
self._wheelfr_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFR")
self._wheelfl_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFL")
self._wheelb_speed_sen = subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
self._wheelfr_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelFR")
self._wheelfl_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelFL")
self._wheelb_stiffness = subdevice.WheelStiffnessActuator(
dcm, mem, "WheelB")
self._wheelfr_stiffness = subdevice.WheelStiffnessActuator(
dcm, mem, "WheelFR")
self._wheelfl_stiffness = subdevice.WheelStiffnessActuator(
dcm, mem, "WheelFL")
self._end = False
def __init__(self, dcm, mem, event, file_name):
"""
@dcm : proxy to DCM (object)
@mem : proxy to ALMemory (object)
@event : event (object)
@file_name : name of log file (string)
"""
threading.Thread.__init__(self)
self._mem = mem
self._event = event
self._file_name = file_name
self._wheelb_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelB")
self._wheelfr_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFR")
self._wheelfl_speed_act = subdevice.WheelSpeedActuator(
dcm, mem, "WheelFL")
self._wheelb_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelB")
self._wheelfr_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelFR")
self._wheelfl_speed_sen = subdevice.WheelSpeedSensor(
dcm, mem, "WheelFL")
self._end = False
def log_bumper_pressions(request, dcm, mem, system, wait_time_bumpers):
"""
If one or more bumpers are pressed,
it saves wheels' speeds [rad/s]
"""
wheel_fr_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelFR")
wheel_fl_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelFL")
wheel_b_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
bumper_right = subdevice.Bumper(dcm, mem, "FrontRight")
bumper_left = subdevice.Bumper(dcm, mem, "FrontLeft")
bumper_back = subdevice.Bumper(dcm, mem, "Back")
list_bumpers = [bumper_right, bumper_left, bumper_back]
file_extension = "csv"
robot_name = system.robotName()
date = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
log_type = "wheels_speeds_when_bumper_pressions"
file_name = "-".join([robot_name, date, log_type])
output = ".".join([file_name, file_extension])
data = open(output, 'w')
data.write("Bumper FR" + "," + "Bumper FL" + "," + "Bumper B" + "," +
"wheel FR speed [rad/s]" + "," + "wheel FL speed [rad/s]" +
"," + "Wheel B speed [rad/s]" + "\n")
data.flush()
threading_flag = threading.Event()
def log(threading_flag):
while not threading_flag.is_set():
line = ""
flag = 0
speed_fr = wheel_fr_speed_sensor.value
speed_fl = wheel_fl_speed_sensor.value
speed_b = wheel_b_speed_sensor.value
for bumper in list_bumpers:
if bumper.value == 1:
flag += 1
line += str(1) + ","
else:
line += str(0) + ","
if flag > 0:
line += str(speed_fr) + "," + \
str(speed_fl) + "," + \
str(speed_b) + "\n"
data.write(line)
data.flush()
tools.wait(dcm, wait_time_bumpers)
log_thread = threading.Thread(target=log, args=(threading_flag, ))
log_thread.start()
def fin():
threading_flag.set()
request.addfinalizer(fin)
def log_wheels_actu_sens(request, dcm, mem, system, log_period):
"""
Log wheels' speeds [rad/s] every 0.5s
"""
wheel_fr_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelFR")
wheel_fl_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelFL")
wheel_b_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, "WheelB")
wheel_fr_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelFR")
wheel_fl_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelFL")
wheel_b_speed_actuator = subdevice.WheelSpeedActuator(dcm, mem, "WheelB")
file_extension = "csv"
robot_name = system.robotName()
date = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
log_type = "wheels_speeds"
file_name = "-".join([robot_name, date, log_type])
output = ".".join([file_name, file_extension])
data = open(output, 'w')
data.write("Time (s)" + "," + "wheel FR speed sensor [rad/s]" + "," +
"wheel FR speed actuator [rad/s]" + "," +
"wheel FL speed sensor [rad/s]" + "," +
"wheel FL speed actuator [rad/s]" + "," +
"Wheel B speed sensor [rad/s]" + "," +
"Wheel B speed actuator [rad/s]" + "\n")
threading_flag = threading.Event()
def log(threading_flag):
cpt = 1
time_init = time.time()
while not threading_flag.is_set():
line = ""
if float(format((time.time() - time_init),
'.1f')) == (cpt * log_period):
cpt += 1
line += str(float(format((time.time() - time_init),
'.1f'))) + "," + \
str(wheel_fr_speed_sensor.value) + "," + \
str(wheel_fr_speed_actuator.value) + "," + \
str(wheel_fl_speed_sensor.value) + "," + \
str(wheel_fl_speed_actuator.value) + "," + \
str(wheel_b_speed_sensor.value) + "," + \
str(wheel_b_speed_actuator.value) + "\n"
data.write(line)
data.flush()
log_thread = threading.Thread(target=log, args=(threading_flag, ))
log_thread.start()
def test_wheels_dico(request, result_base_folder, dcm, mem):
"""
Create the appropriate objects for each joint.
It logs the informations into a dictionnary and save the data into a file
after each wheel test.
"""
wheel_speed_actuator = subdevice.WheelSpeedActuator(
dcm, mem, request.param)
wheel_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, request.param)
logger = qha_tools.Logger()
def fin():
"""Method executed after a joint test."""
result_file_path = "/".join([
result_base_folder, wheel_speed_actuator.subdevice_type, "_".join(
[wheel_speed_actuator.short_name,
str(logger.flag)])
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
# creating a dictionnary with all the objects
dico_object = {
"wheelActuator": wheel_speed_actuator,
"wheelSensor": wheel_speed_sensor,
"logger": logger
}
return dico_object
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 run(self):
""" Log """
robot_on_charging_station = subdevice.ChargingStationSensor(
self.dcm, self.mem)
battery_current = subdevice.BatteryCurrentSensor(self.dcm, self.mem)
back_bumper_sensor = subdevice.Bumper(self.dcm, self.mem, "Back")
wheelfr_speed_actuator = subdevice.WheelSpeedActuator(
self.dcm, self.mem, "WheelFR")
wheelfl_speed_actuator = subdevice.WheelSpeedActuator(
self.dcm, self.mem, "WheelFL")
wheelfr_speed_sensor = subdevice.WheelSpeedSensor(
self.dcm, self.mem, "WheelFR")
wheelfl_speed_sensor = subdevice.WheelSpeedSensor(
self.dcm, self.mem, "WheelFL")
wheelfr_current_sensor = subdevice.WheelCurrentSensor(
self.dcm, self.mem, "WheelFR")
wheelfl_current_sensor = subdevice.WheelCurrentSensor(
self.dcm, self.mem, "WheelFL")
log_file = open(self.file_name, 'w')
log_file.write(
"Time,Detection,Current,BackBumper,WheelFRSpeedActuator," +
"WheelFRSpeedSensor,WheelFRCurrent,WheelFLSpeedActuator," +
"WheelFLSpeedSensor,WheelFLCurrent\n")
plot_server = easy_plot_connection.Server()
time_init = time.time()
while not self._end_plot:
elapsed_time = time.time() - time_init
plot_server.add_point("Detection", elapsed_time,
robot_on_charging_station.value)
plot_server.add_point("Current", elapsed_time,
battery_current.value)
plot_server.add_point("BackBumper", elapsed_time,
back_bumper_sensor.value)
plot_server.add_point("WheelFRSpeedActuator", elapsed_time,
wheelfr_speed_actuator.value)
plot_server.add_point("WheelFRSpeedSensor", elapsed_time,
wheelfr_speed_sensor.value)
line_to_write = ",".join([
str(elapsed_time),
str(robot_on_charging_station.value),
str(battery_current.value),
str(back_bumper_sensor.value),
str(wheelfr_speed_actuator.value),
str(wheelfr_speed_sensor.value),
str(wheelfr_current_sensor.value),
str(wheelfl_speed_actuator.value),
str(wheelfl_speed_sensor.value),
str(wheelfl_current_sensor.value)
])
line_to_write += "\n"
log_file.write(line_to_write)
log_file.flush()
time.sleep(0.1)
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
