def time_reaction(self): rospy.loginfo(self.time_os) r = platform_control() #In the first and in second condition miro is sleeping if (self.time_gb >= 120): self.mood = [0.0, 0.0] self.sleep = [ 0.0, 1.0 ] #value of sleep [0.0,1.0] emotional_state will be 0 NO RESPONSE r.lights_raw[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] #set his color to white self.pub_color.publish(r) elif (self.time_gb >= 80 and time_gb < 120 or time_oa >= 120): self.mood = [0.0, 0.0] self.sleep = [0.0, 1.0] r.lights_raw[255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255] #set his color to white self.pub_color.publish(r) #third condition too much time in oa or in gb miro will be angry elif (self.time_oa >= 60 or self.time_gb >= 60): self.mood = [0.0, 1.0] #ANGRY emotional_state must be 0.1 self.sleep = [1.0, 0.0] r.lights_raw[255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0] #set his color to red self.pub_color.publish(r) elif (self.time_oa >= 30 or self.time_os >= 30): self.mood = [0.5, 1.0] #ACTIVE emotional state must be 0.4 self.sleeo = [1, 0, 0.0] r.lights_raw[0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255] #set his color to blue self.pub_color.publish(r) elif (self.time_os >= 20 and self.time_os < 30): self.mood = [1.0, 1.0] #EXCITED emotional state must be 0.8 self.sleep = [1.0, 0.0] r.lights_raw[0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0] #set his color to green self.pub_color.publish(r) else: self.mood = [1.0, 0.5] #HAPPY emotional state must be 1 self.sleep = [1.0, 0.0] r.lights_raw[0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0] #set his color to green self.pub_color.publish(r) q = core_state() q.mood.valence = self.mood[0] q.mood.arousal = self.mood[1] q.sleep.wakefulness = self.sleep[0] q.sleep.pressure = self.sleep[1] self.pub_affect.publish(q)
def MiroScan(): rate = rospy.Rate(100) global sonar_value, yaw_angle #------------------------------------------------------- # Publishers #Neck joints pub_control_neck = rospy.Publisher("/miro/rob01/platform/control", platform_control) #------------------------------------------------------- q = platform_control() q.body_config_speed = [0, 0, -1, 0] #Taking the distance to the obstacle d = sonar_value theta_r = Angle_max_right theta_l = Angle_max_left i = 0 while (yaw_angle >= Angle_max_right): q.body_config = [0, 0, -3.14, 0] pub_control_neck.publish(q) rate.sleep() #turn head back to middle while (yaw_angle < -0.08): q.body_config = [0, 0, 0, 0] pub_control_neck.publish(q) rate.sleep() #do nothing i = 0 while (yaw_angle <= Angle_max_left): q.body_config = [0, 0, 3.14, 0] pub_control_neck.publish(q) #To be sure it's not just a false value of the sonar we will need 3 values of 0.0 rate.sleep() #turn head back to middle while (yaw_angle > 0.08): q.body_config = [0, 0, 0, 0] pub_control_neck.publish(q) rate.sleep() rospy.set_param("state_avoid", True) rospy.set_param("state_scan", False)
def switching_behaviour(self): ## Function that based on the value of the variable i, that represent the modality of navigation chosen, and the value of ## safe, that represent the presence of the obstacle, publish the behaviour selected on MiRo q = platform_control() p = platform_control() r = rospy.Rate(self.rate) while not rospy.is_shutdown(): if self.safe and self.i == 0: ## If there is not an obstacle and the modality is autonomous print "goal" ## Selecting Goal behaviour q = self.q_miroBOT ## Publishing platform_control message self.pub_behaviour.publish(q) elif not self.safe and self.i == 0: ## If there is an obstacle and the modality is autonomous print "obstacle" ## Selecting Obstacle Avoidance behaviour q = self.q_oab ## Publishing platform_control message self.pub_behaviour.publish(q) time.sleep(1.3) self.body_vel.linear.x = 500 p.body_vel = self.body_vel self.pub_behaviour.publish(p) time.sleep(0.5) time.sleep(0.3)
def callback(data): ## Callback function that receives the data from the joystick and based on the value of the variable i, that represent ## the modality of navigation chosen, publishes the velocity commands global i global counter q = platform_control() ## Vertical left stick axis = linear rate q.body_vel.linear.x = 100 * data.axes[1] ## Horizontal right stick axis = turn rate q.body_vel.angular.z = 100 * data.axes[2] if i == 1: # if counter <=300: ## If the modality is manual ## Publishing the message pub.publish(q)
def miro_kill(self): r = rospy.Rate(self.rate) q = platform_control() while not rospy.is_shutdown(): q.eyelid_closure = 0.4 q.body_config = [0.0, 0.87, 0.0, 0.75] q.body_config_speed = [0.0, -1.0, -1.0, -1.0] q.lights_raw = [ 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0 ] q.tail = 0.68 q.ear_rotate = [1.0, 1.0] q.sound_index_P2 = 15 self.pub_platform_control.publish(q) r.sleep()
def miro_sad(self): r = rospy.Rate(self.rate) q = platform_control() while not rospy.is_shutdown(): q.eyelid_closure = 0.3 q.body_config = [0.0, 1.0, 0.2, 0.1] q.body_config_speed = [0.0, -1.0, -1.0, -1.0] q.lights_raw = [ 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255 ] q.tail = -1.0 q.ear_rotate = [1.0, 1.0] q.body_vel.linear.x = 0.0 q.body_vel.angular.z = 0.2 self.pub_platform_control.publish(q) r.sleep()
def update(self): # Output new control command cmd = platform_control() cmd.body_vel = self.body_vel cmd.body_config = self.body_config cmd.body_config_speed = self.body_config_speed cmd.body_move = self.body_move cmd.tail = self.tail cmd.ear_rotate = self.ear_rotate cmd.eyelid_closure = self.eyelid_closure_out cmd.blink_time = self.blink_time cmd.lights_max_drive = self.lights_max_drive cmd.lights_dphase = self.lights_dphase cmd.lights_phase = self.lights_phase cmd.lights_amp = self.lights_amp cmd.lights_off = self.lights_off cmd.lights_rgb = self.lights_rgb cmd.sound_index_P1 = self.sound_index_P1 cmd.sound_index_P2 = self.sound_index_P2 self.cmd_out.publish(cmd)
def move2goal(self, data): ## Callback function that receives the position of the goal and construct the platform_control message to publish ## Setting the goal position self.goal = Pose2D() self.goal.x = data.x self.goal.y = data.y goal_pose = self.goal q = platform_control() distance_tolerance = 0.1 while self.euclidean_distance(goal_pose) >= distance_tolerance: ## Linear velocity in the x-axis self.body_vel.linear.x = self.linear_vel(goal_pose) self.body_vel.linear.y = 0 self.body_vel.linear.z = 0 ## Angular velocity in the z-axis self.body_vel.angular.x = 0 self.body_vel.angular.y = 0 self.body_vel.angular.z = self.angular_vel(goal_pose) ## Publishing the message q.body_vel = self.body_vel self.pub_platform_control.publish(q) self.rate.sleep() print "Goal reached" ## Publishing '1' on /goal_reached self.pub_goal_reached.publish(1) ## Stopping MiRo when the goal is reached self.body_vel.linear.x = 0 self.body_vel.angular.z = 0 q.body_vel = self.body_vel self.pub_platform_control.publish(q)
def miro_sleep(self): r = rospy.Rate(self.rate) q = platform_control() while True: try: q.eyelid_closure = 1.0 q.lights_raw = [ 0, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255 ] #white color q.tail = -1.0 q.ear_rotate = [0.0, 0.0] q.body_config = [0.0, 0.8, 0.3, 0.02] q.body_config_speed = [0.0, -1.0, -1.0, -1.0] self.pub_platform_control.publish(q) except KeyboardInterrupt: self.pub_platform_control.publish(q) break r.sleep()
def callback_oab(self, sonar_data): ## Callback that receives the data from the robot sensors, and uses the information given by the sonar sensor to evaluate the presence ## of an obstacle q = platform_control() sonar_value = sonar_data.sonar_range.range self.obstacle = sonar_value < self.danger_threshold if self.obstacle: r = rospy.Rate(self.rate) ## Counter of how many times the obstacle is encountered self.counter = self.counter + 1 ## Linear velocity in the x-axis self.body_vel.linear.x = 400 self.body_vel.linear.y = 0 self.body_vel.linear.z = 0 ## Angular velocity in the z-axis self.body_vel.angular.x = 0 self.body_vel.angular.y = 0 self.body_vel.angular.z = -1.9 * self.i ## Publishing the message q.body_vel = self.body_vel self.pub_platform_control.publish(q) ## If the obstacle is encountered more than two times if self.counter == 3: self.i = self.i * -1 # change right/left r.sleep() ## There is no more obstacle else: self.counter = 0
def data_in(self, data): # Update sensor variables self.battery_voltage = data.battery_voltage self.temperature = data.temperature.temperature self.accel_head = data.accel_head # Imu message type self.accel_body = data.accel_body # Imu message type self.odometry = data.odometry # Odometry message type self.joint_state = data.joint_state # JointState message type self.eyelid_closure = data.eyelid_closure self.sonar_range = data.sonar_range.range self.light = list(array("B", data.light)) self.touch_head = list(array("B", data.touch_head)) self.touch_body = list(array("B", data.touch_body)) self.cliff = list(array("B", data.cliff)) # Output new control command cmd = platform_control() cmd.body_vel = self.body_vel cmd.body_config = self.body_config cmd.body_config_speed = self.body_config_speed cmd.body_move = self.body_move cmd.tail = self.tail cmd.ear_rotate = self.ear_rotate cmd.eyelid_closure = self.eyelid_closure_out cmd.blink_time = self.blink_time cmd.lights_max_drive = self.lights_max_drive cmd.lights_dphase = self.lights_dphase cmd.lights_phase = self.lights_phase cmd.lights_amp = self.lights_amp cmd.lights_off = self.lights_off cmd.lights_rgb = self.lights_rgb cmd.sound_index_P1 = self.sound_index_P1 cmd.sound_index_P2 = self.sound_index_P2 self.cmd_out.publish(cmd) # Update relevant flags for flag in self.relevant_flags: flag.update()
def compared_detection(self): r = rospy.Rate(self.rate) q = platform_control() while not rospy.is_shutdown(): if self.ball_right and self.ball_left: self.count_ball = self.count_ball + 1 # rospy.loginfo(self.count_ball) if self.count_ball > 3: self.ball = True print "DETECTION COMPLETE" q.body_vel.linear.x = 100.0 q.body_vel.angular.z = 0.0 q.lights_raw = [ 255, 150, 0, 255, 150, 0, 255, 150, 0, 255, 150, 0, 255, 150, 0, 255, 150, 0 ] self.pub_follow_ball.publish(q) else: self.count_ball = 0 self.ball = False #q.body_vel.linear.x = 0.0 #q.body_vel.angular.z = 0.0 print "NO COMPLETE DETECTION" if self.ball_right: q.body_vel.linear.x = 0.0 q.body_vel.angular.z = -0.2 elif self.ball_left: q.body_vel.linear.x = 0.0 q.body_vel.angular.z = 0.2 else: q.body_vel.linear.x = 0.0 q.body_vel.angular.z = 0.0 self.pub_follow_ball.publish(q) r.sleep()
def callbackrpy(self,sw_data): self.rpy[0] = sw_data.vector.x#to modify self.rpy[1] = sw_data.vector.y#to modify self.rpy[2] = sw_data.vector.z#to modify q=platform_control() self.sw_vel=Twist() newroll=self.rpy[0]+70 #1) TO DO: MODIFY THE THRESHOLD if -15 < newroll < 15 and -20 < self.rpy[1] < 20: self.sw_vel.linear.x=0.0 self.sw_vel.angular.z=0.0 print ('MiRo STAY Still') elif self.rpy[1] > 40 or self.rpy[1] < -60: self.sw_vel.linear.x=-self.rpy[1]*6 self.sw_vel.angular.z=0.0 print ( 'max linear velocity') #if self.rpy[2] > 60: #self.lastyaw=True else: self.sw_vel.linear.x=0.0 self.sw_vel.angular.z=newroll*0.5*pi/180 print ('MiRo follow your owner') q.body_vel = self.sw_vel self.pub_mapping.publish(q)
def callback_turn_180(self, object): # ignore until active if not self.active: return # if turn is false, do nothing if not self.turn: return # send downstream command, ignore upstream data q = platform_control() # timing sync_rate = 50 period = 2 * sync_rate # two seconds per period z = self.count / period # advance pattern if not z == self.z_bak: self.z_bak = z # create body_vel for next pattern segment self.body_vel = Twist() if z < 2: print "turn left" self.body_vel.angular.z = +0.7854 if z == 2: print "turned" self.turn = False self.count = 0 # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) # count self.count = self.count + 1
def callback_platform_sensors(self, object): # ignore until active if not self.active: return # store object self.platform_sensors = object # print self.platform_sensors q = core_control() if np.sum(to_float_array(self.platform_sensors.light)) > 200: q.sleep_drive_target.wakefulness = 0.0 q.sleep_drive_target.pressure = 0.0 else: q.sleep_drive_target.wakefulness = 1.0 q.sleep_drive_target.pressure = 1.0 q.sleep_drive_gamma = 0.5 # print q self.pub_platform_control.publish(platform_control()) self.pub_core_control.publish(q)
def clear_danger(self, state): if state == 0: return print(self.state ) print(self.last_body_move) print(self.last_body_turn) print(self.last_head_change) # state 0= safe, 1 = move backwards, 2 = move forward, 3 = unclear/do nothing/ or turn etc. q = platform_control() # clear danger depending on memory of last actions # find out what is/what happening using current sensor information and last_head_move, last_body_move etc. # default wait # move backwards if self.last_body_move == "forward" and state == 1: q.body_vel.linear.x = -100 # move forward elif self.last_body_move == "backward" and self.platform_state.P1_R_signals == 12: q.body_vel.linear.x = +100 # turn right # turn left self.pub_platform_control.publish(q) # check if danger is still there, if yes repeat, else set state to safe if self.sensors.sonar_range.range > 0.05 and self.platform_state.P1_R_signals != 12 and self.sensors.cliff[0] > 2 and self.sensors.cliff[1] > 2: self.state = 0
def callback_range(self, object): # ignore until active if not self.active: return # store object self.range = object # send downstream command, ignoring upstream data q = platform_control() # timing sync_rate = 50 period = 2 * sync_rate # two seconds per period z = self.count / period # count self.count = self.count + 1 if self.count == 400: self.count = 0 # advance pattern if not z == self.z_bak: self.z_bak = z # create body_vel for next pattern segment self.body_vel = Twist() if self.drive_pattern == "turn": # turn 90deg left print "turn left and move backwards" self.body_vel.angular.z = +0.7854 self.body_vel.linear.x = -050 self.drive_pattern = "explore" self.count = 0 else: # do-si-do if z == 0: print "turn head right" self.body_config = miro.MIRO_YAW_MIN_RAD self.body_config_speed = miro.MIRO_P2U_W_LEAN_SPEED_INF if z == 1: print "turn head left" self.body_config = miro.MIRO_YAW_MAX_RAD self.body_config_speed = miro.MIRO_P2U_W_LEAN_SPEED_INF if z == 2: print "turn head straight" self.body_config = 0.5 * ( miro.MIRO_YAW_MAX_RAD - miro.MIRO_YAW_MIN_RAD) + miro.MIRO_YAW_MIN_RAD self.body_config_speed = miro.MIRO_P2U_W_LEAN_SPEED_INF if z == 3: if self.range.range > 0.20: print "drive forward" self.body_vel.linear.x = +050 else: print "obstacle, turn" self.drive_pattern = "turn" # no head turning at the moment self.body_config = 0.5 * ( miro.MIRO_YAW_MAX_RAD - miro.MIRO_YAW_MIN_RAD) + miro.MIRO_YAW_MIN_RAD self.body_config_speed = miro.MIRO_P2U_W_LEAN_SPEED_INF # point cameras down #q.body_config[1] = 1.0 #q.body_config_speed[1] = miro.MIRO_P2U_W_LEAN_SPEED_INF # publish q.body_vel = self.body_vel q.body_config[2] = self.body_config q.body_config_speed[2] = self.body_config_speed # print q self.pub_platform_control.publish(q)
key = arg[:f] val = arg[f + 1:] if key == "robot": robot_name = val elif key == "x": vel_x = float(val) elif key == "y": vel_y = float(val) else: error("argument not recognised \"" + arg + "\"") pub = rospy.Publisher('/miro/' + robot_name + '/platform/control', platform_control, queue_size=10) rate = rospy.Rate(10) q = platform_control() #-----------------------------END SETUP_MIRO--------------------------------- #-----------------------------START MOVE_FORWARD--------------------------------- body_vel = Twist() body_vel.angular.z = vel_y body_vel.linear.x = vel_x q.body_vel = body_vel #ensures that at least one node is connected before sending message while (pub.get_num_connections() == 0): rate.sleep() pub.publish(q) q.body_vel = body_vel pub.publish(q) #Allow time for the move to be executed #-----------------------------END MOVE_FORWARD---------------------------------
def update_body_vel(self, linear, angular): q = platform_control() q.body_vel.linear.x = linear * 1000 q.body_vel.angular.z = angular self.pub_platform_control.publish(q)
def callback_platform_sensors(self, object): # ignore until active if not self.active: return # store object self.platform_sensors = object # send downstream command, ignoring upstream data q = platform_control() # timing sync_rate = 50 # miro by default publishes at 50 Hz period = 2 * sync_rate # two seconds per period z = self.count / period # when z == 1 means that self.count is 100 which at 50 Hz means 2 seconds which is the period # In Python 2.x, integer divisions truncates instead of becoming a floating point number: 1 / 2 = 0 # advance pattern if not z == self.z_bak: self.z_bak = z # create body_vel for next pattern segment self.body_vel = Twist() if self.drive_pattern == "square": # square dance if z == 0 or z == 2: print "turn left" self.body_vel.angular.z = +0.7854 if z == 1 or z == 3: print "drive forward" self.body_vel.linear.x = +200 elif self.drive_pattern == "safety": if 0.03 < self.platform_sensors.sonar_range < 0.7: self.body_vel.linear.x = -200 else: self.body_vel.linear.x = +200 elif self.drive_pattern == "safety": if 0.03 < self.platform_sensors.sonar_range.range < 0.7: self.body_vel.linear.x = -200 else: self.body_vel.linear.x = 50 print self.platform_sensors.sonar_range.range elif self.drive_pattern == "avoidance1": # if there is something withing the safety threshold rotate left (w > 0) if 0.03 < self.platform_sensors.sonar_range.range < 0.7: self.body_vel.angular.z = +1.5708 # else go straight on else: self.body_vel.linear.x = +200 elif self.drive_pattern == "avoidance2": w = 1.5708 / 2 v = 200 # if measured distance is too close to the wall if 0.03 < self.platform_sensors.sonar_range.range < 0.7: #take a small right and move staright self.body_vel.angular.z = -w else: #take a small left and move staright self.body_vel.angular.z = +w # publish the roation vommand q.body_vel = self.body_vel self.pub_platform_control.publish(q) self.body_vel.linear.x = +v elif self.drive_pattern == "Pcontroller": d_ref = 0.5 Kp = 1 v = 100 # wall on the right if d_sonar < d_ref --> e > 0 --> Kp > 0 since w > 0 turns left and moves away from the wall on the right error = (d_ref - self.platform_sensors.sonar_range.range) self.body_vel.angular.z = Kp * error # for now v is constant self.body_vel.linear.x = +v print error print self.body_vel.angular.z elif self.drive_pattern == "PIDcontroller": v = 100 self.body_vel.angular.z = self.control_effort self.body_vel.linear.x = +v else: # do-si-do if z == 0: print "turn left" self.body_vel.angular.z = +1.5708 if z == 1: print "drive forward" self.body_vel.linear.x = +200 if z == 2: print "turn right" self.body_vel.angular.z = -1.5708 if z == 3: print "drive forward" self.body_vel.linear.x = +200 # point cameras down #q.body_config[1] = 1.0 #q.body_config_speed[1] = miro.MIRO_P2U_W_LEAN_SPEED_INF # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) # count self.count = self.count + 1 if self.count == 400: self.count = 0
def callback_platform_sensors(self, object): # ignore until active if not self.active: return # store object self.platform_sensors = object # send downstream command, ignoring upstream data q = platform_control() # timing sync_rate = 50 # syncRate is at 50Hz as set by the creators (this can be changed, from bridge) period = 2 * sync_rate # two seconds per period z = self.count / period # int/int is resulting in an int # advance pattern if not z == self.z_bak: self.z_bak = z # We enter into this 'if' after every 2 seconds # create body_vel for next pattern segment self.body_vel = Twist() if self.drive_pattern == "CarTopBottom": print "drive forward" self.body_vel.linear.x = +200 elif self.drive_pattern == "CarBottomTop": print "drive backward" self.body_vel.linear.x = -200 elif self.drive_pattern == "PatBody": print "drive clockwise" self.body_vel.linear.x = +200 self.body_vel.angular.z = +0.7854 elif self.drive_pattern == "FixedBody": print "Stop" self.body_vel.angular.x = 0 self.body_vel.angular.y = 0 self.body_vel.angular.z = 0 self.body_vel.linear.x = 0 self.body_vel.linear.y = 0 self.body_vel.linear.z = 0 elif self.drive_pattern == "PatHead": print "turn head" if (z == 0 or z == 2): q.body_config[2] = -1.0 q.body_config_speed[2] = miro.MIRO_P2U_W_LEAN_SPEED_INF if (z == 1): q.body_config[2] = +1.0 q.body_config_speed[2] = miro.MIRO_P2U_W_LEAN_SPEED_INF elif self.drive_pattern == "FixedHead": print "tilt head" q.body_config[1] = 0 q.body_config[2] = 0 q.body_config_speed[1] = miro.MIRO_P2U_W_LEAN_SPEED_INF q.body_config_speed[2] = miro.MIRO_P2U_W_LEAN_SPEED_INF else: self.body_vel.angular.x = 0 self.body_vel.angular.y = 0 self.body_vel.angular.z = 0 self.body_vel.linear.x = 0 self.body_vel.linear.y = 0 self.body_vel.linear.z = 0 # else: # # do-si-do # if z == 0: # print "turn left" # self.body_vel.angular.z = +1.5708 # if z == 1: # print "drive forward" # self.body_vel.linear.x = +100 # if z == 2: # print "turn right" # self.body_vel.angular.z = -1.5708 # if z == 3: # print "drive forward" # self.body_vel.linear.x = +100 # point cameras down # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) # count self.count = self.count + 1 if self.count == 200: self.count = 0
def callback_emotional_behaviour(self,data_affect): self.emotional_state = data_affect.data print(self.emotional_state) if(self.emotional_state == 0): #MiRo is sleeping so nothing is moving self.eyelid_closure = 0.0 self.ear_rotate = [0.0,0.0] self.lights_raw = [255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255]#white color self.tail = -1.0 else: if(self.emotional_state >= 0.2 and self.emotional_state < 0.4): #UNHAPPY q = platform_control() q.lights_raw = self.lights_raw = [255,0,0,255,0,0,255,0,0,255,0,0,255,0,0,255,0,0] #set his color to red q.tail =self.tail = 0.0 q.ear_rotate = self.ear_rotate = [1.0,1.0] q.eyelid_closure = self.eyelid_closure = 0.0 elif(self.emotional_state < 0.2 ): #ANGRY q = platform_control() q.lights_raw = self.lights_raw = [255,0,0,255,0,0,255,0,0,255,0,0,255,0,0,255,0,0] q.ear_rotate = self.ear_rotate = [1.0,1.0] q.eyelid_closure = self.eyelid_closure = 0.0 q.tail = self.tail = 1.0 elif (self.emotional_state >=0.5 and self.emotional_state < 0.7): #RELAXED OR BORED q = platform_control() q.lights_raw = self.lights_raw = [0,0,255,0,0,255,0,0,255,0,0,255,0,0,255,0,0,255]#the color is setted to blue q.eyelid_closure = self.eyelid_closure = 0.5 q.tail = self.tail = -1.0 q.ear_rotate = self.ear_rotate = [0.0,0.0] elif(self.emotional_state > 0.3 and self.emotional_state < 0.5): #ACTIVE q = platform_control() q.lights_raw = self.lights_raw = [0,0,255,0,0,255,0,0,255,0,0,255,0,0,255,0,0,255] q.eyelid_closure = self.eyelid_closure = 0.0 q.tail = self.tail = 1.0 q.ear_rotate = self.ear_rotate = [0.0,0.0] elif (self.emotional_state >= 0.8): #HAPPY OR EXCITED q = platform_control() q.lights_raw = self.lights_raw = [0,255,0,0,255,0,0,255,0,0,255,0,0,255,0,0,255,0] q.eyelid_closure = self.eyelid_closure = 0.0 q.tail = self.tail = 1.0 q.ear_rotate = self.ear_rotate = [0.0,0.0] elif(self.emotional_state >= 0.7 and self.emotional_state < 0.8): #SERENE q = platform_control() q.lights_raw = self.lights_raw = [0,255,0,0,255,0,0,255,0,0,255,0,0,255,0,0,255,0] q.eyelid_closure = self.eyelid_closure = 0.0 q.ear_rotate = self.ear_rotate = [0.0,0.0] q.tail = self.tail = 0.0 self.pub_emotional_behaviour.publish(q) qe=platform_control() qe.lights_raw = q.lights_raw self.pub_light_real.publish(qe)
def callback_platform_sensors(self, subscriber_msg): # ignore until active if not self.active: return ######## The main modified stuff: read the head touch data from the topic print "## HEAD DATA ##" #print(fmt(subscriber_msg.touch_head, '{0:.0f}')) #Uncomment and see what happens in output, It converts from byteArray to str A = int(fmt(subscriber_msg.touch_head, '{0:.0f}')[0]) # Sensor at 16'oclock on head B = int(fmt(subscriber_msg.touch_head, '{0:.0f}')[3]) # Sensor at 14'oclock on head C = int(fmt(subscriber_msg.touch_head, '{0:.0f}')[6]) # Sensor at 10'oclock on head D = int(fmt(subscriber_msg.touch_head, '{0:.0f}')[9]) # Sensor at 20'oclock on head cliff = map(int, fmt(subscriber_msg.cliff, '{0:.0f}').split(", ")) E = int(fmt(subscriber_msg.touch_body, '{0:.0f}')[0]) # Sensor at 16'oclock on body F = int(fmt(subscriber_msg.touch_body, '{0:.0f}')[3]) # Sensor at 14'oclock on body G = int(fmt(subscriber_msg.touch_body, '{0:.0f}')[6]) # Sensor at 10'oclock on body H = int(fmt(subscriber_msg.touch_body, '{0:.0f}')[9]) # Sensor at 20'oclock on body #print(type(subscriber_msg.cliff)) ######## The main modified stuff: print the head touch data from the topic print "## A ==>", A print "## B ==>", B print "## C ==>", C print "## D ==>", D print "## BODY DATA ##" print "## E ==>", E print "## F ==>", F print "## G ==>", G print "## H ==>", H ####### The main modified stuff: drive MiRo using touch data q = platform_control() self.body_vel = Twist() if cliff[0] and cliff[1] > 5: if A == 1 and B == 0: print "turn right" self.body_vel.angular.z = -1.5708 / 4 self.body_vel.linear.x = 0 if B == 1 and A == 0: print "turn right" self.body_vel.angular.z = -1.5708 / 4 self.body_vel.linear.x = 0 if B == 1 and A == 1: print "turn right" self.body_vel.angular.z = -1.5708 / 2 self.body_vel.linear.x = 0 if D == 1 and C == 0: print "turn left" self.body_vel.angular.z = +1.5708 / 4 self.body_vel.linear.x = 0 if C == 1 and D == 0: print "turn left" self.body_vel.angular.z = +1.5708 / 4 self.body_vel.linear.x = 0 if C == 1 and D == 1: print "turn left" self.body_vel.angular.z = +1.5708 / 2 self.body_vel.linear.x = 0 if E == 1 or F == 1: print "go backward" self.body_vel.linear.x = -60 if H == 1 or G == 1: print "go forward" self.body_vel.linear.x = +60 else: self.body_vel.linear.x = 0 self.body_vel.angular.z = 0 # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) #create, copy and publish the odometry message odom = Odometry() odom = subscriber_msg.odometry self.new_odom_publisher.publish(odom)
def callback_platform_sensors(self, object): # ignore until active if not self.active: return # store object self.platform_sensors = object # send downstream command, ignoring upstream data q = platform_control() # timing sync_rate = 50 period = 2 * sync_rate # two seconds per period z = self.count / period # advance pattern if not z == self.z_bak: self.z_bak = z # create body_vel for next pattern segment self.body_vel = Twist() if self.drive_pattern == "square": # square dance if z == 0 or z == 2: print "turn left" self.body_vel.angular.z = +0.7854 if z == 1 or z == 3: print "drive forward" self.body_vel.linear.x = +200 else: # do-si-do if z == 0: print "turn left" self.body_vel.angular.z = +1.5708 if z == 1: print "drive forward" self.body_vel.linear.x = +200 if z == 2: print "turn right" self.body_vel.angular.z = -1.5708 if z == 3: print "drive forward" self.body_vel.linear.x = +200 #we can modify this part to make miro avoid objectsself. #detect objects ahead,reverse #detect objects right, turn left #detect objects left, turn right # point cameras down #q.body_config[1] = 1.0 #q.body_config_speed[1] = miro.MIRO_P2U_W_LEAN_SPEED_INF # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) # count self.count = self.count + 1 if self.count == 400: self.count = 0 #modigy this part
def talker(): pub = rospy.Publisher('Mirostraight', Bool, queue_size=10) pub_platform_control = rospy.Publisher("/miro/rob01/platform/control", platform_control, queue_size=10) rate = rospy.Rate(10) # 10hz q = platform_control() #rospy.loginfo(v) while not rospy.is_shutdown(): global v if 0.1 < IR < 0.5: if v: q.body_vel.linear.x = 0 q.body_vel.linear.y = 0 pub_platform_control.publish(q) rate.sleep() rospy.set_param('state_straight', False) rospy.set_param('state_scan', True) #listenerMotion() ####################all this are to caalculate how much the miro is out on the straight line #param_name = rospy.search_param('virtual_x') #VX = rospy.get_param(param_name) #param_name = rospy.search_param('virtual_y') #VY = rospy.get_param(param_name) #outofline = ((x-start_x)/(VX-start_x))-((y-start_y)/(VY-start_y)) #if outofline > 0.5 #rospy.set_param('avoid_x', 5) #rospy.set_param('avoid_y', 5) #rospy.set_param('state_straight', False) #rospy.set_param('state_avoid', True) ############################################################################ param_name = rospy.search_param('new_goal') n = rospy.get_param(param_name) if x != None and y != None and n: global tetha miroline() rospy.set_param('new_goal', False) param_name = rospy.search_param('new_goal') n = rospy.get_param(param_name) rospy.loginfo(n) if v: #if it dosnt work check the subscriver and the callback q.body_vel.linear.x = 250 #+ 250 #* math.cos(teta) #this allow to go straight to the goal( follow the line could be weird) q.body_vel.linear.y = 0 #+250 #* math.sin(teta) #rospy.loginfo('theta :{} , yaw:{}'.format(tetha, yaw)) if tetha - yaw > 0.3: q.body_vel.angular.z = +1.5 #+3.14 #tetha-yaw rospy.loginfo('theta :{} , yaw:{}'.format(tetha, yaw)) elif tetha - yaw < -0.3: q.body_vel.angular.z = -1.5 #+3.14 #tetha-yaw rospy.loginfo('theta :{} , yaw:{}'.format(tetha, yaw)) else: q.body_vel.angular.z = 0 #print 'on my wayyyyyyyyyyyyyyyyyyyy' #global nangle #nangle = -1 * nangle #quaternion = quaternion_from_euler(0, 0, -180) #q.body_config_speed= [quaternion[0], quaternion[1], quaternion[2],quaternion[3]] #q.body_config_speed= [0.0, 0.296705961227417, -1.0471975803375244, -0.20943951606750488] #-1.0471975803375244 pub_platform_control.publish(q) rate.sleep() #q.body_config= [0.0, 0.296705961227417, -1.0471975803375244, -0.20943951606750488] # 3 for rotation, should look at right the miro, i don t know why it dosn t receive the information q.body_config_speed #q.ear_rotate =[0.5, 0.0] hello_str = 4 #this is not important #if quat != None: #euler = tf.transformations.euler_from_quaternion(quat) #rospy.loginfo(quat) param_name = rospy.search_param('state_straight') v = rospy.get_param(param_name)
def BehaviorCoordination (self): self.body_vel=Twist() threshold = 24.0 config = [0.0,0.0,0.0,0.0] config_speed = [0.0,0.0,0.0,0.0] q = platform_control() ra = rospy.Rate(self.rate) while not rospy.is_shutdown(): if self.r.smartwatch_state: if not self.r.obstacle_avoidance: print "|GESTURE BASED|" q = self.b.q_gb elif self.r.obstacle_avoidance: print "|OBSTACLE AVOIDANCE|" v_gb = self.b.q_gb.body_vel.linear.x w_gb = self.b.q_gb.body_vel.angular.z v_oa = self.b.q_oa.body_vel.linear.x w_oa = self.b.q_oa.body_vel.angular.z G = self.g.human_influence / threshold rospy.loginfo("b.v_gb * G %s", v_gb*G) self.body_vel.linear.x=v_oa*(1-G)+(v_gb*G) self.body_vel.angular.z=w_oa config = self.b.q_oa.body_config config_speed = self.b.q_oa.body_config_speed q.body_vel = self.body_vel q.body_config=config q.body_config_speed=config_speed if self.g.human_influence > threshold: self.body_vel.linear.x=0.0 self.body_vel.angular.z=0.0 config=[0.0,0.0,0.0,0.0] config_speed=[0.0,0.0,-1,0.0] q.body_vel = self.body_vel q.body_config=config q.body_config_speed=config_speed self.pub_platform_control.publish(q) # config_speed =[0.0,0.0,0.0,0.0] rospy.sleep(1) self.pub_arrived_update.publish(True) self.pub_escape.publish(True) else: q=self.b.q_e print "|EMOTION|" self.pub_platform_control.publish(q) ra.sleep()
def __init__(self): ## Node rate self.rate = rospy.get_param('rate', 200) #topic root ## Allow to switch from real robot to simulation from launch file self.robot_name = rospy.get_param('/robot_name', 'rob01') topic_root = "/miro/" + self.robot_name print "topic_root", topic_root ## Initialization of the enabling command self.activate = False ## Initialization of the string to evaluate self.command = String() #------------------ ADD OBJECTS OF NEW COMMANDS ----------------------# ## Platform control Object that represents the sleep action ("Sleep") self.q_sleep = platform_control() ## Platform control Object that represents the miro action when it is scolded ("Bad") self.q_sad = platform_control() ## Platform control Object that represents the miro action when it follows the Ball ("Play") self.q_play = platform_control() ## platform_control object that rapresents the Gesture Based Behavior ("Let's go out") self.q_gbb = platform_control() ## Platform control Object that represents the miro action when it is praised ("Good") self.q_good = platform_control() ## Platform control Object that represents the miro action when it is praised ("Kill") self.q_kill = platform_control() ## Subscriber to the topic /speech_to_text a message of type String that cointains the vocal command converted in text self.sub_speech_to_text = rospy.Subscriber( '/speech_to_text', String, self.callback_receive_command, queue_size=1) #------------------ ADD SUBSCRIBERS TO NEW COMMANDS -------------------# ## Subscriber to the topic /miro_sleep a message of type platform_control that rapresents the action corresponting to the command "Sleep" self.sub_sleep_action = rospy.Subscriber('/miro_sleep', platform_control, self.callback_sleep_action, queue_size=1) ## Subscriber to the topic /miro_sad a message of type platform_control that rapresents the action corresponting to the command "Bad" self.sub_sad_action = rospy.Subscriber('/miro_sad', platform_control, self.callback_sad_action, queue_size=1) ## Subscriber to the topic /miro_follow a message of type platform_control that rapresents the action corresponting to the command "Play" self.sub_play_action = rospy.Subscriber('/miro_play', platform_control, self.callback_play_action, queue_size=1) ## Subscriber to the topic /miro_dance a message of type platform_control that rapresents the action corresponting to the command "Let's go out" self.sub_gbb = rospy.Subscriber('/gbb', platform_control, self.callback_gbb, queue_size=1) ## Subscriber to the topic /miro_good a message of type platform_control that rapresents the action corresponting to the command "Good" self.sub_good_action = rospy.Subscriber('/miro_good', platform_control, self.callback_good_action, queue_size=1) ## Subscriber to the topic /miro_good a message of type platform_control that rapresents the action corresponting to the command "Kill" self.sub_kill_action = rospy.Subscriber('/miro_kill', platform_control, self.callback_kill_action, queue_size=1) ## Publisher to the topic /platform/control a message of type platform_control which execute Miro actions self.pub_platform_control = rospy.Publisher(topic_root + "/platform/control", platform_control, queue_size=0)
def switching_commands(self): q = platform_control() q.eyelid_closure = 1.0 r = rospy.Rate(self.rate) count_bad = 0 count_miro = 0 count_sleep = 0 while not rospy.is_shutdown(): #ACTIVATION COMMAND if self.command == "Miro" or self.command == " Miro" or self.command == "miro" or self.command == " miro": count_miro = 0 count_miro = count_miro + 1 rospy.loginfo(count_miro) count_sleep = 0 if count_miro == 1: q.eyelid_closure = 0.0 q.lights_raw = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] q.tail = -1.0 q.ear_rotate = [0.0, 0.0] q.body_config = [0.0, 0.0, 0.0, 0.0] q.body_config_speed = [0.0, -1.0, -1.0, -1.0] self.pub_platform_control.publish(q) self.activate = True # SLEEP if self.activate and self.command == "Sleep" or self.command == " Sleep" or self.command == "sleep" or self.command == " sleep": count_miro = 0 count_bad = 0 q = self.q_sleep self.pub_platform_control.publish(q) self.activate = False count_sleep = 1 print "Sleep" # BAD elif self.activate and (self.command == "Bad" or self.command == " Bad" or self.command == "bad" or self.command == " bad"): count_bad = count_bad + 1 rospy.loginfo(count_bad) if count_bad < 2000: q = self.q_sad self.pub_platform_control.publish(q) else: q.body_vel.linear.x = 0.0 q.body_vel.angular.z = 0.0 q.lights_raw = [ 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0, 255, 0, 0 ] self.pub_platform_control.publish(q) print "Bad" # PLAY elif self.activate and (self.command == "Play" or self.command == " Play" or self.command == "play" or self.command == " play"): count_bad = 0 q = self.q_play self.pub_platform_control.publish(q) # LET'S GO OUT elif self.activate and (self.command == "Let's go out" or self.command == " Let's go out" or self.command == "let's go out" or self.command == " let's go out"): count_bad = 0 q.body_config = [0.0, 0.0, 0.0, 0.0] q.body_config_speed = [0.0, -1.0, -1.0, -1.0] q = self.q_gbb self.pub_platform_control.publish(q) print "Let's go out" # GOOD elif self.activate and (self.command == "Good" or self.command == " Good" or self.command == "good" or self.command == " good"): count_bad = 0 q = self.q_good self.pub_platform_control.publish(q) print "Good" # KILL elif self.activate and (self.command == "Kill" or self.command == " Kill" or self.command == "kill" or self.command == " kill"): count_bad = 0 q = self.q_kill self.pub_platform_control.publish(q) print "Kill" # HANDLING OF DIFFERENT COMMANDS elif count_sleep == 1 and (not self.activate and not self.command == "Sleep"): rospy.loginfo(count_sleep) q.eyelid_closure = 1 self.pub_platform_control.publish(q) r.sleep()
def loop(self): r = rospy.Rate(self.rate) q = platform_control() while not rospy.is_shutdown(): if self.drive_pattern == "Pcontroller": d_ref = 0.5 Kp = 1 v = 100 # wall on the right if d_sonar < d_ref --> e > 0 --> Kp > 0 since w > 0 turns left # and moves away from the wall on the right error = (d_ref - self.platform_sensors.sonar_range.range) self.body_vel.angular.z = Kp * error # for now v is constant self.body_vel.linear.x = +v print 'error', error print 'angular velocity (controller)', self.body_vel.angular.z # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) elif self.drive_pattern == "obstacle_avoidance": # the robot circumnavigates the new obstacle clockwise until it reaches the m-line while self.new_obstacle: #stop the robot (reinitialize the velocities from previous runs) self.body_vel.linear.x = 0 self.body_vel.angular.z = 0 #rotate head to the right self.body_config_speed = [ 0, 0, -1, 0 ] # maximum spped to yaw rotation self.body_config = [0, 0, -1.04, 0] #yaw rotation # publish q.body_vel = self.body_vel q.body_config_speed = self.body_config_speed q.body_config = self.body_config self.pub_platform_control.publish(q) #rotate 90 deg to the left while abs( abs(self.th) - 1.5708 ) > self.th_threshold_first_rotation and self.new_obstacle and not rospy.is_shutdown( ): self.body_vel.linear.x = 0 self.body_vel.angular.z = self.K_first_rotation * abs( abs(self.th) - 1.5708) # rad/s positive rotation to the left # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) r.sleep() # rospy.sleep(5) #debug purposes # go around the obstacle simple proportionaol controller until m-line is met (y axis) # x_threshold is needed to avoid the robot thinks he is arrived when it is around the # the initial position: y = 0, x = 0 while (self.x < self.x_threshold or self.y > self.y_threshold ) and self.new_obstacle and not rospy.is_shutdown(): # control action definition: # wall on the right if d_sonar < d_ref --> e > 0 --> Kp > 0 since w > 0 turns left # and moves away from the wall on the right error = (self.d_ref_wall_following - self.platform_sensors.sonar_range.range) self.body_vel.angular.z = self.K_wall_following * error self.body_vel.linear.x = self.v_wall_following print 'error', error print 'angular velocity (controller)', self.body_vel.angular.z # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) r.sleep() #rotate head to look forward self.body_config_speed = [ 0, 0, -1, 0 ] # maximum spped to yaw rotation self.body_config = [0, 0, 0, 0] #yaw rotation # publish q.body_config_speed = self.body_config_speed q.body_config = self.body_config self.pub_platform_control.publish(q) # rospy.sleep(5) #debug purposes #rotate to the left until the robot does not see the obstacle anymore # do not tell the robot to re-orient as zero theta since it may hit the obstacle with the tail while abs( self.th ) > 0.05 and self.new_obstacle and not rospy.is_shutdown(): self.body_vel.linear.x = 0 self.body_vel.angular.z = abs(self.th) # rad/s # publish q.body_vel = self.body_vel self.pub_platform_control.publish(q) r.sleep() #publish True on /arrived topics once when the avoidance is complete self.pub_arrived.publish(True) # once the obstacle avoidance is complete reset the new_obstacle flag waiting for # the relative callback to update it self.new_obstacle = False r.sleep()
def location_miro(): # get relative destination # param_name = rospy.search_param('virtual_x') # x_dist = rospy.get_param(param_name) # param_name2 = rospy.search_param('virtual_y') # y_dist = rospy.get_param(param_name2) param_name = rospy.search_param('turn_dir') kak = rospy.get_param(param_name) # robot target robot = '/miro/rob01' # initialise ROS node rospy.init_node('location_miro', anonymous=True) # topic publishers pub = rospy.Publisher(robot + '/platform/control', platform_control, queue_size=10) pub2 = rospy.Publisher(robot + '/core/config', core_config, queue_size=10) # topic subscribers sub = rospy.Subscriber(robot + '/platform/state', platform_state, callback_state) # intialise rate object rate = rospy.Rate(10) # sleep to let the ROS node start up, or our config message # will not make it through time.sleep(1) # configure core to enable BODY module q = core_config() q.msg_flags = core_config.FLAG_UPDATE_SIGNALS q.P2U_W_body_signals = miro.MIRO_P2U_W_BODY_ENABLE pub2.publish(q) # create control message q = platform_control() #q.body_move.x = x_dist #q.body_move.y = y_dist if kak: q.body_move.x = 600 #in mm, positive to go forward q.body_move.y = -800 #negative to go to the right, positive to the left q.body_move.theta = 0 rospy.set_param('turn_dir',False) else: q.body_move.x = 600 #in mm, positive to go forward q.body_move.y = 800 #negative to go to the right, positive to the left q.body_move.theta = 0 rospy.set_param('turn_dir',True) # close eyes and hope for the best! q.eyelid_closure = 1 # main loop count = 1 while not rospy.is_shutdown(): # add if condition here to check for state_avoid parameter # configure move flags if count >= 2: q.body_move_flags = miro.MIRO_BODY_MOVE_CONTINUE if count == 2: q.body_move_flags |= miro.MIRO_BODY_MOVE_START # publish control message pub.publish(q) # postamble rospy.loginfo("@" + str(count) + ", move_underway=" + str(move_underway)) rate.sleep() count += 1 print count # exit if move_complete: #global counter rospy.set_param('state_straight', True) rospy.set_param('state_avoid', False) #counter=2 break;