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 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 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 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)