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;
