def heading_error(self, location, goal):
"""
return difference in heading between location and goal
"""
loc_head = quaternion_to_heading(location.pose.pose.orientation)
goal_head = quaternion_to_heading(goal.pose.pose.orientation)
return minimize_angle(loc_head - goal_head)
def cross_readings(self, old_reading, new_reading):
'''
Find the intersection of the two vectors defined by the position and
direction of the two readings.
Returns a tuple of the x, y pair where the two vectors intersect
This will return not None even if the vectors/half-lines don't intersect
See https://en.wikipedia.org/wiki/Line-line_intersection
Input:
(Odometry, Blob,) old_reading
(Odometry, Blob,) new_reading
Output:
(float, float)
'''
x1 = old_reading[0].pose.pose.position.x
y1 = old_reading[0].pose.pose.position.y
h1 = quaternion_to_heading(old_reading[0].pose.pose.orientation)
h1 = h1 + old_reading[1].bearing
x2 = x1 + cos(h1)
y2 = y1 + sin(h1)
x3 = new_reading[0].pose.pose.position.x
y3 = new_reading[0].pose.pose.position.y
h3 = quaternion_to_heading(new_reading[0].pose.pose.orientation)
h3 = h3 + new_reading[1].bearing
x4 = x3 + cos(h3)
y4 = y3 + sin(h3)
### temps
t0 = x1 * y2 - y1 * x2
t1 = x3 - x4
t2 = x1 - x2
t3 = x3 * y4 - x4 * y3
t4 = t2
t5 = y3 - y4
t6 = y1 - y2
t7 = t1
t8 = t0
t9 = t5
t10 = t6
t11 = t3
t12 = t4
t13 = t5
t14 = t6
t15 = t7
### end temps
if (t4 * t5 - t6 * t7) == 0 or (t12 * t13 - t14 * t15) == 0:
return None
return (
(t0 * t1 - t2 * t3) / (t4 * t5 - t6 * t7),
(t8 * t9 - t10 * t11) / (t12 * t13 - t14 * t15),
)
def cross_readings(self, old_reading, new_reading):
'''
Find the intersection of the two vectors defined by the position and
direction of the two readings.
Returns a tuple of the x, y pair where the two vectors intersect
This will return not None even if the vectors/half-lines don't intersect
See https://en.wikipedia.org/wiki/Line-line_intersection
Input:
(Odometry, Blob,) old_reading
(Odometry, Blob,) new_reading
Output:
(float, float)
'''
x1 = old_reading[0].pose.pose.position.x
y1 = old_reading[0].pose.pose.position.y
h1 = quaternion_to_heading(old_reading[0].pose.pose.orientation)
h1 = h1+old_reading[1].bearing
x2 = x1+cos(h1)
y2 = y1+sin(h1)
x3 = new_reading[0].pose.pose.position.x
y3 = new_reading[0].pose.pose.position.y
h3 = quaternion_to_heading(new_reading[0].pose.pose.orientation)
h3 = h3+new_reading[1].bearing
x4 = x3+cos(h3)
y4 = y3+sin(h3)
### temps
t0 = x1*y2-y1*x2
t1 = x3-x4
t2 = x1-x2
t3 = x3*y4-x4*y3
t4 = t2
t5 = y3-y4
t6 = y1-y2
t7 = t1
t8 = t0
t9 = t5
t10 = t6
t11 = t3
t12 = t4
t13 = t5
t14 = t6
t15 = t7
### end temps
if (t4*t5-t6*t7) == 0 or (t12*t13-t14*t15) == 0:
return None
return ((t0*t1-t2*t3)/(t4*t5-t6*t7), (t8*t9-t10*t11)/(t12*t13-t14*t15),)
def odom_transform_2d(self, data, new_frame, trans, rot):
# NOTES: only in 2d rotation
# also, it removes covariance, etc information
odom_new = Odometry()
odom_new.header = data.header
odom_new.header.frame_id = new_frame
odom_new.pose.pose.position.x = data.pose.pose.position.x + trans[0]
odom_new.pose.pose.position.y = data.pose.pose.position.y + trans[1]
odom_new.pose.pose.position.z = data.pose.pose.position.z + trans[2]
# rospy.loginfo('td: %s tr: %s' % (str(type(data)), str(type(rot))))
q = data.pose.pose.orientation
q_tuple = (q.x, q.y, q.z, q.w,)
# Quaternion(*(tft quaternion)) converts a tf-style quaternion to a ROS msg quaternion
odom_new.pose.pose.orientation = Quaternion(*tft.quaternion_multiply(q_tuple, rot))
heading_change = quaternion_to_heading(rot)
odom_new.twist.twist.linear.x = data.twist.twist.linear.x*math.cos(heading_change) - data.twist.twist.linear.y*math.sin(heading_change)
odom_new.twist.twist.linear.y = data.twist.twist.linear.y*math.cos(heading_change) + data.twist.twist.linear.x*math.sin(heading_change)
odom_new.twist.twist.linear.z = 0
odom_new.twist.twist.angular.x = 0
odom_new.twist.twist.angular.y = 0
odom_new.twist.twist.angular.z = data.twist.twist.angular.z
return odom_new
def summary(self):
'''
average x, y, heading
'''
x_sum = 0.0
y_sum = 0.0
heading_dx = 0.0
heading_dy = 0.0
count = float(len(self.particles))
for particle in self.particles:
if rospy.is_shutdown():
break
x_sum += float(particle.state.pose.pose.position.x)
y_sum += float(particle.state.pose.pose.position.y)
heading_t = float(
quaternion_to_heading(particle.state.pose.pose.orientation))
heading_dx += cos(heading_t)
heading_dy += sin(heading_t)
x = x_sum / count
y = y_sum / count
heading = math.atan2(heading_dy, heading_dx)
return (
x,
y,
heading,
)
def reading_distance_function(self, state1, blob1, state2, blob2):
'''
Find a distance between two state-blob rays with colors. If they rays do
not intersect, the distance is infinite. Otherwise, it is the distance
between two colors.
'''
x1 = state1.pose.pose.position.x
y1 = state1.pose.pose.position.y
b1 = blob1.bearing + quaternion_to_heading(state1.pose.pose.orientation)
x2 = state2.pose.pose.position.x
y2 = state2.pose.pose.position.y
b2 = blob2.bearing + quaternion_to_heading(state2.pose.pose.orientation)
if not self.ray_intersect(x1, y1, b1, x2, y2, b2):
return float('inf')
return self.color_distance(blob1, blob2)
def transform_obstacles(self, pose):
# take a list of r-theta obstacles and convert them to x-y, so they can
# be used for any position instead of just the last odom
heading = quaternion_to_heading(pose.orientation)
for i in xrange(0, len(self.obstacles)):
old_obstacle = self.obstacles[i]
x = pose.position.x + old_obstacle[0]*cos(old_obstacle[1]+heading)
y = pose.position.y + old_obstacle[0]*sin(old_obstacle[1]+heading)
self.obstacles[i] = (x,y,)
def __init__(self, odom):
self.x = odom.pose.pose.position.x
self.y = odom.pose.pose.position.y
self.theta = quaternion_to_heading(odom.pose.pose.orientation)
self.v = odom.twist.twist.linear.x
self.w = odom.twist.twist.angular.z
self.a = 0
self.alpha = 0
self.frame_id = odom.header.frame_id
def reading_distance_function(self, state1, blob1, state2, blob2):
'''
Find a distance between two state-blob rays with colors. If they rays do
not intersect, the distance is infinite. Otherwise, it is the distance
between two colors.
'''
x1 = state1.pose.pose.position.x
y1 = state1.pose.pose.position.y
b1 = blob1.bearing + quaternion_to_heading(
state1.pose.pose.orientation)
x2 = state2.pose.pose.position.x
y2 = state2.pose.pose.position.y
b2 = blob2.bearing + quaternion_to_heading(
state2.pose.pose.orientation)
if not self.ray_intersect(x1, y1, b1, x2, y2, b2):
return float('inf')
return self.color_distance(blob1, blob2)
def depart_vector(self, start, unused_end, current):
# axis direction
axis_direction = quaternion_to_heading(start.pose.pose.orientation)
# correction to move away from axis
errors = calc_errors(current, start)
off_axis = errors[1]
heading_correction = math.atan(1.5*off_axis)
final_direction = axis_direction+heading_correction
return (math.cos(final_direction), math.sin(final_direction), 0,)
def off_axis_error(self, location, goal):
"""
calc error normal to axis defined by the goal position and direction
input: two nav_msgs.msg.Odometry, current best location estimate and
goal
output: double distance along the axis
axis is defined by a vector from the unit circle aligned with the goal
heading
relative position is the vector from the goal x, y to the location x, y
distance is defined by subtracting the parallel vector from the total
relative position vector
example use:
see calc_errors above
"""
relative_position_x = (location.pose.pose.position.x -
goal.pose.pose.position.x)
relative_position_y = (location.pose.pose.position.y -
goal.pose.pose.position.y)
# relative position of the best estimate position and the goal
# vector points from the goal to the location
## relative_position = (relative_position_x, relative_position_y, 0.0)
goal_heading = quaternion_to_heading(goal.pose.pose.orientation)
goal_vector_x = math.cos(goal_heading)
goal_vector_y = math.sin(goal_heading)
# vector in the direction of the goal heading, axis of desired motion
goal_vector = (goal_vector_x, goal_vector_y, 0.0)
along_axis_error = self.along_axis_error(location, goal)
along_axis_vec = scale(unit(goal_vector), along_axis_error)
new_rel_x = relative_position_x - along_axis_vec[0]
new_rel_y = relative_position_y - along_axis_vec[1]
new_rel_vec = (new_rel_x, new_rel_y, 0.0)
error_magnitude = math.sqrt(new_rel_x*new_rel_x +
new_rel_y*new_rel_y)
if error_magnitude < .0001:
return 0.0
if cross_product(goal_vector, new_rel_vec)[2] >= 0.0:
return error_magnitude
else:
return -error_magnitude
def new_scan(self, from_pose, scan):
if self.RRT_VIS is None:
self.RRT_VIS = rospy.Publisher('/rrt/visual', Odometry, queue_size=1)
# add in all the new obstacles
# rospy.loginfo('new scan')
angle = scan.angle_min+quaternion_to_heading(from_pose.orientation)
# rospy.loginfo('begin rotating through scan.ranges %d' % len(scan.ranges))
# rospy.loginfo('pose x: %f y: %f' % (from_pose.position.x, from_pose.position.y,))
count = 0
for reading in scan.ranges:
count += 1
if count % 10 == 0 or count > 90:
# rospy.loginfo('scan range %d %f' % (count, reading,))
pass
# add a new obstacle
if reading > scan.range_max - .01:
# rospy.loginfo('scan max!')
continue
if reading < scan.range_min + .01:
# rospy.loginfo('scan min!')
continue
x = from_pose.position.x + reading*cos(angle)
y = from_pose.position.x + reading*sin(angle)
radius = reading*scan.angle_increment
new_pose = Pose()
new_pose.position.x = x
new_pose.position.y = y
# rospy.loginfo('new obstacle: x: %f y: %f' % (x, y,))
self.obstacles.add_obstacle(deepcopy(new_pose), radius)
# rospy.loginfo('end adding obstacle')
# check if I need to prune
try:
self.prune_local(new_pose, radius)
except KeyError as ke:
pass
# rospy.loginfo('start prune local')
angle += scan.angle_increment
# check all of the points, especially nodes that might have edges
# passing by new obstacles
# rospy.loginfo('prune recursive')
self.prune_recursive()
def arrive_vector(self, unused_start, end, current):
# axis direction
axis_direction = minimize_angle(quaternion_to_heading(end.pose.pose.orientation))
# correction to move away from axis
errors = calc_errors(current, end)
off_axis = errors[1]
heading_correction = minimize_angle(math.atan(-1.5*off_axis))
final_direction = minimize_angle(axis_direction+heading_correction)
# rospy.loginfo('avr: axis '+str(axis_direction)[0:4]+' corr '+str(heading_correction)[0:4]+' off '+str(off_axis)[0:5]+' fina '+str(final_direction)[0:5])
return (math.cos(final_direction), math.sin(final_direction), 0,)
def motion_model(particle, twist, dt):
# pS h1 |
# 0-----------------0
# | h2
v = twist.linear.x
w = twist.angular.z
# new_particle = FilterParticle()
new_particle = copy_module.deepcopy(particle)
print(new_particle.state.pose.pose.position.y)
print(particle.state.pose.pose.position.y)
dheading = twist.angular.z * dt
drive_noise = normal(0, abs(.005 * v) + abs(.001 * w) + .0001, 1)
print('original ' + str(twist.linear.x * dt))
print('drive noise ' + str(drive_noise))
ds = twist.linear.x * dt + drive_noise
print('and now, ds ' + str(ds))
prev_heading = quaternion_to_heading(particle.state.pose.pose.orientation)
print('prev_heading ' + str(prev_heading))
heading_noise = normal(0, abs(.005 * w) + abs(.001 * v) + .0001, 1)
heading_1 = prev_heading + dheading / 2 + heading_noise
print('heading_1 ' + str(heading_1))
print('heading noise ' + str(heading_noise))
heading_noise = normal(0, abs(.005 * w) + abs(.001 * v) + .0001, 1)
heading_2 = heading_1 + dheading / 2 + heading_noise
print('heading_2 ' + str(heading_1))
print('heading noise ' + str(heading_noise))
dx = ds * cos(heading_1)
dy = ds * sin(heading_1)
new_particle.state.pose.pose.position.x += dx
new_particle.state.pose.pose.position.y += dy
# pylint: disable=line-too-long
new_particle.state.pose.pose.orientation = heading_to_quaternion(heading_2)
return new_particle
def motion_model(particle, twist, dt):
# pS h1 |
# 0-----------------0
# | h2
v = twist.linear.x
w = twist.angular.z
# new_particle = FilterParticle()
new_particle = copy_module.deepcopy(particle)
print(new_particle.state.pose.pose.position.y)
print(particle.state.pose.pose.position.y)
dheading = twist.angular.z * dt
drive_noise = normal(0, abs(.005*v)+abs(.001*w)+.0001, 1)
print('original '+str(twist.linear.x * dt))
print('drive noise '+str(drive_noise))
ds = twist.linear.x * dt + drive_noise
print('and now, ds '+str(ds))
prev_heading = quaternion_to_heading(particle.state.pose.pose.orientation)
print('prev_heading '+str(prev_heading))
heading_noise = normal(0, abs(.005*w)+abs(.001*v)+.0001, 1)
heading_1 = prev_heading+dheading/2+heading_noise
print('heading_1 '+str(heading_1))
print('heading noise '+str(heading_noise))
heading_noise = normal(0, abs(.005*w)+abs(.001*v)+.0001, 1)
heading_2 = heading_1+dheading/2+heading_noise
print('heading_2 '+str(heading_1))
print('heading noise '+str(heading_noise))
dx = ds*cos(heading_1)
dy = ds*sin(heading_1)
new_particle.state.pose.pose.position.x += dx
new_particle.state.pose.pose.position.y += dy
# pylint: disable=line-too-long
new_particle.state.pose.pose.orientation = heading_to_quaternion(heading_2)
return new_particle
def process_position(self, odom):
odom.header.frame_id = 'map'
self.original_odom.publish(odom)
# print('add noise')
# noise = np.random.normal(0,1)
## noise params ##
# pose.pose.position.[x,y] 2
# pose.pose.orientation.[x,y,z,w] (heading) 3
# twist.twist.linear.[x] 4
# twist.twist.angular.[z] 5
x = odom.pose.pose.position.x
y = odom.pose.pose.position.y
v = abs(odom.twist.twist.linear.x)
w = abs(odom.twist.twist.angular.z)
odom.pose.pose.position.x += noise(0,self.position_variation) # 1
odom.pose.pose.position.y += noise(0,self.position_variation) # 2
if odom.pose.pose.position.z: # if it is not 0
odom.pose.pose.position.z += noise(0,self.position_variation)
heading = quaternion_to_heading(odom.pose.pose.orientation)
heading += noise(0,self.heading_variation+.05*w+.01*v)
odom.pose.pose.orientation = heading_to_quaternion(heading) # 3
odom.twist.twist.linear.x += noise(0,self.linear_vel+.05*v+.01*w) # 4
if odom.twist.twist.linear.y: # if it is not 0
odom.twist.twist.linear.y += noise(0,self.linear_vel)
if odom.twist.twist.linear.z: # if it is not 0
odom.twist.twist.linear.z += noise(0,self.linear_vel)
odom.twist.twist.angular.z += noise(0,self.angular_vel+.05*w+.01*v) # 5
if odom.twist.twist.angular.x: # if it is not 0
odom.twist.twist.angular.x += noise(0,self.angular_vel)
if odom.twist.twist.angular.y: # if it is not 0
odom.twist.twist.angular.y += noise(0,self.angular_vel)
self.noisy_odom.publish(odom)
def motion_model(self, particle, twist, dt):
# pS h1 |
# 0-----------------0
# | h2
# dt = dt.secs + dt.nsecs*math.pow(10,-9)
# rospy.loginfo('dt secs: '+str(dt.to_sec()))
dt = dt.to_sec()
v = twist.linear.x
w = twist.angular.z
new_particle = FilterParticle()
new_particle = copy_module.deepcopy(particle)
dheading = twist.angular.z * dt
drive_noise = normal(0, abs(.05 * v) + abs(.005 * w) + .0005, 1)
ds = twist.linear.x * dt + drive_noise
prev_heading = quaternion_to_heading(
particle.state.pose.pose.orientation)
heading_noise = normal(0, abs(.025 * w) + abs(.005 * v) + .0005, 1)
heading_1 = prev_heading + dheading / 2 + heading_noise
heading_noise = normal(0, abs(.025 * w) + abs(.005 * v) + .0005, 1)
heading_2 = heading_1 + dheading / 2 + heading_noise
# rospy.loginfo('asdf;kjasdf; '+str(heading_1 ))
dx = ds * cos(heading_1)
dy = ds * sin(heading_1)
# rospy.loginfo('ds delta: '+str(ds - v*dt))
new_particle.state.pose.pose.position.x += dx
new_particle.state.pose.pose.position.y += dy
# pylint: disable=line-too-long
new_particle.state.pose.pose.orientation = heading_to_quaternion(
heading_2)
return new_particle
def motion_model(self, particle, twist, dt):
# pS h1 |
# 0-----------------0
# | h2
# dt = dt.secs + dt.nsecs*math.pow(10,-9)
# rospy.loginfo('dt secs: '+str(dt.to_sec()))
dt = dt.to_sec()
v = twist.linear.x
w = twist.angular.z
new_particle = FilterParticle()
new_particle = copy_module.deepcopy(particle)
dheading = twist.angular.z * dt
drive_noise = normal(0, abs(.05*v)+abs(.005*w)+.0005, 1)
ds = twist.linear.x * dt + drive_noise
prev_heading = quaternion_to_heading(particle.state.pose.pose.orientation)
heading_noise = normal(0, abs(.025*w)+abs(.005*v)+.0005, 1)
heading_1 = prev_heading+dheading/2+heading_noise
heading_noise = normal(0, abs(.025*w)+abs(.005*v)+.0005, 1)
heading_2 = heading_1+dheading/2+heading_noise
# rospy.loginfo('asdf;kjasdf; '+str(heading_1 ))
dx = ds*cos(heading_1)
dy = ds*sin(heading_1)
# rospy.loginfo('ds delta: '+str(ds - v*dt))
new_particle.state.pose.pose.position.x += dx
new_particle.state.pose.pose.position.y += dy
# pylint: disable=line-too-long
new_particle.state.pose.pose.orientation = heading_to_quaternion(heading_2)
return new_particle
def motion_model_noisy(self, previous_state, twist, dt):
'''
A method that returns a noisy (default) version of the motion model for
evolving a previous state based on a twist and a time step
Inputs:
previous_state - ROS Odometry-like
twist - ROS Twist message
dt - a time delta
Outputs:
next_state - ROS Odometry-like
'''
# pS h1 |
# 0-----------------0
# | h2
dheading = twist.twist.angular.z * dt
drive_noise = normal(0, .05, 1)
ds = twist.twist.linear.x * dt + drive_noise
# prev_x = previous_state.pose.pose.position.x
# prev_y = previous_state.pose.pose.position.y
# pylint: disable=line-too-long
prev_heading = quaternion_to_heading(
previous_state.pose.pose.orientation)
heading_noise = normal(0, .05, 1)
heading_1 = prev_heading + dheading / 2 + heading_noise
heading_noise = normal(0, .05, 1)
heading_2 = heading_1 + dheading / 2 + heading_noise
dx = ds * math.cos(heading_1)
dy = ds * math.cos(heading_1)
previous_state.pose.pose.position.x += dx
previous_state.pose.pose.position.y += dy
previous_state.pose.pose.orientation = heading_to_quaternion(heading_2)
return previous_state
def motion_model_noisy(self, previous_state, twist, dt):
'''
A method that returns a noisy (default) version of the motion model for
evolving a previous state based on a twist and a time step
Inputs:
previous_state - ROS Odometry-like
twist - ROS Twist message
dt - a time delta
Outputs:
next_state - ROS Odometry-like
'''
# pS h1 |
# 0-----------------0
# | h2
dheading = twist.twist.angular.z * dt
drive_noise = normal(0, .05, 1)
ds = twist.twist.linear.x * dt + drive_noise
# prev_x = previous_state.pose.pose.position.x
# prev_y = previous_state.pose.pose.position.y
# pylint: disable=line-too-long
prev_heading = quaternion_to_heading(previous_state.pose.pose.orientation)
heading_noise = normal(0, .05, 1)
heading_1 = prev_heading+dheading/2+heading_noise
heading_noise = normal(0, .05, 1)
heading_2 = heading_1+dheading/2+heading_noise
dx = ds*math.cos(heading_1)
dy = ds*math.cos(heading_1)
previous_state.pose.pose.position.x += dx
previous_state.pose.pose.position.y += dy
previous_state.pose.pose.orientation = heading_to_quaternion(heading_2)
return previous_state
def along_axis_error(self, location, goal):
"""
calc error along the axis defined by the goal position and direction
input: two nav_msgs.msg.Odometry, current best location estimate + goal
output: double distance along the axis
axis is defined by a vector from the unit circle aligned with the goal
heading
relative position is the vector from the goal x, y to the location x, y
distance is defined by the dot product
example use:
see calc_errors above
"""
relative_position_x = (goal.pose.pose.position.x -
location.pose.pose.position.x)
relative_position_y = (goal.pose.pose.position.y -
location.pose.pose.position.y)
# relative position of the best estimate position and the goal
# vector points from the goal to the location
relative_position = (relative_position_x, relative_position_y, 0.0)
goal_heading = quaternion_to_heading(goal.pose.pose.orientation)
goal_vector_x = math.cos(goal_heading)
goal_vector_y = math.sin(goal_heading)
# vector in the direction of the goal heading, axis of desired motion
goal_vector = (goal_vector_x, goal_vector_y, 0.0)
val = dot_product(relative_position, goal_vector)
if abs(val) < .0001:
return 0.0
return -val
def summary(self):
'''
average x, y, heading
'''
x_sum = 0.0
y_sum = 0.0
heading_dx = 0.0
heading_dy = 0.0
count = float(len(self.particles))
for particle in self.particles:
if rospy.is_shutdown():
break
x_sum += float(particle.state.pose.pose.position.x)
y_sum += float(particle.state.pose.pose.position.y)
heading_t = float(quaternion_to_heading(particle.state.pose.pose.orientation))
heading_dx += cos(heading_t)
heading_dy += sin(heading_t)
x = x_sum / count
y = y_sum / count
heading = math.atan2(heading_dy, heading_dx)
return (x, y, heading,)
def probability_of_match(self, state, blob, feature):
'''
Calculate the probability that a state and a blob observation match a
given feature
Input:
Odometry state
Blob blob
Feature feature
Output:
float (probability)
'''
f_mean = feature.mean # [x, y, r, b, g]
f_covar = feature.covar
f_x = f_mean[0]
f_y = f_mean[1]
s_x = state.pose.pose.position.x
s_y = state.pose.pose.position.y
s_heading = quaternion_to_heading(state.pose.pose.orientation)
# rospy.loginfo('atan2(%f - %f, %f - %f) - %f' % (f_y, s_y, f_x, s_x, s_heading,))
expected_bearing = math.atan2(f_y - s_y, f_x - s_x) - s_heading
observed_bearing = blob.bearing
# rospy.loginfo('%f vs. %f' % (expected_bearing, observed_bearing,))
# rospy.loginfo('feature mean: %s' % str(f_mean))
# rospy.loginfo('state (%f, %f, %f,) and blob %f, size(%f)' % (s_x, s_y, s_heading, observed_bearing, blob.size))
del_bearing = observed_bearing - expected_bearing
# while(del_bearing > math.pi*2):
# del_bearing = del_bearing - math.pi*2
# while(del_bearing < math.pi*-2):
# del_bearing = del_bearing + math.pi*2
# if(del_bearing > math.pi):
# del_bearing = math.pi*-2 + del_bearing
# if(del_bearing < -math.pi):
# del_bearing = math.pi*2 + del_bearing
color_distance = (math.pow(blob.color.r - f_mean[2], 2) +
math.pow(blob.color.g - f_mean[3], 2) +
math.pow(blob.color.b - f_mean[4], 2))
# add the 500s to boost small numbers away from 0. Everything gets the
# multiple, but it should make the multiplication of the two numbers
# less likely to hit 0 unless one of them really is 0
if abs(del_bearing) > 0.5:
# rospy.loginfo('color distance was ... %f' % color_distance)
# rospy.loginfo('bearing exit')
return 0.0
else:
# pylint: disable=line-too-long
bearing_prob = 500.0 * self.prob_position_match(
f_mean, f_covar, s_x, s_y, observed_bearing)
if abs(color_distance) > 300:
# rospy.loginfo('%d %d %d | %d %d %d' % (blob.color.r, blob.color.g, blob.color.b, f_mean[2], f_mean[3], f_mean[4]))
# rospy.loginfo('color exit')
return 0.0
else:
color_prob = 500.0 * self.prob_color_match(f_mean, f_covar, blob)
# rospy.loginfo('bp: %f' % bearing_prob)
# rospy.loginfo('cp: %f' % color_prob)
if not isinstance(bearing_prob, float):
rospy.loginfo('type(bearing_prob) %s' % str(type(bearing_prob)))
if not isinstance(color_prob, float):
rospy.loginfo('type(color_prob) %s' % str(type(color_prob)))
return bearing_prob * color_prob / (250000.0)
def odom_cb(odom):
global start
global count
global goals
if start is None:
start = odom.pose.pose
return
positions[0] = odom
if len(goals) <= 0:
# set speed to 0 and return
if DRIVER is not None:
if distance(end, odom.pose.pose) > .01:
rospy.loginfo('Driver: get a new set of goals')
if rrt:
DRIVER.publish(Twist())
rospy.wait_for_service('/rrt/reset')
reset_root(positions[0].pose.pose, end)
goals = get_plan(odom.pose.pose, end).allpoints
if (len(goals) == 0):
rospy.loginfo('Driver: arrived at goal')
DRIVER.publish(Twist())
global crash_flag
crash_flag = True
else:
for pose in goals:
ps = PoseStamped()
ps.pose = pose
ps.header.frame_id = '/odom'
if PATHVIZ is not None:
PATHVIZ.publish(ps)
else:
rospy.loginfo('Driver: empty goals list')
DRIVER.publish(Twist())
goals = get_plan(odom.pose.pose, end).allpoints
if (len(goals) == 0):
rospy.loginfo('Driver: arrived at goal')
DRIVER.publish(Twist())
sys.exit(0)
return
elif distance(odom.pose.pose, goals[0]) < .05:
# if I'm on the goal, remove the current goal, set the speed to 0
goals.pop(0)
count += 1
if DRIVER is not None:
# rospy.loginfo('Driver: next goal')
DRIVER.publish(Twist())
return
else:
# find the deltas between my position and the angle to the next goal
# drive towards the goal position
# rospy.loginfo('odom pose: %s', odom.pose.pose)
hz = 10.0
dt = 1.0/hz
rospy.loginfo('Driver: moving on to %d/%d' % (count, len(goals),))
t = Twist()
current_position = odom.pose.pose
next_ = goals[0]
t.angular.z = 0.0
dx = next_.position.x - current_position.position.x
dy = next_.position.y - current_position.position.y
goal_direction = math.atan2(dy, dx)
current_direction = quaternion_to_heading(current_position.orientation)
dtheta = goal_direction - current_direction
dtheta = minimize(dtheta)
t.angular.z = dtheta/(2.0*dt)
if t.angular.z > 0.25:
t.angular.z = 0.25
if t.angular.z < -0.25:
t.angular.z = -0.25
t.linear.x = 0.0
rospy.loginfo('dtheta %f if.' % (dtheta,))
if abs(dtheta) < .01:
# I'm pointing in the right direction, go forward
dist = distance(next_, end)
t.linear.x = 0.2 + dist*dist
rospy.loginfo('if. dist %f' % (t.linear.x,))
if t.linear.x > 0.5:
t.linear.x = 0.5
if t.linear.x < -0.5:
t.linear.x = -0.5
rospy.loginfo('D.p(%f,%f)' % (t.linear.x, t.angular.z,))
DRIVER.publish(t)
def odom_cb(odom):
global start
global count
global goals
if start is None:
start = odom.pose.pose
return
positions[0] = odom
if len(goals) <= 0:
# set speed to 0 and return
if DRIVER is not None:
if distance(end, odom.pose.pose) > .01:
rospy.loginfo('Driver: get a new set of goals')
goals = get_plan(odom.pose.pose, end).allpoints
if (len(goals) == 0):
rospy.loginfo('Driver: arrived at goal')
DRIVER.publish(Twist())
global crash_flag
crash_flag = True
else:
rospy.loginfo('Driver: empty goals list')
DRIVER.publish(Twist())
goals = get_plan(odom.pose.pose, end).allpoints
if (len(goals) == 0):
rospy.loginfo('Driver: arrived at goal')
DRIVER.publish(Twist())
import sys
sys.exit(0)
return
elif distance(odom.pose.pose, goals[0]) < .02:
# if I'm on the goal, remove the current goal, set the speed to 0
to_print = goals.pop(0)
count += 1
# rospy.loginfo('@ '+str(to_print))
if DRIVER is not None:
# rospy.loginfo('Driver: next goal')
DRIVER.publish(Twist())
return
else:
# find the deltas between my position and the angle to the next goal
# drive towards the goal position
# rospy.loginfo('odom pose: %s', odom.pose.pose)
hz = 10.0
dt = 1.0/hz
rospy.loginfo('Driver: moving on to %d/%d' % (count, len(goals),))
t = Twist()
current_position = odom.pose.pose
next_ = goals[0]
t.angular.z = 0.0
dx = next_.position.x - current_position.position.x
dy = next_.position.y - current_position.position.y
goal_direction = math.atan2(dy, dx)
current_direction = quaternion_to_heading(current_position.orientation)
dtheta = goal_direction - current_direction
while dtheta >= 2*math.pi:
dtheta = dtheta - 2*math.pi
while dtheta <= -2*math.pi:
dtheta = dtheta + 2*math.pi
if dtheta > math.pi:
dtheta = -2*math.pi + dtheta
if dtheta < -math.pi:
dtheta = 2*math.pi + dtheta
t.angular.z = dtheta/(2.0*dt)
if t.angular.z > 0.25:
t.angular.z = 0.25
if t.angular.z < -0.25:
t.angular.z = -0.25
t.linear.x = 0.0
rospy.loginfo('dtheta %f if.' % (dtheta,))
if abs(dtheta) < .01:
# I'm pointing in the right direction, go forward
dist = distance(next_, end)
t.linear.x = 0.2 + dist*dist
rospy.loginfo('if. dist %f' % (t.linear.x,))
if t.linear.x > 0.5:
t.linear.x = 0.5
if t.linear.x < -0.5:
t.linear.x = -0.5
rospy.loginfo('D.p(%f,%f)' % (t.linear.x, t.angular.z,))
DRIVER.publish(t)
def probability_of_match(self, state, blob, feature):
'''
Calculate the probability that a state and a blob observation match a
given feature
Input:
Odometry state
Blob blob
Feature feature
Output:
float (probability)
'''
f_mean = feature.mean # [x, y, r, b, g]
f_covar = feature.covar
f_x = f_mean[0]
f_y = f_mean[1]
s_x = state.pose.pose.position.x
s_y = state.pose.pose.position.y
s_heading = quaternion_to_heading(state.pose.pose.orientation)
# rospy.loginfo('atan2(%f - %f, %f - %f) - %f' % (f_y, s_y, f_x, s_x, s_heading,))
expected_bearing = math.atan2(f_y-s_y, f_x-s_x) - s_heading
observed_bearing = blob.bearing
# rospy.loginfo('%f vs. %f' % (expected_bearing, observed_bearing,))
# rospy.loginfo('feature mean: %s' % str(f_mean))
# rospy.loginfo('state (%f, %f, %f,) and blob %f, size(%f)' % (s_x, s_y, s_heading, observed_bearing, blob.size))
del_bearing = observed_bearing - expected_bearing
# while(del_bearing > math.pi*2):
# del_bearing = del_bearing - math.pi*2
# while(del_bearing < math.pi*-2):
# del_bearing = del_bearing + math.pi*2
# if(del_bearing > math.pi):
# del_bearing = math.pi*-2 + del_bearing
# if(del_bearing < -math.pi):
# del_bearing = math.pi*2 + del_bearing
color_distance = (math.pow(blob.color.r - f_mean[2], 2) +
math.pow(blob.color.g - f_mean[3], 2) +
math.pow(blob.color.b - f_mean[4], 2))
# add the 500s to boost small numbers away from 0. Everything gets the
# multiple, but it should make the multiplication of the two numbers
# less likely to hit 0 unless one of them really is 0
if abs(del_bearing) > 0.5:
# rospy.loginfo('color distance was ... %f' % color_distance)
# rospy.loginfo('bearing exit')
return 0.0
else:
# pylint: disable=line-too-long
bearing_prob = 500.0*self.prob_position_match(f_mean, f_covar, s_x, s_y, observed_bearing)
if abs(color_distance) > 300:
# rospy.loginfo('%d %d %d | %d %d %d' % (blob.color.r, blob.color.g, blob.color.b, f_mean[2], f_mean[3], f_mean[4]))
# rospy.loginfo('color exit')
return 0.0
else:
color_prob = 500.0*self.prob_color_match(f_mean, f_covar, blob)
# rospy.loginfo('bp: %f' % bearing_prob)
# rospy.loginfo('cp: %f' % color_prob)
if not isinstance(bearing_prob, float):
rospy.loginfo('type(bearing_prob) %s' % str(type(bearing_prob)))
if not isinstance(color_prob, float):
rospy.loginfo('type(color_prob) %s' % str(type(color_prob)))
return bearing_prob*color_prob / (250000.0)
