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