def execute(self, userdata):
global isToTheLeft, CURRENT_STATE
CURRENT_STATE = "SeanTurn"
turn_direction = 1
start_pose = [0, 0, 0, 0]
if self.angle == 0: # target is goal + 0
goal = start_pose[1]
elif self.angle == 90: # target is goal + turn_direction * 90
goal = start_pose[1] + np.pi / 2 * turn_direction
elif self.angle == 180: # target is goal + turn_direction * 180
goal = start_pose[1] + np.pi * turn_direction
elif self.angle == -90: # target is goal + turn_direction * 270
goal = start_pose[1] - np.pi / 2 * turn_direction
elif self.angle == -100: # target is goal + turn_direction * 270
goal = start_pose[1] - 5 * np.pi / 9 * turn_direction
elif self.angle == 120:
goal = start_pose[1] + 2 * np.pi / 3 * turn_direction
elif self.angle == 135:
goal = start_pose[1] + 150 * np.pi / 180 * turn_direction
elif self.angle == 999:
if isToTheLeft:
goal = start_pose[1] - 85 * np.pi / 180 * turn_direction
else:
goal = start_pose[1] + 85 * np.pi / 180 * turn_direction
goal = angles_lib.normalize_angle(goal)
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
speed = 0.55
rate = rospy.Rate(30)
direction = turn_direction
if 2 * np.pi - angles_lib.normalize_angle_positive(
goal) < angles_lib.normalize_angle_positive(
goal) or self.angle == 0:
direction = turn_direction * -1
while not rospy.is_shutdown():
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
# slow down turning as we get closer to the target heading
if cur < 0.1:
speed = 0.1
if cur < 0.0174533:
break
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = direction * speed
self.cmd_pub.publish(msg)
rate.sleep()
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = 0.0
self.cmd_pub.publish(msg)
return 'done'
def execute(self, userdata):
global turn_direction
global START
global POSE
if not START:
return 'exit'
start_pose = POSE
if self.angle == 0: # target is goal + 0
goal = start_pose[1]
elif self.angle == 90: # target is goal + turn_direction * 90
goal = start_pose[1] + np.pi/2 * turn_direction
elif self.angle == 180: # target is goal + turn_direction * 180
goal = start_pose[1] + np.pi * turn_direction
elif self.angle == -90: # target is goal + turn_direction * 270
goal = start_pose[1] - np.pi/2 * turn_direction
elif self.angle == -100: # target is goal + turn_direction * 270
goal = start_pose[1] - 5*np.pi/9 * turn_direction
goal = angles_lib.normalize_angle(goal)
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
speed = 0.55
rate = rospy.Rate(30)
direction = turn_direction
if 2 * np.pi - angles_lib.normalize_angle_positive(goal) < angles_lib.normalize_angle_positive(goal) or self.angle == 0:
direction = turn_direction * -1
while not rospy.is_shutdown():
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
# slow down turning as we get closer to the target heading
if cur < 0.1:
speed = 0.15
if cur < 0.0571:
break
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = direction * speed
self.cmd_pub.publish(msg)
rate.sleep()
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = 0.0
self.cmd_pub.publish(msg)
return 'success'
def tranformation_string(res):
result = [0]*len(res)
for i in range(int(len(res)/3)):
if 0 > res[3*i]:
result[3*i] = -res[3*i]
result[3*i+1] = res[3*i+1] + pi
elif 0 < res[3*i]:
result[3*i] = res[3*i] # a1
result[3*i+1] = res[3*i+1] # b1
# lm1
result[3*i] = normalize_angle(result[3*i])
result[3*i+1] = normalize_angle(result[3*i+1])
result[3*i+2] = res[3*i+2] * res[3*i] / sin(res[3*i] / 2) / 2
return result
def check_goal(self, x, y, angle):
(trans, rot) = self.listener.lookupTransform(
'map', 'base_link', rospy.Time(0))
rpy = tf.transformations.euler_from_quaternion(rot)
self.assertAlmostEqual(trans[0], x, delta=0.2)
self.assertAlmostEqual(trans[1], y, delta=0.2)
self.assertAlmostEqual(rpy[0], 0.0, delta=0.005)
self.assertAlmostEqual(rpy[1], 0.0, delta=0.005)
diff = angles.normalize_angle(rpy[2]) - angles.normalize_angle(angle)
if diff > math.pi:
diff = diff - math.pi
if diff < -math.pi:
diff = diff + math.pi
self.assertAlmostEqual(diff, 0.0, delta=0.5)
def detect_from_lidar(self):
time_diff = rospy.Time.now().to_sec(
) - self.enemy_position.header.stamp.to_sec()
if time_diff > self.enemy_time_tolerance: # 敵情報が古かったら無視
self.detect_counter = 0
return False, 0.0, 0.0
else:
self.detect_counter = self.detect_counter + 1
if self.detect_counter < self.counter_th:
return False, 0.0, 0.0
map_topic = self.robot_namespace + "map"
baselink_topic = self.robot_namespace + "base_link"
trans, rot, vaild = self.get_position_from_tf(map_topic,
baselink_topic)
if vaild == False:
return False, 0.0, 0.0
dx = self.enemy_position.pose.pose.position.x - trans[0]
dy = self.enemy_position.pose.pose.position.y - trans[1]
enemy_distance = math.sqrt(dx * dx + dy * dy)
_, _, yaw = tf.transformations.euler_from_quaternion(rot)
enemy_direction = math.atan2(dy, dx)
enemy_direction_diff = angles.normalize_angle(enemy_direction - yaw)
return True, enemy_distance, enemy_direction_diff
def execute(self, userdata):
rospy.loginfo('Executing state ' + self.__class__.__name__)
try:
rospy.loginfo(
'Max ang vel:{}, Min ang vel:{}, Turn angle={}'.format(
math.degrees(self.min_velocity),
math.degrees(self.max_velocity),
math.degrees(userdata.turn_angle)))
pub = rospy.Publisher(self.cmd_vel, Twist, queue_size=10)
odom = Odometry()
odom = self.get_message()
if odom is None:
rospy.logerr('Cannot get oeometry')
return 'ng'
theta_pre = self.__get_yaw(odom)
theta_sum = 0
theta_sum_max = abs(userdata.turn_angle)
sign = 1
if userdata.turn_angle < 0:
sign = -1
vel = Twist()
vel.linear.x = 0
vel.linear.y = 0
vel.linear.z = 0
vel.angular.x = 0
vel.angular.y = 0
vel.angular.z = 0
self.time_checker.start()
while self.time_checker.is_timeout() is False:
odom = self.get_message()
if odom is None:
continue
theta = self.__get_yaw(odom)
d_theta = angles.normalize_angle(theta - theta_pre)
theta_sum = theta_sum + abs(d_theta)
theta_pre = theta
finished = False
if theta_sum >= theta_sum_max:
vel.angular.z = 0
finished = True
else:
vel_abs = theta_sum_max - theta_sum
vel_abs = min(self.max_velocity, vel_abs)
vel_abs = max(self.min_velocity, vel_abs)
vel.angular.z = sign * vel_abs
rospy.loginfo('Ang vel:{}, Turn sum={}, Turn to={}'.format(
math.degrees(vel.angular.z), math.degrees(theta_sum),
math.degrees(theta_sum_max)))
pub.publish(vel)
if finished:
return 'ok'
return 'timeout'
except Exception as e:
rospy.logerr(str(e))
return 'ng'
def execute(self, userdata):
global turn_direction
start_pose = userdata.start_pose_in
if self.angle == 0: # target is goal + 0
goal = start_pose[1]
elif self.angle == 90: # target is goal + turn_direction * 90
goal = start_pose[1] + np.pi / 2 * turn_direction
goal = angles_lib.normalize_angle(goal)
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
speed = 0.55
rate = rospy.Rate(30)
direction = turn_direction
if self.angle == 0:
direction = turn_direction * -1
while not rospy.is_shutdown():
cur = np.abs(angles_lib.normalize_angle(self.tb_rot[2]) - goal)
# slow down turning as we get closer to the target heading
if cur < 0.1:
speed = 0.15
if cur < 0.0571:
break
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = direction * speed
self.cmd_pub.publish(msg)
rate.sleep()
msg = Twist()
msg.linear.x = 0.0
msg.angular.z = 0.0
self.cmd_pub.publish(msg)
return 'success'
def pose_callback(cur_pose):
lock.acquire()
x = goal.x - cur_pose.x
y = goal.y - cur_pose.y
lock.release()
dist_to_goal = math.sqrt(x**2 + y**2)
error_angle = angles.shortest_angular_distance(angles.normalize_angle(cur_pose.theta), math.atan2(y, x))
velocity = Twist()
if abs(dist_to_goal) > 0.05:
velocity.linear.x = kf * dist_to_goal * math.cos(error_angle)
velocity.angular.z = kr * error_angle
vel_pub.publish(velocity)
def goToGoal(self):
self.goal_d = self.goal_distance(self.goal, self.pose)
if self.goal_d < 0.1:
goal_direction = self.goalYaw
else:
goal_direction = self.goal_direction(self.goal, self.pose)
self.new_heading = goal_direction if goal_direction > 0 else 2 * math.pi + goal_direction #when going to a self.goal location
self.new_heading = angles.normalize_angle(self.new_heading)
t_amt = angles.shortest_angular_distance(self.yaw, self.new_heading)
if abs(t_amt) > self.tolerance:
self.turn = True
return t_amt
def pose_callback(cur_pose):
lock.acquire()
x = goal.x - cur_pose.x
y = goal.y - cur_pose.y
lock.release()
dist_to_goal = math.sqrt(x**2 + y**2)
error_angle = angles.shortest_angular_distance(
angles.normalize_angle(cur_pose.theta), math.atan2(y, x))
velocity = Twist()
if abs(dist_to_goal) > 0.05:
velocity.linear.x = kf * dist_to_goal * math.cos(error_angle)
velocity.angular.z = kr * error_angle
vel_pub.publish(velocity)
def get_angle_and_dist_to_escape_home(detections):
"""Return the angle to turn and distance to drive in order to get
out of the home area if the rover is trapped in there. Should be
used in conjuction with Planner.is_inside_home_ring()
Args:
* detections - the list of AprilTagDetections
Returns:
* angle - the angle in radians to turn.
* distance - the distance in meters to drive.
Raises:
* tf.Exception if the transform into the base_link frame fails.
"""
OVERSHOOT_DIST = 0.4 # meters, distance to overshoot target by
result = {'angle': sys.maxint, 'dist': None}
see_home_tag = False
for detection in detections:
if detection.id == 256:
see_home_tag = True
home_detection = swarmie.transform_pose('/base_link',
detection.pose)
quat = [
home_detection.pose.orientation.x,
home_detection.pose.orientation.y,
home_detection.pose.orientation.z,
home_detection.pose.orientation.w
]
_r, _p, y = tf.transformations.euler_from_quaternion(quat)
y -= math.pi / 2
y = angles.normalize_angle(y)
if abs(y) < result['angle']:
result['angle'] = y
result['dist'] = \
(math.sqrt(home_detection.pose.position.x ** 2
+ home_detection.pose.position.y **2)
+ OVERSHOOT_DIST)
if not see_home_tag:
# doesn't make sense to turn or drive if no home tags were seen
return 0, 0
return result['angle'], result['dist']
def update(self, encoder_counts): # left: 0, right: 1
ticks = [
encoder_counts[Constants().WHEEL_LEFT] -
self._last_encs[Constants().WHEEL_LEFT],
encoder_counts[Constants().WHEEL_RIGHT] -
self._last_encs[Constants().WHEEL_RIGHT]
]
self._last_encs = copy.deepcopy(encoder_counts)
dist = [0.0, 0.0]
for i in range(0, 2):
dist[i] = ticks[i] / self._kinematic_parameters.ticks_per_meter
total_dist = (dist[Constants().WHEEL_LEFT] +
dist[Constants().WHEEL_RIGHT]) / 2.0
current_time = rospy.Time.now()
d_time = (current_time - self._last_enc_time).to_sec()
self._last_enc_time = current_time
# TODO find better what to determine going straight,
# this means slight deviation is accounted
if ticks[Constants().WHEEL_LEFT] == ticks[Constants().WHEEL_RIGHT]:
d_theta = 0.0
self._cur_pose.x += total_dist * cos(self._cur_pose.theta)
self._cur_pose.y += total_dist * sin(self._cur_pose.theta)
else:
d_theta = (dist[Constants().WHEEL_RIGHT] -
dist[Constants().WHEEL_LEFT]) / self.__tread
new_theta = self._cur_pose.theta + d_theta
radius = total_dist / d_theta
self._cur_pose.x += radius * \
(sin(new_theta) - sin(self._cur_pose.theta))
self._cur_pose.y -= radius * \
(cos(new_theta) - cos(self._cur_pose.theta))
self._cur_pose.theta = angles.normalize_angle(new_theta)
twist = Twist()
if abs(d_time) < 0.000001:
return twist
else:
twist.linear.x = total_dist / d_time
twist.angular.z = d_theta / d_time
return twist
def explore():
global turn, new_heading, turn_amt, avoid_obstacle, drd_heading, goal
global ear
pub = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size=1)
pub_hdg_setpoint = rospy.Publisher('/hdg/setpoint', Float64, queue_size=1)
pub_hdg_state = rospy.Publisher('/hdg/state', Float64, queue_size=1)
sub_imu = rospy.Subscriber('/mobile_base/sensors/imu_data',
Imu,
callback_imu,
queue_size=1)
sub_bumper = rospy.Subscriber('/mobile_base/events/bumper',
BumperEvent,
callback_bumper,
queue_size=1)
sub_hdg_pid = rospy.Subscriber('/hdg/control_effort',
Float64,
callback_hdg_pid,
queue_size=1)
sub_odom = rospy.Subscriber('/odom', Odometry, callback_odom, queue_size=1)
pub_log = rospy.Publisher('/my_turtle/log', String, queue_size=1)
sub_log = rospy.Subscriber('/my_turtle/log',
String,
callback_log,
queue_size=1)
rate = rospy.Rate(hz)
straight = Twist()
straight.linear.x = linear_vel
turn_left = Twist()
turn_left.angular.z = angular_vel
turn_left.linear.x = 0
turn_right = Twist()
turn_right.angular.z = -angular_vel
turn_right.linear.x = linear_vel #-linear_vel/10.0
stop = Twist()
stop.angular.z = 0
stop.linear.x = 0
x = 0
goal.x = 15
goal.y = 0
sound_intensity = 0
time.sleep(10)
stops = [3, 2.7, 2.4, 2.1, 1.8, 1.5, 1.2, 0.9, 0.6, 0.2]
pt = 0
stop_duration = 10
t_elapsed = 0
t = rospy.Time.now().to_sec()
saved = False #check if recording has been saved. Initially no, so use False
while not rospy.is_shutdown(): # and x < 10 * 60 * 4
if not ear.data is None and not ear.fft is None:
aa = ear.fft #[ear.fftx>=100]
# ww = ear.fftx[ear.fftx>=100]
# aa = aa[ww<=500]
# ww = ww[ww<=500]
# print aa,ww
sound_intensity = np.mean(aa) #aa[ww==500.]
# sound_intensity = sound_intensity[0]
sound_data.append(ear.streamdata)
# sound_intensity = np.mean(aa)
# print sound_intensity,aa[ww==500.]
x += 1
if random.random() < turn_prob and not turn:
turn = True
temp_heading = yaw + random.gauss(mu, sigma)
new_heading = angles.normalize_angle(temp_heading) #% 360
turn_amt = angles.shortest_angular_distance(
yaw, new_heading) # (new_heading - yaw) % 360
if avoid_obstacle and not turn:
if bumper.bumper == 0: #left obstacle
temp_heading = yaw - math.pi / 4.0
elif bumper.bumper == 2: #right obstacle
temp_heading = yaw + math.pi / 4.0
else: #front obstacle
temp_heading = yaw + random.random() * math.pi + math.pi / 2.0
print bumper.state, 'bumper', bumper.bumper, temp_heading
new_heading = angles.normalize_angle(temp_heading) #% 360
turn_amt = angles.shortest_angular_distance(
yaw, new_heading) # (new_heading - yaw) % 360
turn = True
avoid_obstacle = False
drd_heading = goal_direction(goal,
pose) #when going to a goal location
rvel = straight
if turn and False:
drd_heading = new_heading
t_amt = angles.shortest_angular_distance(yaw, new_heading)
if turn_amt < 0:
pub.publish(turn_right)
# print 'tr',yaw,new_heading,turn_amt
else:
pub.publish(turn_left)
# print 'tl',yaw,new_heading,turn_amt
if abs(t_amt) < tolerance:
turn = False
avoid_obstacle = False
else:
goal_d = goal_distance(goal, pose)
# rospy.loginfo("goal_d = %.2f, x = %.2f, y = %.2f, intensity = %.2f",goal_d,pose.x,pose.y,sound_intensity)
# print rospy.Time.now().to_sec()
standing_test = rospy.Time.now().to_sec() - t > 150
if goal_d < 0.1 or standing_test:
rvel = stop
ear.close()
if not saved:
waveFile = wave.open(sound_data_filename, 'wb')
waveFile.setnchannels(1)
waveFile.setsampwidth(
ear.p.get_sample_size(pyaudio.paInt16))
waveFile.setframerate(ear.rate)
waveFile.writeframes(b''.join(sound_data))
waveFile.close()
saved = True
else:
rot_vel = hdg_ctrl_effort * hdg_scale
if rot_vel > angular_vel: rot_vel = angular_vel
if rot_vel < -angular_vel: rot_vel = -angular_vel
rvel.angular.z = rot_vel
if avoid_obstacle:
rvel = stop
if t_elapsed >= stop_duration:
pt += 1
c = False
if pt < len(stops) and c:
print abs(goal_d - stops[pt]), t_elapsed
if abs(goal_d - stops[pt]) < 0.1:
rvel = stop
t_elapsed += 1.0 / hz
else:
t_elapsed = 0
else:
t_elapsed = 0
log = '{},{},{},{},{},{}'.format(rospy.Time.now().to_sec() - t,
goal.x - goal_d, pose.x, pose.y,
yaw, sound_intensity)
print rospy.Time.now().to_sec() - t, goal_d
if goal_d > 0.1 or standing_test:
pub_log.publish(log)
if standing_test:
rvel = stop
pub.publish(rvel)
# if rospy.Time.now().to_sec() - t < 90:
# turn_left.linear.x = 0
# turn_left.angular.z = 0.2
# pub.publish(turn_left)
# else:
# pub.publish(stop)
# ear.close()
# print 'straight',yaw,drd_heading,rot_vel
set_p = drd_heading
state_p = yaw
if set_p < 0:
set_p = set_p + math.pi * 2.0
if state_p < 0:
state_p = state_p + math.pi * 2.0
# print 'straight',state_p,set_p,rot_vel
pub_hdg_setpoint.publish(set_p)
pub_hdg_state.publish(state_p)
# print(yaw)
rate.sleep()
print("Quitting")
def test_normalize_angle(self, a):
a = a * np.pi
ref_r = normalize_angle(a)
self.assertAlmostEqual(speed_up_and_execute(spw.normalize_angle, [a]), ref_r, places=5)
def add_pitch(self, val):
self.mutex.acquire()
self.estimated_sensor_rotation_pitch = normalize_angle(
self.estimated_sensor_rotation_pitch + val)
self.mutex.release()
def test_normalize_angle(self):
self.assertAlmostEqual(0, normalize_angle(0))
self.assertAlmostEqual(pi, normalize_angle(pi))
self.assertAlmostEqual(0, normalize_angle(2 * pi))
self.assertAlmostEqual(pi, normalize_angle(3 * pi))
self.assertAlmostEqual(0, normalize_angle(4 * pi))
self.assertAlmostEqual(0, normalize_angle(-0))
self.assertAlmostEqual(pi, normalize_angle(-pi))
self.assertAlmostEqual(0, normalize_angle(-2 * pi))
self.assertAlmostEqual(pi, normalize_angle(-3 * pi))
self.assertAlmostEqual(0, normalize_angle(-4 * pi))
self.assertAlmostEqual(0, normalize_angle(-0))
self.assertAlmostEqual(-pi / 2, normalize_angle(-pi / 2))
self.assertAlmostEqual(pi, normalize_angle(-pi))
self.assertAlmostEqual(pi / 2, normalize_angle(-3 * pi / 2))
self.assertAlmostEqual(0, normalize_angle(-4 * pi / 2))
self.assertAlmostEqual(0, normalize_angle(0))
self.assertAlmostEqual(pi / 2, normalize_angle(pi / 2))
self.assertAlmostEqual(pi / 2, normalize_angle(5 * pi / 2))
self.assertAlmostEqual(pi / 2, normalize_angle(9 * pi / 2))
self.assertAlmostEqual(pi / 2, normalize_angle(-3 * pi / 2))
def add_roll(self, val):
self.mutex.acquire()
self.estimated_sensor_rotation_roll = normalize_angle(
self.estimated_sensor_rotation_roll + val)
self.mutex.release()
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def usage():
return "Usage: %s [desired x(m)] [desired y(m)] [desired yaw(deg)] \nFor example: ./walkTo.py 0.2, 0.2, 45" % \
sys.argv[0]
if __name__ == "__main__":
if len(sys.argv) == 4:
world_x_goal = float(sys.argv[1])
world_y_goal = float(sys.argv[2])
world_yaw_goal = np.deg2rad(float(sys.argv[3]))
world_yaw_goal = angles.normalize_angle(world_yaw_goal)
# generate desired frame wrt world
world_translation_goal = trans.translation_matrix(
np.array([world_x_goal, world_y_goal, 0.0]))
world_rotation_goal = trans.euler_matrix(0.0, 0.0, world_yaw_goal,
'sxyz')
world_T_goal = world_translation_goal.dot(world_rotation_goal)
# get walkman location wrt world
# walkman_state = get_model_state('walkman')
# walkman_state.pose.position.z = 0.0
# world_T_walkman = poseToMatrix(walkman_state.pose)
#
# world_P_walkman, world_Quaternion_walkman = poseToPositionQuaternion(walkman_state.pose)
# world_RPY_walkman = trans.euler_from_quaternion(world_Quaternion_walkman)
def _calc_heading_error(self, start_xy, next_xy, robot_heading):
path_delta = next_xy - start_xy
path_heading = np.arctan2(path_delta[1], path_delta[0])
return normalize_angle(path_heading-robot_heading)
def explore(self,sound=True):
pub = rospy.Publisher('/{}/mobile_base/commands/velocity'.format(self.robotID), Twist,queue_size=1)
pub_hdg_setpoint = rospy.Publisher('/{}/hdg/setpoint'.format(self.robotID),Float64,queue_size=1)
pub_hdg_state = rospy.Publisher('/{}/hdg/state'.format(self.robotID),Float64,queue_size=1)
sub_imu = rospy.Subscriber('/{}/mobile_base/sensors/imu_data'.format(self.robotID),Imu,self.callback_imu,queue_size=1)
sub_bumper = rospy.Subscriber('/{}/mobile_base/events/bumper'.format(self.robotID),BumperEvent,self.callback_bumper,queue_size=1)
sub_hdg_pid = rospy.Subscriber('/{}/hdg/control_effort'.format(self.robotID),Float64,self.callback_hdg_pid,queue_size=1)
sub_odom = rospy.Subscriber('/{}/odom'.format(self.robotID),Odometry,self.callback_odom,queue_size=1)
sub_experimentStart = rospy.Subscriber('/experimentStart',Bool,self.callback_experimentStart,queue_size=10)
pub_log = rospy.Publisher('/{}/log'.format(self.robotID),String,queue_size=5)
# sub_log = rospy.Subscriber('/nest_move/log',String,callback_log,queue_size=1)
rate = rospy.Rate(self.hz)
straight = Twist()
straight.linear.x = self.linear_vel
turn_left = Twist()
turn_left.angular.z = self.angular_vel
turn_left.linear.x = 0
turn_right = Twist()
turn_right.angular.z = -self.angular_vel
turn_right.linear.x = self.linear_vel#-linear_vel/10.0
stop = Twist()
stop.angular.z = 0
stop.linear.x = 0
x = 0
#start sound
if sound is not None:
#create thread for starting nest's speaker as it starts moving
if sound == 'white':
audio_gen = Audio_gen(filename=self.pkg_path+'White-noise-sound-20sec-mono-44100Hz.wav')
else:
audio_gen = Audio_gen(frequency = sound)
thread = Thread(target = audio_gen.send_to_speaker)
thread.start()
t_elapsed = 0
goal_d = self.goal_distance(self.goal,self.pose)
logheader = self.robotID + ':t,goal_d,x,y,yaw'
#pause for some seconds before starting motion
time.sleep(self.experimentWaitDuration)
while not self.experimentStart: #busy wait till experiment start is true
pub_log.publish(logheader)
rate.sleep()
t = rospy.Time.now().to_sec()
while not rospy.is_shutdown():# and x < 10 * 60 * 4
t_elapsed = rospy.Time.now().to_sec()-t
# print t_elapsed,goal_d,self.bumper.state
x +=1
rvel = straight#default is straight
if self.linear_vel > 0:
print self.linear_vel
#stop based on distance travelled
goal_d = self.goal_distance(self.goal,self.pose)
stopCondition = goal_d < 0.1 or self.pose.x > self.goal.x
else:
#stop based on experimet duration
stopCondition = self.experimentDuration < t_elapsed
if stopCondition:
#stop robot
rvel = stop
#close speaker
audio_gen.close_speaker()
break
else:
rot_vel = self.hdg_ctrl_effort * self.hdg_scale * 10
if rot_vel > self.angular_vel: rot_vel = self.angular_vel
if rot_vel < -self.angular_vel: rot_vel = -self.angular_vel
rvel.angular.z = rot_vel
#stop whenever an obstacle is hit
if self.avoid_obstacle:
rvel = stop
# pub_log.publish(log)
pub.publish(rvel)
#heading control
self.drd_heading = math.atan2(self.goal.y - self.pose.y, self.goal.x - self.pose.x)
set_p = self.drd_heading if self.drd_heading > 0 else 2 * math.pi + self.drd_heading
set_p = angles.normalize_angle(set_p)
state_p = self.yaw
if set_p < 0:
set_p = set_p + math.pi * 2.0
if state_p < 0:
state_p = state_p + math.pi * 2.0
# print 'straight',state_p,set_p,rot_vel
pub_hdg_setpoint.publish(set_p)
pub_hdg_state.publish(state_p)
# print(yaw)
log = '{}:{:.4f},{:.4f},{:.4f},{:.4f}'.format(self.robotID,goal_d,self.pose.x,self.pose.y,self.yaw)
pub_log.publish(log)
rospy.loginfo(str(t_elapsed) + ',' + log)
rate.sleep()
print("Quitting")
def explore():
global turn, new_heading, turn_amt, avoid_obstacle, drd_heading, goal
global ear
pub = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size=1)
pub_hdg_setpoint = rospy.Publisher('/hdg/setpoint', Float64, queue_size=1)
pub_hdg_state = rospy.Publisher('/hdg/state', Float64, queue_size=1)
sub_imu = rospy.Subscriber('/mobile_base/sensors/imu_data',
Imu,
callback_imu,
queue_size=1)
sub_bumper = rospy.Subscriber('/mobile_base/events/bumper',
BumperEvent,
callback_bumper,
queue_size=1)
sub_hdg_pid = rospy.Subscriber('/hdg/control_effort',
Float64,
callback_hdg_pid,
queue_size=1)
sub_odom = rospy.Subscriber('/odom', Odometry, callback_odom, queue_size=1)
pub_log = rospy.Publisher('/my_turtle/log', String, queue_size=1)
sub_log = rospy.Subscriber('/my_turtle/log',
String,
callback_log,
queue_size=1)
rate = rospy.Rate(hz)
straight = Twist()
straight.linear.x = linear_vel
turn_left = Twist()
turn_left.angular.z = angular_vel
turn_left.linear.x = -linear_vel / 10.0
turn_right = Twist()
turn_right.angular.z = -angular_vel
turn_right.linear.x = -linear_vel / 10.0
stop = Twist()
stop.angular.z = 0
stop.linear.x = 0
x = 0
# goal.x = 3
# goal.y = 0
sound_intensity = 0
# time.sleep(15)
while not rospy.is_shutdown(): # and x < 10 * 60 * 4
if not ear.data is None and not ear.fft is None:
aa = ear.fft[ear.fftx > (freq - 10)]
ww = ear.fftx[(ear.fftx > (freq - 10))]
aa = aa[ww < (freq + 10)]
ww = ww[ww < (freq + 10)]
sound_intensity = aa[0]
# print sound_intensity
x += 1
if random.random() < turn_prob and not turn:
turn = True
temp_heading = yaw + random.gauss(mu, sigma)
new_heading = angles.normalize_angle(temp_heading) #% 360
turn_amt = angles.shortest_angular_distance(
yaw, new_heading) # (new_heading - yaw) % 360
if avoid_obstacle and not turn:
if bumper.bumper == 0: #left obstacle
temp_heading = yaw - math.pi / 4.0
elif bumper.bumper == 2: #right obstacle
temp_heading = yaw + math.pi / 4.0
else: #front obstacle
temp_heading = yaw + random.random() * math.pi + math.pi / 2.0
print bumper.state, 'bumper', bumper.bumper, temp_heading
new_heading = angles.normalize_angle(temp_heading) #% 360
turn_amt = angles.shortest_angular_distance(
yaw, new_heading) # (new_heading - yaw) % 360
turn = True
avoid_obstacle = False
# drd_heading = goal_direction(goal,pose)#when going to a goal location
if turn:
drd_heading = new_heading
t_amt = angles.shortest_angular_distance(yaw, new_heading)
if turn_amt < 0:
pub.publish(turn_right)
# print 'tr',yaw,new_heading,turn_amt
else:
pub.publish(turn_left)
# print 'tl',yaw,new_heading,turn_amt
if abs(t_amt) < tolerance:
turn = False
avoid_obstacle = False
else:
# goal_d = goal_distance(goal,pose)
# rospy.loginfo("goal_d = %.2f, x = %.2f, y = %.2f, intensity = %.2f",goal_d,pose.x,pose.y,sound_intensity)
# print rospy.Time.now().to_sec()
# if goal_d < 0.1:
# straight.linear.x = 0
# straight.angular.z = 0
# else:
rot_vel = hdg_ctrl_effort * hdg_scale
if rot_vel > angular_vel: rot_vel = angular_vel
if rot_vel < -angular_vel: rot_vel = -angular_vel
straight.angular.z = rot_vel
# if avoid_obstacle:
# straight = stop
pub.publish(straight)
log = '{},{},{},{},{}'.format(rospy.Time.now().to_sec(), pose.x,
pose.y, yaw, sound_intensity)
pub_log.publish(log)
# print 'straight',yaw,drd_heading,rot_vel
set_p = drd_heading
state_p = yaw
if set_p < 0:
set_p = set_p + math.pi * 2.0
if state_p < 0:
state_p = state_p + math.pi * 2.0
# print 'straight',state_p,set_p,rot_vel
pub_hdg_setpoint.publish(set_p)
pub_hdg_state.publish(state_p)
# print(yaw)
rate.sleep()
print("Quitting")
def test_normalize_angle(self):
self.assertAlmostEqual(0, normalize_angle(0))
self.assertAlmostEqual(pi, normalize_angle(pi))
self.assertAlmostEqual(0, normalize_angle(2*pi))
self.assertAlmostEqual(pi, normalize_angle(3*pi))
self.assertAlmostEqual(0, normalize_angle(4*pi))
self.assertAlmostEqual(0, normalize_angle(-0))
self.assertAlmostEqual(pi, normalize_angle(-pi))
self.assertAlmostEqual(0, normalize_angle(-2*pi))
self.assertAlmostEqual(pi, normalize_angle(-3*pi))
self.assertAlmostEqual(0, normalize_angle(-4*pi))
self.assertAlmostEqual(0, normalize_angle(-0))
self.assertAlmostEqual(-pi/2, normalize_angle(-pi/2))
self.assertAlmostEqual(pi, normalize_angle(-pi))
self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
self.assertAlmostEqual(0, normalize_angle(-4*pi/2))
self.assertAlmostEqual(0, normalize_angle(0))
self.assertAlmostEqual(pi/2, normalize_angle(pi/2))
self.assertAlmostEqual(pi/2, normalize_angle(5*pi/2))
self.assertAlmostEqual(pi/2, normalize_angle(9*pi/2))
self.assertAlmostEqual(pi/2, normalize_angle(-3*pi/2))
def explore(self):
pub = rospy.Publisher('/{}/mobile_base/commands/velocity'.format(
self.robotID),
Twist,
queue_size=1)
pub_hdg_setpoint = rospy.Publisher('/{}/hdg/setpoint'.format(
self.robotID),
Float64,
queue_size=1)
pub_hdg_state = rospy.Publisher('/{}/hdg/state'.format(self.robotID),
Float64,
queue_size=1)
sub_imu = rospy.Subscriber('/{}/mobile_base/sensors/imu_data'.format(
self.robotID),
Imu,
self.callback_imu,
queue_size=1)
sub_bumper = rospy.Subscriber('/{}/mobile_base/events/bumper'.format(
self.robotID),
BumperEvent,
self.callback_bumper,
queue_size=1)
sub_hdg_pid = rospy.Subscriber('/{}/hdg/control_effort'.format(
self.robotID),
Float64,
self.callback_hdg_pid,
queue_size=1)
sub_odom = rospy.Subscriber('/{}/robot_pose_ekf/odom_combined'.format(
self.robotID),
PoseWithCovarianceStamped,
self.callback_odom,
queue_size=1)
pub_bump = rospy.Publisher('/{}/my_turtle/bump_info'.format(
self.robotID),
String,
queue_size=1)
sub_experimentStart = rospy.Subscriber('/experimentStart',
Bool,
self.callback_experimentStart,
queue_size=1)
pub_log = rospy.Publisher('/{}/log'.format(self.robotID),
String,
queue_size=5)
# sub_log = rospy.Subscriber('/my_turtle/log',String,self.callback_log,queue_size=1)
rate = rospy.Rate(self.hz) #looping rate
straight = Twist()
straight.angular.z = 0
straight.linear.x = self.linear_vel
reverse = Twist()
reverse.angular.z = 0
reverse.linear.x = -self.linear_vel
turn_left = Twist()
turn_left.angular.z = self.angular_vel
turn_left.linear.x = 0
turn_right = Twist()
turn_right.angular.z = -self.angular_vel
turn_right.linear.x = 0
stop = Twist()
stop.angular.z = 0
stop.linear.x = 0
x = 0
sound_intensity = 0
prev_sound = 0
curr_sound = 0
first_sound = True
revStart = None # for holding location reverse motion started
revD = 0.05 # reversing distance
self.turn_prob = self.base_prob # initial self.turn_prob is same as self.base_prob
logheader = self.robotID + ':t,x,y,yaw,prev_sound,curr_sound,turn_prob,acTion'
#pause for some seconds before starting motion
time.sleep(self.experimentWaitDuration)
#log = logheader
while not self.experimentStart: #busy wait till experiment start is true
pub_log.publish(logheader)
rate.sleep()
# mlist = []
# mAvg = 0
# m = 0
if self.ear != None:
self.ear.stream_start()
t = rospy.Time.now().to_sec()
while not rospy.is_shutdown(): # and x < 10 * 60 * 4
self.goal_d = np.Inf # initially set self.goal distance to be infinite to prevent stopping
if self.ear != None:
self.turn_prob, prev_sound, curr_sound = self.computeGrad(
self.turn_prob, prev_sound, curr_sound, self.turn
) #last values of soundIntensity and self.turn_prob
#chemotaxis should only be activated when sound intensity is below a threshold
if curr_sound > self.theta_A:
self.turn_prob = self.base_prob
else:
self.turn_prob = self.base_prob
x += 1
bound_check = 3
if not self.turn:
(bound_check, self.avoid_obstacle, bumpside,
bumpyaw) = self.wall.bumper_event(self.pose, self.yaw)
if self.bumper_event:
self.avoid_obstacle = self.bumper_event
bound_check = self.bumper
self.bumper_event = False
pub_bump.publish('bumper={},avoid={},{},({},{},{})'.format(
bound_check, self.avoid_obstacle, bumpside, self.pose.x,
self.pose.y, bumpyaw))
if self.goal != None and self.base_prob == 0: #if goal directed behaviour, go toward goal
t_amt = self.goToGoal()
elif rospy.Time.now().to_sec(
) - t >= self.expDuration: # if time limited experiment
# Experiment duration reached. Go self.home.
if self.goal == None: #if goal is not to return to a specific goal pose
break
else:
t_amt = self.goToGoal()
elif random.random(
) < self.turn_prob and not self.turn and not self.avoid_obstacle:
# perform random walk
self.turn = True
temp_heading = self.yaw + angles.normalize_angle(
random.gauss(self.mu, self.sigma))
self.new_heading = angles.normalize_angle(temp_heading) #% 360
# self.self.turn_amt = angles.shortest_angular_distance(self.yaw,self.new_heading)# (self.new_heading - self.yaw) % 360
# if self.avoid_obstacle or True:
# y = self.yaw / angles.pi * 180
# if y < 0:
# y = y + 360
# print bound_check,self.avoid_obstacle,self.pose.x,self.pose.y,y
if bound_check != 3 and self.avoid_obstacle and not self.turn:
#boundary obstacle avoidance behaviour
if bound_check == 0: #left obstacle
temp_heading = self.yaw - math.pi / 4.0
elif bound_check == 2: #right obstacle
temp_heading = self.yaw + math.pi / 4.0
else: #front obstacle
temp_heading = self.yaw + random.random(
) * math.pi + math.pi / 2.0
#print self.bumper.state,'self.bumper',self.bumper.self.bumper,temp_heading
self.new_heading = angles.normalize_angle(temp_heading) #% 360
self.turn_amt = angles.shortest_angular_distance(
self.yaw,
self.new_heading) # (self.new_heading - self.yaw) % 360
self.turn = True
self.reverseBool = True
revStart = self.pose
self.avoid_obstacle = False
# self.drd_heading = self.goal_direction(self.goal,self.pose)#when going to a self.goal location
acTion = 'straight'
if self.reverseBool and revStart != None:
acTion = 'reverse'
#reverse for at lease revD distance before self.turning
pub.publish(reverse)
if self.goal_distance(revStart, self.pose) >= revD:
revStart = None
self.reverseBool = False
elif self.turn:
self.drd_heading = self.new_heading
t_amt = angles.shortest_angular_distance(
self.yaw, self.new_heading)
if t_amt < 0:
acTion = 'turnRight'
pub.publish(turn_right)
# print 'tr',self.yaw,self.new_heading,self.self.turn_amt
else:
acTion = 'turnLeft'
pub.publish(turn_left)
# print 'tl',self.yaw,self.new_heading,self.self.turn_amt
if abs(t_amt) < self.tolerance:
self.turn = False
self.avoid_obstacle = False
elif self.goal_d > 0.1:
# self.goal_d = self.goal_distance(self.goal,self.pose)
# rospy.loginfo("self.goal_d = %.2f, x = %.2f, y = %.2f, intensity = %.2f",self.goal_d,self.pose.x,self.pose.y,sound_intensity)
# print rospy.Time.now().to_sec()
rot_vel = self.hdg_ctrl_effort * self.hdg_scale
if rot_vel > self.angular_vel: rot_vel = self.angular_vel
if rot_vel < -self.angular_vel: rot_vel = -self.angular_vel
straight.angular.z = rot_vel
# if self.avoid_obstacle:
# straight = stop
pub.publish(straight)
else:
#t_amt = self.turnToGoalYaw()
if self.goal_d <= 0.1 and abs(
t_amt) < self.tolerance and rospy.Time.now().to_sec(
) - t >= self.expDuration:
acTion = 'stop'
#stop if self.goal (i.e. self.home in go self.home behaviour) is reached
pub.publish(stop)
break
# print 'straight',self.yaw,self.drd_heading,rot_vel
set_p = self.drd_heading
state_p = self.yaw
if set_p < 0:
set_p = set_p + math.pi * 2.0
if state_p < 0:
state_p = state_p + math.pi * 2.0
# print 'straight',state_p,set_p,rot_vel
pub_hdg_setpoint.publish(set_p)
pub_hdg_state.publish(state_p)
# print(self.yaw)
log = '{}:{:.4f},{:.4f},{:.4f},{:.4f},{:.4f},{},{}'.format(
self.robotID, self.pose.x, self.pose.y, self.yaw, prev_sound,
curr_sound, self.turn_prob, acTion) #,m,mAvg)
pub_log.publish(log)
if self.new_comm_signal:
prev_sound = curr_sound # update sound intensity value
self.new_comm_signal = False
rospy.loginfo(log)
rate.sleep()
if self.ear != None:
self.ear.close()
print("Quitting")
def test_normalize_angle(self, a):
ref_r = normalize_angle(a)
self.assertAlmostEqual(w.compile_and_execute(w.normalize_angle, [a]), ref_r, places=5)
def main(self):
r = rospy.Rate(self.rate)
# wait for gazebo/get_model_state service to be available
rospy.wait_for_service('/gazebo/get_model_state')
evaluation_timeout = rospy.Time.now() + rospy.Duration.from_sec(
self.race_timeout)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
try:
get_model_state = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
robot_state = get_model_state(self.robotName, None)
except Exception as e:
print(e)
# get position
self.position = robot_state.pose.position
# calculate orientation
orientation_q = robot_state.pose.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z,
orientation_q.w
]
(roll, pitch, self.theta) = euler_from_quaternion(orientation_list)
if not self.race_started and not self.eval_finished:
# check initial point
if self.position.x < 0.001 and self.position.y < 0.001:
self.correct_position = True
elif self.correct_position:
self.start_time = current_time
self.race_started = True
if self.debug:
self.print_diagnostics('race_started', 'evaluator')
elif not self.correct_position:
self.disqualified = True
self.disqualification_reason = 'incorrect_start'
self.print_diagnostics('incorrect_start', 'evaluator')
elif not self.eval_finished:
# calculate sum_theta and sum_distance
self.sum_theta += math.fabs(
normalize_angle(self.theta - self.prev_theta))
self.sum_distance += math.sqrt((
(self.position.x - self.prev_position.x)**2) + (
(self.position.y - self.prev_position.y)**2))
# update position
self.current_tile = self.update_current_tile(
self.position, self.prev_position)
# state checking
if self.current_tile == -1:
self.disqualified = True
self.disqualification_reason = 'out_of_track'
self.print_diagnostics('out_of_track', 'evaluator')
elif self.current_tile != self.prev_tile:
# we check state transition by checking if prev_tile is -1
# robot could start with tile 1, but this would be disqualified before by previous checks
pt = self.tiles['track_%s' % self.prev_tile]
if self.prev_tile + 1 == self.current_tile:
# state transition between tiles occurs here
pt.lock_tile()
if self.debug:
self.print_tile_stats(pt, current_time)
elif self.prev_tile == (len(self.tiles) -
1) and self.current_tile == 0:
# race finished
pt.lock_tile()
self.finished_time = current_time
self.race_finished = True
if self.debug:
self.print_tile_stats(pt, current_time)
self.print_diagnostics('race_finished',
'evaluator')
else:
# wrong track, disqualified
self.disqualified = True
self.disqualification_reason = 'out_of_sequence'
self.print_diagnostics('out_of_sequence', 'evaluator')
# sum total area from tracks
self.get_sum_area()
# notice prev_position, and prev_tile must be recorded only when race has started
self.prev_position = self.position
self.prev_tile = self.current_tile
# notice prev theta should be recorded even if race has not started
self.prev_theta = self.theta
# publish evaluator data
self.publish_metrics(current_time)
# disqualification by timeout
if (evaluation_timeout < current_time) and not self.eval_finished:
self.disqualified = True
self.disqualification_reason = 'out_of_time'
self.print_diagnostics('out_of_time', 'evaluator')
# safety
self.display_metrics[
'disqualification_reason'] = self.disqualification_reason
# send result metrics
self.populate_result_metrics()
self.send_results(0.0, self.disqualified,
**self.result_metrics)
# print self metrics here, because will skip next statement because self.eval_finished will be true
rospy.loginfo(self.display_metrics)
self.eval_finished = True
# notice race could be finished, or disqualified here
if (self.race_finished
or self.disqualified) and not self.eval_finished:
# safety
self.display_metrics[
'disqualification_reason'] = self.disqualification_reason
if self.race_finished:
self.calculate_score(self.elapsed_time, self.sum_distance,
self.sum_area)
self.display_metrics['score'] = str(self.score)
# send result metrics
self.populate_result_metrics()
self.send_results(self.score, self.disqualified,
**self.result_metrics)
rospy.loginfo(self.display_metrics)
self.eval_finished = True
