def move_180():
qbo_controller = qbo_control_client()
angular_speed = 0.8
lineal_speed = 0.0
turn_time = 1.35
qbo_controller.setLinearAngular(lineal_speed, angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed, angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed, angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed, angular_speed)
time.sleep(turn_time)
angular_speed = 0.0
lineal_speed = 0.2
turn_time = 1.2
qbo_controller.setLinearAngular(lineal_speed, angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed, angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed, angular_speed)
time.sleep(turn_time)
angular_speed = 0.0
lineal_speed = 0.0
qbo_controller.setLinearAngular(lineal_speed, angular_speed)
def move_180():
qbo_controller=qbo_control_client()
angular_speed=0.8
lineal_speed=0.0
turn_time=1.35
qbo_controller.setLinearAngular(lineal_speed,angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed,angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed,angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed,angular_speed)
time.sleep(turn_time)
angular_speed=0.0
lineal_speed=0.2
turn_time=1.2
qbo_controller.setLinearAngular(lineal_speed,angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed,angular_speed)
time.sleep(turn_time)
qbo_controller.setLinearAngular(lineal_speed,angular_speed)
time.sleep(turn_time)
angular_speed=0.0
lineal_speed=0.0
qbo_controller.setLinearAngular(lineal_speed,angular_speed)
def __init__(self):
rospy.init_node('DiagnosticFrontRight')
self.qbo_controller = qbo_control_client()
# The rate at which to publish the diagnostic sensor front right
self.rate = rospy.get_param("~rate", 1)
# Convert to a ROS rate
self.r = rospy.Rate(self.rate)
# Initialize the min alert distance to the params Qbo_arduqbo (in m)
self.alert_front_right = rospy.get_param(
"/qbo_arduqbo/controllers/sens_con/sensors/front/front_right_srf10/min_alert_distance"
)
client_speak = rospy.ServiceProxy("/say_fr1", Text2Speach)
#service_manager = rospy.ServiceProxy('manager', Text2Write)
# Create a diagnostics publisher
self.diag_pub_R = rospy.Publisher("diagnostics",
DiagnosticArray,
queue_size=1)
def __init__(self):
self.qbo_controller=qbo_control_client()
#self.qbo_talk_controller=qbo_talk_client()
self.lineal_speed=0.0
self.angular_speed=0.0
self.turn_time=0.1
self.wall_distance_limit=0.8
self.last_turn_direction=False #True means left
self.bad_word_said=False
#self.talking=False
self.sentences=['Ups','A can not pass though a wall','I will brake this wall. Can anyone put a cannon on me?','Atom, can you help me with this wall?', 'Oh my God. I am gonna crash', 'I shall not pass']
def __init__(self):
self.qbo_controller=qbo_control_client()
#self.qbo_talk_controller=qbo_talk_client()
self.sensors_distance=0.215
self.lineal_speed=0.0
self.angular_speed=0.0
self.turn_time=0.1
self.wall_distance_limit=0.3
self.last_turn_direction=False #True means left
self.bad_word_said=False
self.uniform_lineal_speed_change=0.05
#self.talking=False
self.sentences=['Ups','A can not pass though a wall','I will brake this wall. Can anyone put a cannon on me?','Atom, can you help me with this wall?', 'Oh my God. I am gonna crash', 'I shall not pass']
rospy.Subscriber("/qbo_face_tracking/face_pos_and_size", FacePosAndDist, face_callback)
def __init__(self):
self.qbo_controller=qbo_control_client()
#self.qbo_talk_controller=qbo_talk_client()
self.sensors_distance=0.215
self.lineal_speed=0.0
self.angular_speed=0.0
self.turn_time=0.1
self.wall_distance_limit=0.3
self.last_turn_direction=False #True means left
self.bad_word_said=False
self.uniform_lineal_speed_change=0.05
#self.talking=False
#self.sentences=['Ups','A can not pass though a wall','I will brake this wall. Can anyone put a cannon on me?','Atom, can you help me with this wall?', 'Oh my God. I am gonna crash', 'I shall not pass']
self.sentences=["向上", "我不能爬过这面墙", "我将制动这堵墙. 谁能够给我一个炮?", "阿童木,你能帮我克服这堵墙吗?", "天啊，我快摔倒了", "我无法通过"]
rospy.Subscriber("/qbo_face_tracking/face_pos_and_size", FacePosAndDist, face_callback)
def __init__(self):
self.qbo_controller = qbo_control_client()
#self.qbo_talk_controller=qbo_talk_client()
self.lineal_speed = 0.0
self.angular_speed = 0.0
self.turn_time = 0.1
self.wall_distance_limit = 0.8
self.last_turn_direction = False #True means left
self.bad_word_said = False
#self.talking=False
self.sentences = [
'Ups', 'A can not pass though a wall',
'I will brake this wall. Can anyone put a cannon on me?',
'Atom, can you help me with this wall?',
'Oh my God. I am gonna crash', 'I shall not pass'
]
def __init__(self):
rospy.init_node('DiagnosticBackLeft')
self.qbo_controller=qbo_control_client()
# The rate at which to publish the diagnostic sensor back left
self.rate = rospy.get_param("~rate", 1)
# Convert to a ROS rate
self.r = rospy.Rate(self.rate)
# Initialize the min alert distance to the params Qbo_arduqbo (in m)
self.alert_back_left = rospy.get_param("/qbo_arduqbo/controllers/sens_con/sensors/back/back_left_srf10/min_alert_distance")
# Create a diagnostics publisher
self.diag_pub_L = rospy.Publisher("diagnostics", DiagnosticArray)
def __init__(self):
rospy.init_node('DiagnosticFrontLeft')
self.qbo_controller=qbo_control_client()
# The rate at which to publish the diagnostic sensor front left
self.rate = rospy.get_param("~rate", 1)
# Convert to a ROS rate
self.r = rospy.Rate(self.rate)
# Initialize the min alert distance to the params Qbo_arduqbo (in m)
self.alert_front_left = rospy.get_param("/qbo_arduqbo/controllers/sens_con/sensors/front/front_left_srf10/min_alert_distance")
# Create a diagnostics publisher
self.diag_pub_L = rospy.Publisher("diagnostics", DiagnosticArray, queue_size = 1)
def __init__(self):
rospy.init_node('DiagnosticBackRight')
self.qbo_controller=qbo_control_client()
# The rate at which to publish the diagnostic sensor back right
self.rate = rospy.get_param("~rate", 1)
# Convert to a ROS rate
self.r = rospy.Rate(self.rate)
# Initialize the min alert distance to the params Qbo_arduqbo (in m)
self.alert_back_right = rospy.get_param("/qbo_arduqbo/controllers/sens_con/sensors/back/back_right_srf10/min_alert_distance")
# Create a diagnostics publisher
self.diag_pub_R = rospy.Publisher("diagnostics", DiagnosticArray)
def __init__(self):
self.qbo_controller = qbo_control_client()
#self.qbo_talk_controller=qbo_talk_client()
self.lineal_speed = 0.0
self.angular_speed = 0.0
self.turn_time = 0.1
self.wall_distance_limit = 0.2
self.last_turn_direction = False #True means left
self.bad_word_said = False
self.uniform_lineal_speed_change = 0.05
#self.talking=False
self.sentences = [
'Ups', 'A can not pass though a wall',
'I will brake this wall. Can anyone put a cannon on me?',
'Atom, can you help me with this wall?',
'Oh my God. I am gonna crash', 'I shall not pass'
]
rospy.Subscriber("/qbo_face_tracking/face_pos_and_dist",
FacePosAndDist, face_callback)
rospy.Subscriber("/listen/en_default", Listened, listen_callback)
def __init__(self):
rospy.init_node('DiagnosticFrontRight')
self.qbo_controller=qbo_control_client()
# The rate at which to publish the diagnostic sensor front right
self.rate = rospy.get_param("~rate", 1)
# Convert to a ROS rate
self.r = rospy.Rate(self.rate)
# Initialize the min alert distance to the params Qbo_arduqbo (in m)
self.alert_front_right = rospy.get_param("/qbo_arduqbo/controllers/sens_con/sensors/front/front_right_srf10/min_alert_distance")
client_speak = rospy.ServiceProxy("/qbo_talk/pico2wave_say", Text2Speach)
service_manager = rospy.ServiceProxy('manager', Text2Write)
# Create a diagnostics publisher
self.diag_pub_R = rospy.Publisher("diagnostics", DiagnosticArray)
def __init__(self):
self.qbo_controller = qbo_control_client()
self.obstacles = False
self.solMax = 35
self.solMin = 10
self.angle = -1.9 # angle maxi avant gestion de chute arrière, Ligne 71
self.no_floor = True
self.floorObstacle = True
self.too_imu_angle = True
self.blocked = True
self.sensors_distance = 0.215 #distance entre capteurs, sert a calculer l'angle, Ligne 129
self.lineal_speed = 0.0
self.angular_speed = 0.0
self.turn_time = 0.1
self.wall_distance_limit = 0.4
self.last_turn_direction = False # signifie tourne a droite
self.uniform_lineal_speed_change = 0.06 # permet un peu de variation de vitesse en ligne droite
self.sentences = [
'mais ya des murs partout? dans cette piaice', '', 'mince',
'sait la galère avec ces murs', '', '', '',
'y aurai pas un mur? la?', '', '', 'bon, encore un mur', '', '',
'j en ai marre de ces murs', '',
'bon? vincent? il est pourri ton programme'
]
self.sentences_chute = [
'Attention, je vais tomber', 'jai le vertige', 'rattrappe-moi',
'il faut que je recule', 'je vais tomber', 'vite', 'attention'
]
rospy.Subscriber('/distance_sensors_state/floor_sensor',
PointCloud,
self.capt_sol,
queue_size=1)
rospy.Subscriber('/imu_state/data',
Imu,
self.ImuCallback,
queue_size=1)
rospy.Subscriber('/odom', Odometry, self.odometry, queue_size=1)
def __init__(self):
self.qbo_controller=qbo_control_client()
self.obstacles=False
self.solMax=35
self.solMin=10
self.angle=-1.9 # angle maxi avant gestion de chute arrière, Ligne 71
self.no_floor=True
self.floorObstacle=True
self.too_imu_angle=True
self.blocked=True
self.sensors_distance=0.215 #distance entre capteurs, sert a calculer l'angle, Ligne 129
self.lineal_speed=0.0
self.angular_speed=0.0
self.turn_time=0.1
self.wall_distance_limit=0.4
self.last_turn_direction=False # signifie tourne a droite
self.uniform_lineal_speed_change=0.06 # permet un peu de variation de vitesse en ligne droite
self.sentences=['mais ya des murs partout? dans cette piaice','','mince','sait la galère avec ces murs','','','','y aurai pas un mur? la?','','','bon, encore un mur','','','j en ai marre de ces murs','','bon? vincent? il est pourri ton programme']
self.sentences_chute=['Attention, je vais tomber','jai le vertige','rattrappe-moi','il faut que je recule', 'je vais tomber','vite','attention']
rospy.Subscriber('/distance_sensors_state/floor_sensor', PointCloud, self.capt_sol, queue_size=1)
rospy.Subscriber('/imu_state/data',Imu,self.ImuCallback, queue_size=1)
rospy.Subscriber('/odom', Odometry, self.odometry, queue_size=1)
def __init__(self):
rospy.init_node('DiagnosticFloor')
# The rate at which to publish the diagnostic sensor floor
self.rate = rospy.get_param("~rate", 1)
# Convert to a ROS rate
r = rospy.Rate(self.rate)
self.qbo_controller=qbo_control_client()
# Create a diagnostics publisher
diag_pub = rospy.Publisher("diagnostics", DiagnosticArray)
# Initialize the min and the max alert distance to the params Qbo_arduqbo (in m)
floor_min = rospy.get_param("/qbo_arduqbo/controllers/sens_con/sensors/floor/floor_sensor/min_alert_distance")
floor_max = rospy.get_param("/qbo_arduqbo/controllers/sens_con/sensors/floor/floor_sensor/max_alert_distance")
#print floor_min
#print floor_max
rospy.loginfo("Publishing diagnostic message from the floor sensor")
while not rospy.is_shutdown():
# Initialize the diagnostics status
status = DiagnosticStatus()
status.name = "Floor Sensor"
status.hardware_id = "Sensors"
getFloorSensor = self.qbo_controller.getFloorSensor()
#print getFloorSensor
distanceFloor = "%.2f" %getFloorSensor[0][0]
#print distanceFloor
if getFloorSensor[0][0] > floor_max :
status.message = "Alert Floor Sensor"
status.level = DiagnosticStatus.WARN
elif getFloorSensor[0][0] < floor_min :
status.message = "Alert Floor Sensor"
status.level = DiagnosticStatus.WARN
else:
status.message = "Floor Sensor OK"
status.level = DiagnosticStatus.OK
now=rospy.Time.now()
# Add the raw floor sensor and limits to the diagnostics message
status.values.append(KeyValue("Floor sensor", str(distanceFloor)))
status.values.append(KeyValue("floor_min", str(floor_min)))
status.values.append(KeyValue("floor_max", str(floor_max)))
# Build the diagnostics array message
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
msg.status.append(status)
diag_pub.publish(msg)
r.sleep()
def __init__(self):
rospy.init_node('DiagnosticFloor')
# The rate at which to publish the diagnostic sensor floor
self.rate = rospy.get_param("~rate", 1)
# Convert to a ROS rate
r = rospy.Rate(self.rate)
self.qbo_controller = qbo_control_client()
# Create a diagnostics publisher
diag_pub = rospy.Publisher("diagnostics", DiagnosticArray)
# Initialize the min and the max alert distance to the params Qbo_arduqbo (in m)
floor_min = rospy.get_param(
"/qbo_arduqbo/controllers/sens_con/sensors/floor/floor_sensor/min_alert_distance"
)
floor_max = rospy.get_param(
"/qbo_arduqbo/controllers/sens_con/sensors/floor/floor_sensor/max_alert_distance"
)
#print floor_min
#print floor_max
rospy.loginfo("Publishing diagnostic message from the floor sensor")
while not rospy.is_shutdown():
# Initialize the diagnostics status
status = DiagnosticStatus()
status.name = "Floor Sensor"
status.hardware_id = "Sensors"
getFloorSensor = self.qbo_controller.getFloorSensor()
#print getFloorSensor
distanceFloor = "%.2f" % getFloorSensor[0][0]
#print distanceFloor
if getFloorSensor[0][0] > floor_max:
status.message = "Alert Floor Sensor"
status.level = DiagnosticStatus.WARN
elif getFloorSensor[0][0] < floor_min:
status.message = "Alert Floor Sensor"
status.level = DiagnosticStatus.WARN
else:
status.message = "Floor Sensor OK"
status.level = DiagnosticStatus.OK
now = rospy.Time.now()
# Add the raw floor sensor and limits to the diagnostics message
status.values.append(KeyValue("Floor sensor", str(distanceFloor)))
status.values.append(KeyValue("floor_min", str(floor_min)))
status.values.append(KeyValue("floor_max", str(floor_max)))
# Build the diagnostics array message
msg = DiagnosticArray()
msg.header.stamp = rospy.Time.now()
msg.status.append(status)
diag_pub.publish(msg)
r.sleep()
def __init__(self):
self.qbo_control = qbo_control_client()
self.qbo_arduqbo_params = []
self.qbo_arduqbo_get_functions = {}
self.qbo_arduqbo_set_functions = {}
#self.qbo_arduqbo_params.append('linearSpeed')
#self.qbo_arduqbo_get_functions['linearSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['linearSpeed']=self.qbo_control
#self.qbo_arduqbo_params.append('angularSpeed')
#self.qbo_arduqbo_get_functions['angularSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['angularSpeed']=self.qbo_control
self.qbo_arduqbo_params.append('speed')
self.qbo_arduqbo_get_functions['speed'] = self.qbo_control.speedGet
self.qbo_arduqbo_set_functions['speed'] = self.qbo_control.speedPut
self.qbo_arduqbo_params.append('position')
self.qbo_arduqbo_get_functions[
'position'] = self.qbo_control.positionGet
#self.qbo_arduqbo_params.append('robotPosX')
#self.qbo_arduqbo_get_functions['robotPosX']=self.qbo_control
#self.qbo_arduqbo_params.append('robotPosY')
#self.qbo_arduqbo_get_functions['robotPosY']=self.qbo_control
#self.qbo_arduqbo_params.append('robotPosTheta')
#self.qbo_arduqbo_get_functions['robotPosTheta']=self.qbo_control
#self.qbo_arduqbo_params.append('LCDstatus')
#self.qbo_arduqbo_get_functions['LCDstatus']=self.qbo_control
#self.qbo_arduqbo_params.append('LCDtext')
#self.qbo_arduqbo_set_functions['LCDtext']=self.qbo_control
self.qbo_arduqbo_params.append('LCD')
#self.qbo_arduqbo_get_functions['LCD']=self.qbo_control
self.qbo_arduqbo_set_functions['LCD'] = self.qbo_control.LCDPut
#self.qbo_arduqbo_params.append('SRFs')
#self.qbo_arduqbo_get_functions['SRFs']=self.qbo_control.
#self.qbo_arduqbo_params.append('SRF0')
#self.qbo_arduqbo_get_functions['SRF']=self.qbo_control
#self.qbo_arduqbo_params.append('SRF1')
#self.qbo_arduqbo_get_functions['SRF1']=self.qbo_control
#self.qbo_arduqbo_params.append('SRF2')
#self.qbo_arduqbo_get_functions['SRF2']=self.qbo_control
#self.qbo_arduqbo_params.append('SRF3')
#self.qbo_arduqbo_get_functions['SRF3']=self.qbo_control
#self.qbo_arduqbo_params.append('motorLeftState')
#self.qbo_arduqbo_get_functions['motorLeftState']=self.qbo_control
#self.qbo_arduqbo_set_functions['motorLeftState']=self.qbo_control
#self.qbo_arduqbo_params.append('motorRightState')
#self.qbo_arduqbo_get_functions['motorRightState']=self.qbo_control
#self.qbo_arduqbo_set_functions['motorRightState']=self.qbo_control
#self.qbo_arduqbo_params.append('motors')
#self.qbo_arduqbo_get_functions['motors']=self.qbo_control
#self.qbo_arduqbo_set_functions['motors']=self.qbo_control
self.qbo_arduqbo_params.append('headServos')
self.qbo_arduqbo_get_functions[
'headServos'] = self.qbo_control.headServosGet
self.qbo_arduqbo_set_functions[
'headServos'] = self.qbo_control.headServosPut
self.qbo_arduqbo_params.append('eyesServos')
self.qbo_arduqbo_get_functions[
'eyesServos'] = self.qbo_control.eyelidServosGet
self.qbo_arduqbo_set_functions[
'eyesServos'] = self.qbo_control.eyelidServosPut
#self.qbo_arduqbo_params.append('panServoState')
#self.qbo_arduqbo_get_functions['panServoState']=self.qbo_control
#self.qbo_arduqbo_set_functions['panServoState']=self.qbo_control
#self.qbo_arduqbo_params.append('tiltServoState')
#self.qbo_arduqbo_get_functions['tiltServoState']=self.qbo_control
#self.qbo_arduqbo_set_functions['tiltServoState']=self.qbo_control
#self.qbo_arduqbo_params.append('panServoPosition')
#self.qbo_arduqbo_get_functions['panServoPosition']=self.qbo_control
#self.qbo_arduqbo_set_functions['panServoPosition']=self.qbo_control
#self.qbo_arduqbo_params.append('tiltServoPosition')
#self.qbo_arduqbo_get_functions['tiltServoPosition']=self.qbo_control
#self.qbo_arduqbo_set_functions['tiltServoPosition']=self.qbo_control
#self.qbo_arduqbo_params.append('leftEyelidServoPosition')
#self.qbo_arduqbo_get_functions['leftEyelidServoPosition']=self.qbo_control
#self.qbo_arduqbo_set_functions['leftEyelidServoPosition']=self.qbo_control
#self.qbo_arduqbo_params.append('rightEyelidServoPosition')
#self.qbo_arduqbo_get_functions['rightEyelidServoPosition']=self.qbo_control
#self.qbo_arduqbo_set_functions['rightEyelidServoPosition']=self.qbo_control
#self.qbo_arduqbo_params.append('panServoSpeed')
#self.qbo_arduqbo_get_functions['panServoSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['panServoSpeed']=self.qbo_control
#self.qbo_arduqbo_params.append('tiltServoSpeed')
#self.qbo_arduqbo_get_functions['tiltServoSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['tiltServoSpeed']=self.qbo_control
#self.qbo_arduqbo_params.append('leftEyelidServoSpeed')
#self.qbo_arduqbo_get_functions['leftEyelidServoSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['leftEyelidServoSpeed']=self.qbo_control
#self.qbo_arduqbo_params.append('rightEyelidServoSpeed')
#self.qbo_arduqbo_get_functions['rightEyelidServoSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['rightEyelidServoSpeed']=self.qbo_control
self.qbo_arduqbo_params.append('MICs')
self.qbo_arduqbo_get_functions['MICs'] = self.qbo_control.MICsGet
#self.qbo_arduqbo_set_functions['MICs']=self.qbo_control.MICsPut
#self.qbo_arduqbo_params.append('MIC0')
#self.qbo_arduqbo_get_functions['MIC0']=self.qbo_control
#self.qbo_arduqbo_params.append('MIC1')
#self.qbo_arduqbo_get_functions['MIC1']=self.qbo_control
#self.qbo_arduqbo_params.append('MIC2')
#self.qbo_arduqbo_get_functions['MIC2']=self.qbo_control
#self.qbo_arduqbo_params.append('micsMute')
#self.qbo_arduqbo_set_functions['micsMute']=self.qbo_control
self.qbo_arduqbo_params.append('battery')
self.qbo_arduqbo_get_functions['battery'] = self.qbo_control.batteryGet
#self.qbo_arduqbo_params.append('batLevel')
#self.qbo_arduqbo_get_functions['batLevel']=self.qbo_control
self.qbo_arduqbo_params.append('test')
self.qbo_arduqbo_get_functions['test'] = self.qbo_control.testBoards
def __init__(self):
self.qbo_control=qbo_control_client()
self.qbo_arduqbo_params=[]
self.qbo_arduqbo_get_functions={}
self.qbo_arduqbo_set_functions={}
#self.qbo_arduqbo_params.append('linearSpeed')
#self.qbo_arduqbo_get_functions['linearSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['linearSpeed']=self.qbo_control
#self.qbo_arduqbo_params.append('angularSpeed')
#self.qbo_arduqbo_get_functions['angularSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['angularSpeed']=self.qbo_control
self.qbo_arduqbo_params.append('speed')
self.qbo_arduqbo_get_functions['speed']=self.qbo_control.speedGet
self.qbo_arduqbo_set_functions['speed']=self.qbo_control.speedPut
self.qbo_arduqbo_params.append('position')
self.qbo_arduqbo_get_functions['position']=self.qbo_control.positionGet
#self.qbo_arduqbo_params.append('robotPosX')
#self.qbo_arduqbo_get_functions['robotPosX']=self.qbo_control
#self.qbo_arduqbo_params.append('robotPosY')
#self.qbo_arduqbo_get_functions['robotPosY']=self.qbo_control
#self.qbo_arduqbo_params.append('robotPosTheta')
#self.qbo_arduqbo_get_functions['robotPosTheta']=self.qbo_control
#self.qbo_arduqbo_params.append('LCDstatus')
#self.qbo_arduqbo_get_functions['LCDstatus']=self.qbo_control
#self.qbo_arduqbo_params.append('LCDtext')
#self.qbo_arduqbo_set_functions['LCDtext']=self.qbo_control
self.qbo_arduqbo_params.append('LCD')
#self.qbo_arduqbo_get_functions['LCD']=self.qbo_control
self.qbo_arduqbo_set_functions['LCD']=self.qbo_control.LCDPut
#self.qbo_arduqbo_params.append('SRFs')
#self.qbo_arduqbo_get_functions['SRFs']=self.qbo_control.
#self.qbo_arduqbo_params.append('SRF0')
#self.qbo_arduqbo_get_functions['SRF']=self.qbo_control
#self.qbo_arduqbo_params.append('SRF1')
#self.qbo_arduqbo_get_functions['SRF1']=self.qbo_control
#self.qbo_arduqbo_params.append('SRF2')
#self.qbo_arduqbo_get_functions['SRF2']=self.qbo_control
#self.qbo_arduqbo_params.append('SRF3')
#self.qbo_arduqbo_get_functions['SRF3']=self.qbo_control
#self.qbo_arduqbo_params.append('motorLeftState')
#self.qbo_arduqbo_get_functions['motorLeftState']=self.qbo_control
#self.qbo_arduqbo_set_functions['motorLeftState']=self.qbo_control
#self.qbo_arduqbo_params.append('motorRightState')
#self.qbo_arduqbo_get_functions['motorRightState']=self.qbo_control
#self.qbo_arduqbo_set_functions['motorRightState']=self.qbo_control
#self.qbo_arduqbo_params.append('motors')
#self.qbo_arduqbo_get_functions['motors']=self.qbo_control
#self.qbo_arduqbo_set_functions['motors']=self.qbo_control
self.qbo_arduqbo_params.append('headServos')
self.qbo_arduqbo_get_functions['headServos']=self.qbo_control.headServosGet
self.qbo_arduqbo_set_functions['headServos']=self.qbo_control.headServosPut
self.qbo_arduqbo_params.append('eyesServos')
self.qbo_arduqbo_get_functions['eyesServos']=self.qbo_control.eyelidServosGet
self.qbo_arduqbo_set_functions['eyesServos']=self.qbo_control.eyelidServosPut
#self.qbo_arduqbo_params.append('panServoState')
#self.qbo_arduqbo_get_functions['panServoState']=self.qbo_control
#self.qbo_arduqbo_set_functions['panServoState']=self.qbo_control
#self.qbo_arduqbo_params.append('tiltServoState')
#self.qbo_arduqbo_get_functions['tiltServoState']=self.qbo_control
#self.qbo_arduqbo_set_functions['tiltServoState']=self.qbo_control
#self.qbo_arduqbo_params.append('panServoPosition')
#self.qbo_arduqbo_get_functions['panServoPosition']=self.qbo_control
#self.qbo_arduqbo_set_functions['panServoPosition']=self.qbo_control
#self.qbo_arduqbo_params.append('tiltServoPosition')
#self.qbo_arduqbo_get_functions['tiltServoPosition']=self.qbo_control
#self.qbo_arduqbo_set_functions['tiltServoPosition']=self.qbo_control
#self.qbo_arduqbo_params.append('leftEyelidServoPosition')
#self.qbo_arduqbo_get_functions['leftEyelidServoPosition']=self.qbo_control
#self.qbo_arduqbo_set_functions['leftEyelidServoPosition']=self.qbo_control
#self.qbo_arduqbo_params.append('rightEyelidServoPosition')
#self.qbo_arduqbo_get_functions['rightEyelidServoPosition']=self.qbo_control
#self.qbo_arduqbo_set_functions['rightEyelidServoPosition']=self.qbo_control
#self.qbo_arduqbo_params.append('panServoSpeed')
#self.qbo_arduqbo_get_functions['panServoSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['panServoSpeed']=self.qbo_control
#self.qbo_arduqbo_params.append('tiltServoSpeed')
#self.qbo_arduqbo_get_functions['tiltServoSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['tiltServoSpeed']=self.qbo_control
#self.qbo_arduqbo_params.append('leftEyelidServoSpeed')
#self.qbo_arduqbo_get_functions['leftEyelidServoSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['leftEyelidServoSpeed']=self.qbo_control
#self.qbo_arduqbo_params.append('rightEyelidServoSpeed')
#self.qbo_arduqbo_get_functions['rightEyelidServoSpeed']=self.qbo_control
#self.qbo_arduqbo_set_functions['rightEyelidServoSpeed']=self.qbo_control
self.qbo_arduqbo_params.append('MICs')
self.qbo_arduqbo_get_functions['MICs']=self.qbo_control.MICsGet
#self.qbo_arduqbo_set_functions['MICs']=self.qbo_control.MICsPut
#self.qbo_arduqbo_params.append('MIC0')
#self.qbo_arduqbo_get_functions['MIC0']=self.qbo_control
#self.qbo_arduqbo_params.append('MIC1')
#self.qbo_arduqbo_get_functions['MIC1']=self.qbo_control
#self.qbo_arduqbo_params.append('MIC2')
#self.qbo_arduqbo_get_functions['MIC2']=self.qbo_control
#self.qbo_arduqbo_params.append('micsMute')
#self.qbo_arduqbo_set_functions['micsMute']=self.qbo_control
self.qbo_arduqbo_params.append('battery')
self.qbo_arduqbo_get_functions['battery']=self.qbo_control.batteryGet
#self.qbo_arduqbo_params.append('batLevel')
#self.qbo_arduqbo_get_functions['batLevel']=self.qbo_control
self.qbo_arduqbo_params.append('test')
self.qbo_arduqbo_get_functions['test']=self.qbo_control.testBoards
def __init__(self):
global client_speak
global path
client_speak = rospy.ServiceProxy("/say_fr1", Text2Speach) # for french
rospy.Subscriber("/qbo_face_tracking/face_pos_and_dist", FacePosAndDist, self.face_callback)
rospy.Subscriber('listened', String, self.listen_callback)
os.system("amixer -c 1 sset Mic,0 nocap")
run_process("rosrun qbo_system_info plugin_system.py")
#Import des fonctions
self.kernel = aiml.Kernel()
self.system = Plug_System()
self.controller = qbo_control_client()
#Paramétres pour la Recognition
self.startNeo = False
self.startNeoOk = False
self.face_stabilizer =0
self.stabilizer_max = 10
self.stabilizer_thr = 7
self.distance_head_max = 1.5
self.recognized_name = ""
self.recog_name_certainty = 0.94
self.launch_reco = False
self.launch_listen = False
self.time_of_stop = 0
self.stop_reco = False
wait_time = 20
#Paramétre wake_up
self.OFFSET_X = 0.15
#Paramétre déplacement
self.bot_move = False
#Paramétres pour l'AIML
self.question =''
sessionTest = rospy.get_param("~sessionTest", True)
self.sessionId = 'unknown'
self.last_undetected = False
self.diagnostics ={}
self.ref_file = os.walk(path+'/reference_file')
fileBot = path+'/config/setBotPredicate.txt'
fileBotParameter = path+'/config/setBotParameter.txt'
self.fileParameterId = path+'/'+ self.sessionId +'/parameter/parameterId.txt'
self.fileParameterAiml = path+'/'+self.sessionId+'/parameter/parameterAiml.txt'
self.fileConversationId = path+'/'+self.sessionId+'conversation/conversationId.txt'
filesession = path+'/'+str(self.sessionId)+'/sessionData.txt'
#Permet de rappeller les infos du bot
f = open(fileBot)
for line in f.readlines():
try:
line = line.replace("\n","")
parts = line.split("|")
name = parts[0]
value = parts[1]
self.kernel.setBotPredicate(str(name),str(value))
print "kernel.setBotPredicate("+str(name)+", "+str(value)+")"
except Exception:
print 'Excepcion: '
pass
f.close()
if os.path.isfile("/home/neo/catkin_ws/src/qbo_ai/neo_ai_fr.brn") and not sessionTest:
self.kernel.bootstrap(brainFile = "/home/neo/catkin_ws/src/qbo_ai/neo_ai_fr.brn")
else:
self.kernel.bootstrap(learnFiles="/home/neo/catkin_ws/src/qbo_ai/neo_ai_fr/*.aiml")# charge tous les fichiers aiml
self.kernel.saveBrain("/home/neo/catkin_ws/src/qbo_ai/neo_ai_fr.brn")
self.system.netconf()
self.system.web()
self.system.testUsb()
self.system.testArduqbo()
self.diagnostics = self.system.aiml_system()
cleSyst = list(self.diagnostics.keys())
for y in range(0,len(cleSyst)):
try:
self.kernel.setPredicate(str(cleSyst[y]),str(self.diagnostics[cleSyst[y]]))
print str(cleSyst[y]) +' : '+str(self.diagnostics[cleSyst[y]])
except Exception:
pass
autonBat = self.kernel.getPredicate("autonBat")
reponse = self.kernel.respond('START')
speak_this(reponse)
rospy.sleep(4)
self.startNeo = True
self.time_of_stop = rospy.Time.now()
#Boucle principale de l'AIML de Neo
while not rospy.is_shutdown() :
self.set_period()
time_diff = rospy.Time.now() - self.time_of_stop
self.diagnostics = self.system.aiml_system()
autonBat = self.kernel.getPredicate("autonBat")
#Boucle secondaire : conversation avec la personne reconnu
if self.face_stabilizer >= self.stabilizer_thr and self.recognized_name != "" and self.recognized_name != "unknown" and not self.last_undetected:
print "j'ouvre le dossier de :"+str(self.recognized_name)
self.sessionId = str(self.recognized_name)
self.fileParameterId = path+'/'+ self.sessionId +'/parameter/parameterId.txt'
self.fileParameterAiml = path+'/'+self.sessionId+'/parameter/parameterAiml.txt'
self.fileConversationId = path+'/'+self.sessionId+'/conversation/conversationId.txt'
self.last_undetected = True
self.open_data(self.fileParameterId)
self.open_data(self.fileParameterAiml)
self.set_period()
client_quit = self.kernel.getPredicate("aurevoir",self.sessionId)
client_move = self.kernel.getPredicate("move",self.sessionId)
sessionData = self.kernel.getSessionData(self.sessionId)
reponse = self.kernel.respond('CONNECT',self.sessionId)
speak_this(reponse)
self.system.testUsb()
while not rospy.is_shutdown() and not (client_quit == str("oui") or client_move == str("1")):
time_diff = rospy.Time.now() - self.time_of_stop
client_quit = self.kernel.getPredicate("aurevoir",self.sessionId)
client_date = self.kernel.getPredicate("date",self.sessionId)
client_period = self.kernel.getPredicate("period",self.sessionId)
client_move = self.kernel.getPredicate("move",self.sessionId)
sessionData = self.kernel.getSessionData(self.sessionId)
if self.question != '' and self.last_undetected:
print client_period
cleSyst = list(self.diagnostics.keys())
for y in range(0,len(cleSyst)):
try:
self.kernel.setPredicate(str(cleSyst[y]),str(self.diagnostics[cleSyst[y]]), self.sessionId)
except Exception:
pass
reponse = self.kernel.respond(self.question, self.sessionId)
speak_this(reponse)
self.question = ''
elif time_diff.to_sec()>wait_time and not self.stop_reco :
self.kernel.setPredicate("aurevoir","oui",self.sessionId)
self.question = ''
if client_move == str("1") :
self.bot_move = True
self.kernel.setPredicate("aurevoir","oui",self.sessionId)
self.kernel.setPredicate("aurevoir","",self.sessionId)
self.last_undetected = False
print "je sauvegarde"
self.kernel.saveBrain("/home/Neo/catkin_ws/src/qbo_ai/Neo.brn")
self.save_file(self.sessionId,sessionData)
#self.save_conversation(sessionData)
rospy.sleep(3)
#Boucle secondaire : aprentissage d'une personne
elif self.face_stabilizer >= self.stabilizer_thr and self.recognized_name == "unknown" and not self.last_undetected :
self.last_undetected = True
reponse = self.kernel.respond('CONNECT')
speak_this(reponse)
client_quit = self.kernel.getPredicate("aurevoir")
client_move = self.kernel.getPredicate("move")
sessionData = self.kernel.getSessionData()
self.system.testUsb()
while not rospy.is_shutdown() and not (client_quit == str("oui") or client_move == str("1")):
time_diff = rospy.Time.now() - self.time_of_stop
client_quit = self.kernel.getPredicate("aurevoir")
client_date = self.kernel.getPredicate("date")
client_period = self.kernel.getPredicate("period")
client_move = self.kernel.getPredicate("move")
sessionData = self.kernel.getSessionData()
if self.question != '' and self.last_undetected:
print client_period
cleSyst = list(self.diagnostics.keys())
for y in range(0,len(cleSyst)):
try:
self.kernel.setPredicate(str(cleSyst[y]),str(self.diagnostics[cleSyst[y]]))
except Exception:
pass
reponse = self.kernel.respond(self.question)
speak_this(reponse)
self.question = ''
elif time_diff.to_sec()>wait_time and not self.stop_reco :
self.kernel.setPredicate("aurevoir","oui")
self.question = ''
if client_move == str("1") :
self.bot_move = True
self.kernel.setPredicate("aurevoir","oui")
self.kernel.setPredicate("aurevoir","")
self.last_undetected = False
print "je sauvegarde pas car je ne le connais pas"
elif time_diff.to_sec()>wait_time and not self.stop_reco and not self.bot_move :
run_process("rosnode kill /qbo_face_tracking")
run_process("rosnode kill /qbo_face_following")
self.stop_reco = True
self.last_undetected = False
self.wake_up()
#Mise à jour des diagnostiques de ROS dans l'Aiml
cleSyst = list(self.diagnostics.keys())
for y in range(0,len(cleSyst)):
try:
self.kernel.setPredicate(str(cleSyst[y]),str(self.diagnostics[cleSyst[y]]))
except Exception:
pass
