def control():
""" Control if brakes slip"""
kneepitch_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "KneePitch")
kneepitch_position_sensor = subdevice.JointPositionSensor(
dcm, mem, "KneePitch")
kneepitch_hardness_actuator = subdevice.JointHardnessActuator(
dcm, mem, "KneePitch")
hippitch_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "HipPitch")
hippitch_position_sensor = subdevice.JointPositionSensor(
dcm, mem, "HipPitch")
hippitch_hardness_actuator = subdevice.JointHardnessActuator(
dcm, mem, "HipPitch")
hippitch_hardness_actuator.qqvalue = 0.
kneepitch_hardness_actuator.qqvalue = 0.
while not thread_flag.is_set():
if abs(hippitch_position_sensor.value) > 0.1 or\
abs(kneepitch_position_sensor.value) > 0.1:
hippitch_hardness_actuator.qqvalue = 1.
kneepitch_hardness_actuator.qqvalue = 1.
hippitch_position_actuator.qvalue = (0., 1000)
kneepitch_position_actuator.qvalue = (0., 1000)
tools.wait(dcm, 2100)
hippitch_hardness_actuator.qqvalue = 0.
kneepitch_hardness_actuator.qqvalue = 0.
def check_wheel_temperatures(dcm, leds, list_wheel_temperature,
list_wheel_speed, list_wheel_stiffness):
"""
Pauses the robot if the wheels are too hot
"""
if list_wheel_temperature[0].value >= 80 or \
list_wheel_temperature[1].value >= 80 or \
list_wheel_temperature[2].value >= 80:
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 >= 45 or \
list_wheel_temperature[1].value >= 45 or \
list_wheel_temperature[2].value >= 45:
tools.wait(dcm, 20000)
leds.reset("shoulder_group")
stiff_wheels(list_wheel_stiffness)
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 unstiff_joints(dcm, mem, wait_time):
"""
Unstiff all joints except HipPitch, KneePitch and Wheels
"""
joints = tools.use_section("config.cfg", "JulietteJoints")
for joint in joints:
joint_hardness = subdevice.JointHardnessActuator(dcm, mem, joint)
joint_hardness.qqvalue = 0.0
tools.wait(dcm, wait_time)
def _msg_send_timer(self, *arg, **kw):
if len(self.msg_buff_list) > 0:
msg_list = self.msg_buff_list.copy()
self.msg_buff_list.clear()
for msg in msg_list:
while len(msg) != 0:
send_msg = msg[:900]
msg = msg[900:]
self._send_msg(send_msg)
tools.wait(5)
def fin():
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")
wheel_fr_speed_actuator.qvalue = (0.0, 0)
wheel_fl_speed_actuator.qvalue = (0.0, 0)
wheel_b_speed_actuator.qvalue = (0.0, 0)
print "Robot stopped"
tools.wait(dcm, wait_time)
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 run(protocol, csvfile):
tools.seed_random(23)
node_count = 50
state = topology.create_nodes(node_count)
mobility.randomize_positions(state, xy_range=1000)
mobility.connect_range(state, max_links=150)
# create network and start routing software
network.apply(state=state, link_command=get_tc_command)
software.start(protocol)
tools.sleep(30)
for step_distance in [50, 100, 150, 200, 250, 300, 350, 400]:
print(f'{protocol}: step_distance {step_distance}')
traffic_beg = tools.traffic()
for n in range(0, 6):
#with open(f'graph-{step_distance}-{n}.json', 'w+') as file:
# json.dump(state, file, indent=' ')
# connect nodes range
wait_beg_ms = tools.millis()
# update network representation
mobility.move_random(state, distance=step_distance)
mobility.connect_range(state, max_links=150)
# update network
network.apply(state=state, link_command=get_tc_command)
# Wait until wait seconds are over, else error
tools.wait(wait_beg_ms, 10)
paths = tools.get_random_paths(state, 2 * 200)
paths = tools.filter_paths(state, paths, min_hops=2, path_count=200)
ping_result = tools.ping_paths(paths=paths, duration_ms=2000, verbosity='verbose')
packets_arrived_pc = 100 * (ping_result.received / ping_result.send)
traffic_end = tools.traffic()
# add data to csv file
extra = (['node_count', 'time_ms', 'step_distance_m', 'n', 'packets_arrived_pc'], [node_count, tools.millis() - wait_beg_ms, step_distance, n, packets_arrived_pc])
tools.csv_update(csvfile, '\t', extra, (traffic_end - traffic_beg).getData(), ping_result.getData())
traffic_beg = traffic_end
software.clear()
network.clear()
def verify_written_max(self, out):
success, error_counter, attempts = False, 0, 5
while not success:
maxs = self.pull_maxs()
success = self.__successful_update(out, maxs)
if success:
logging.debug("%s successfully written" % out)
return True
else:
logging.debug(
"run check again. written are %s (%s) and out %s" %
(set(maxs), maxs, set([out])))
error_counter += 1
wait(self.options["update_effect_time"])
if error_counter >= attempts:
logging.warning("Unsuccessful write check (%s attempts)" %
attempts)
return False
return True
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)
def control_forever(self):
last_successful_update, warnings_flag = None, False
while True:
try:
last_successful_update, warnings_flag = last_update_verification(
last_successful_update, warnings_flag, self)
if self._do_basic_control_cycle():
logging.debug("Successful control cycle")
last_successful_update, warnings_flag = utcnow(), True
wait(self.options["refresh_period"])
else:
logging.warning("Unsuccesful control cycle")
wait(self.options["refresh_period"])
except:
msg = "Unexpected error: %s" % str(sys.exc_info())
logging.critical(msg)
wait(self.options["refresh_period"])
logging.critical("CONTROL SCRIPT IS EXITING ABNORMALLY")
def do_control_cycle(self):
wait(self.options["refresh_period"])
return self._do_basic_control_cycle()
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