def distance_correction(lon_now, lat_now, bearing_now, lon_target, lat_target,
bearing_target, correction_type):
distance = gpsmath.haversine(lon_now, lat_now, lon_target, lat_target)
bearing = gpsmath.bearing(lon_now, lat_now, lon_target, lat_target)
# check the bearing now and bearing target
rospy.loginfo("Correction Type: %s", correction_type)
rospy.loginfo("GPS now [%f, %f], GPS target: [%f, %f]", lon_now, lat_now,
lon_target, lat_target)
rospy.loginfo("Bearing now %f, bearing target %f, Bearing move %f, ",
bearing_now, bearing_target, bearing)
diff_angle = (bearing_target - bearing_now + 360.0) % 360.0
if (diff_angle > 180.0):
diff_angle = diff_angle - 360.0
#if((bearing - bearing_now + 360.0)%360.0 >= 90):
# distance = -distance
#if(bearing > 90.0 and bearing < 270.0):
# distance = -distance
# bearing = (bearing + 180.0) % 360.0
rospy.loginfo(
"There's a %f mm distance error, towards %f bearing, a target angle difference of %f, %f",
distance, bearing, diff_angle, min_correction_distance)
need_correct_distance = abs(distance) > min_correction_distance
need_correct_angle = abs(diff_angle) > min_correction_angle
def distance_correction(lon_now, lat_now, bearing_now, lon_target, lat_target, bearing_target, correction_type):
distance = gpsmath.haversine(lon_now, lat_now, lon_target, lat_target)
bearing = gpsmath.bearing(lon_now, lat_now, lon_target, lat_target)
# check the bearing now and bearing target
rospy.loginfo("GPS now [%f, %f], GPS target: [%f, %f]", lon_now, lat_now, lon_target, lat_target)
rospy.loginfo("Bearing move %f, Bearing now %f, bearing target %f", bearing, bearing_now, bearing_target)
diff_angle = (bearing_target - bearing_now + 360.0) % 360.0
#if((bearing - bearing_now + 360.0)%360.0 >= 90):
# distance = -distance
#if(bearing > 90.0 and bearing < 270.0):
# distance = -distance
# bearing = (bearing + 180.0) % 360.0
rospy.loginfo("There's a %f mm distance error, towards %f bearing, a target angle difference of %f, %f", distance, bearing, diff_angle, min_correction_distance)
need_correct_distance = abs(distance) > min_correction_distance
need_correct_angle = abs(diff_angle) > min_correction_angle
#need_correct_angle = diff_angle > min_correcton_angle and diff_angle < (360.0 - min_correction_angle)
if need_correct_distance or need_correct_angle:
rospy.loginfo("Add jobs to correction.")
#robot_job.insert_compensation_jobs(lon_now, lat_now, lon_target, lat_target, correction_type, need_correct_distance, need_correct_angle)
robot_job.insert_compensation_jobs(lon_now, lat_now, bearing_now, lon_target, lat_target, bearing_target, correction_type, need_correct_distance, need_correct_angle)
else:
rospy.loginfo("no need to compensate errors")
def update_particles_with_gps(self):
""" Updates the particle weights in response to the gps location """
for particle in self.particle_cloud:
d = gpsmath.haversine(particle.x, particle.y, self.lonlat[0],
self.lonlat[1]) #mm
d = d / 1000.0 #m
# calculate nearest distance to particle's position
prob = math.exp((-d**2) / (2 * self.sigma**2)) #m
particle.w = prob
def distance_bearing_to_target():
# Get the distance
distance = gpsmath.haversine(robot_drive.lon_now, robot_drive.lat_now,
robot_drive.lon_target,
robot_drive.lat_target)
bearing = gpsmath.bearing(robot_drive.lon_now, robot_drive.lat_now,
robot_drive.lon_target, robot_drive.lat_target)
angle = gpsmath.format_bearing(bearing - robot_drive.bearing_now)
return distance, angle
def distance_route(gps_lon_lst, gps_lat_lst):
dist_total = 0.0
for k in range(len(gps_lon_lst) - 1):
k_nex = k + 1
distance_cal = gpsmath.haversine(gps_lon_lst[k], gps_lat_lst[k],
gps_lon_lst[k_nex],
gps_lat_lst[k_nex])
dist_total = dist_total + distance_cal
return dist_total
def append_backward_job(lon_source, lat_source, lon_target, lat_target,
bearing_now):
global job_lists
rospy.loginfo("Added a job to move from (%f, %f) to (%f, %f)", lon_source,
lat_source, lon_target, lat_target)
bearing = gpsmath.bearing(lon_source, lat_source, lon_target, lat_target)
distance = gpsmath.haversine(lon_source, lat_source, lon_target,
lat_target)
move_job = Job(lon_target, lat_target, bearing_now, 'N', 'B', distance)
job_lists.extend([move_job])
def find_closest_loop_index():
# Find the loop points which is cloest to the current position
distance = 100000000.0
gps_num = len(gps_lon)
index = 0
for k in range(gps_num):
distance_cal = gpsmath.haversine(robot_drive.lon_now,
robot_drive.lat_now, gps_lon[k],
gps_lat[k])
if (distance_cal < distance):
distance = distance_cal
index = k
return index
def append_regular_jobs(lon_source, lat_source, lon_target, lat_target):
global job_lists
rospy.loginfo("Added a job to move from (%f, %f) to (%f, %f)", lon_source,
lat_source, lon_target, lat_target)
bearing = gpsmath.bearing(lon_source, lat_source, lon_target, lat_target)
distance = gpsmath.haversine(lon_source, lat_source, lon_target,
lat_target)
turn_job = Job(lon_source, lat_source, bearing, 'N', 'T', bearing)
move_job = Job(lon_target, lat_target, bearing, 'N', 'F', distance)
rospy.loginfo("Added a turn job: Turn to %f", bearing)
rospy.loginfo("Added a move job: Move %f mm", distance)
job_lists.extend([turn_job])
job_lists.extend([move_job])
def insert_compensation_jobs(lon_source, lat_source, bearing_source,
lon_target, lat_target, bearing_target,
correction_type, need_correct_distance,
need_correct_angle):
global job_lists
bearing = gpsmath.bearing(lon_source, lat_source, lon_target, lat_target)
distance = gpsmath.haversine(lon_source, lat_source, lon_target,
lat_target)
turn_job = Job(lon_source, lat_source, bearing_target, correction_type,
'T', bearing_target)
turn_before_move_job = Job(lon_source, lat_source, bearing,
correction_type, 'T', bearing)
move_job = Job(lon_target, lat_target, bearing, correction_type, 'F',
distance)
#reverse_job = Job(lon_target, lat_target, bearing, correction_type, 'B', distance)
#if (need_correct_distance and not need_correct_angle):
# if((bearing - bearing_source + 360.0)%360.0 >= 90):
# rospy.loginfo("Added a backward distance correction")
# job_lists.insert(0, reverse_job)
# else:
# rospy.loginfo("Added a forward distance correction")
# job_lists.insert(0, move_job)
if need_correct_distance and need_correct_angle:
rospy.loginfo("Added a distance correction and angle correction")
job_lists.insert(0, turn_before_move_job)
job_lists.insert(1, move_job)
job_lists.insert(2, turn_job)
elif need_correct_distance and not need_correct_angle:
rospy.loginfo("Added an distance correction")
job_lists.insert(0, turn_before_move_job)
job_lists.insert(1, move_job)
job_lists.insert(2, turn_job)
elif (need_correct_angle and not need_correct_distance):
rospy.loginfo("Added a angle correction")
job_lists.insert(0, turn_job)
def update_particles_with_odom(self):
""" Update the particles using the newly given odometry pose.
The function computes the value delta which is a tuple (x,y,theta)
that indicates the change in position and angle between the odometry
when the particles were last updated and the current odometry.
msg: this is not really needed to implement this, but is here just in case.
"""
# compute the change in x,y,theta since our last update
if self.current_odom_xy_theta:
old_odom_xy_theta = self.current_odom_xy_theta
dist = gpsmath.haversine(self.current_odom_xy_theta[0],
self.current_odom_xy_theta[1],
self.new_odom_xy_theta[0],
self.new_odom_xy_theta[1]) #mm
delta_angle = self.new_odom_xy_theta[
2] - self.current_odom_xy_theta[2]
if delta_angle > 180.0:
delta_angle = delta_angle - 360.0
elif delta_angle <= -180.0:
delta_angle = delta_angle + 360.0
self.current_odom_xy_theta = self.new_odom_xy_theta
else:
self.current_odom_xy_theta = self.new_odom_xy_theta
return
for particle in self.particle_cloud:
particle.theta = particle.theta + delta_angle
if particle.theta >= 360.0:
particle.theta = particle.theta - 360.0
elif particle.theta < 0.0:
particle.theta = particle.theta + 360.0
x_new, y_new = gpsmath.get_gps(particle.x, particle.y, dist,
particle.theta) #mm
particle.x = x_new
particle.y = y_new
def left_gps_distance():
job_now = current_job()
dist_temp = gpsmath.haversine(job_now.lon_target, job_now.lat_target,
robot_drive.lon_now, robot_drive.lat_now)
return dist_temp
