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)