def sonar_callback(sonar1='', sonar2='', sonar3=''):
sonar_ranges = {}
range_max = 5
if sonar1 is not '':
#print('Sonar1: {}'.format(sonar1.range))
range_max = sonar1.max_range
sonar_ranges['sonar1'] = sonar1.range
if sonar2 is not '':
#print('Sonar2: {}'.format(sonar2.range))
sonar_ranges['sonar2'] = sonar2.range
if sonar3 is not '':
#print('Sonar3: {}'.format(sonar3.range))
sonar_ranges['sonar3'] = sonar3.range
# Use obstacle avoidance algorithm
nav_cmds = sonar_avoidance(sonar_ranges, sonar_angles, range_max)
#nav_cmds = test_avoidance(sonar_ranges, range_max)
msg = OverrideRCIn()
msg.channels[0] = nav_cmds['yaw']
msg.channels[1] = 0
msg.channels[2] = nav_cmds['throttle']
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
obstacle_avoidance_cmds_pub.publish(msg)
print(nav_cmds)
def sonar_callback(sonar1='',
sonar2='',
sonar3='',
sonar4='',
sonar5='',
sonar6='',
sonar7='',
sonar8='',
sonar9='',
sonar10=''):
global vehicle
global last_vehicle_mode
global history_queue
global last_heading
ang = angle_to_current_waypoint(vehicle)
#print('Bearing: {} \t Heading: {}'.format(ang, last_heading))
sonar_ranges = {}
range_max = 2.5
hybrid_zone_cutoff = 2
# Read any sonar data available and put it into a single dict called sonar_ranges
# Also have a modifier for range_max - This is in case the max sensing capabilities are changed in the URDF file
# It is -0.5 because at the default angle that sonars are passed on the rover, they catch the ground at the very end
# of the sensing range. We want to ignore the ground readings while still picking up small objects
for j in range(1, 11):
current_sonar = 'sonar' + str(j)
if eval(current_sonar) is not '':
range_max = eval(current_sonar).max_range - 0.5
sonar_ranges[current_sonar] = eval(current_sonar).range
# Check to see if an object is in the path of the rover
if all(i >= range_max for i in sonar_ranges.values()):
# All is good
if (vehicle.mode == VehicleMode("MANUAL")):
#print('Clear of obstacle! Switching to {}'.format(last_vehicle_mode))
vehicle.mode = last_vehicle_mode
else:
if (vehicle.mode != VehicleMode("MANUAL")):
#print('Object detected! Obstacle avoidance engaged!')
last_vehicle_mode = vehicle.mode
vehicle.mode = VehicleMode("MANUAL")
# Use obstacle avoidance algorithm
nav_cmds = sonar_avoidance(sonar_ranges, sonar_angles, range_max)
msg = OverrideRCIn()
msg.channels[0] = nav_cmds['yaw']
msg.channels[1] = 0
msg.channels[2] = nav_cmds['throttle']
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
obstacle_avoidance_cmds_pub.publish(msg)
def sonar_callback(sonar1='', sonar2='', sonar3=''):
global vehicle
global last_vehicle_mode
global history_queue
sonar_ranges = {}
range_max = 4
hybrid_zone_cutoff = 2
if sonar1 is not '':
#print('Sonar1: {}'.format(sonar1.range))
range_max = sonar1.max_range - 0.5
sonar_ranges['sonar1'] = sonar1.range
if sonar2 is not '':
#print('Sonar2: {}'.format(sonar2.range))
sonar_ranges['sonar2'] = sonar2.range
if sonar3 is not '':
#print('Sonar3: {}'.format(sonar3.range))
sonar_ranges['sonar3'] = sonar3.range
# Check to see if an object is in the path of the rover
if all(i >= range_max for i in sonar_ranges.values()):
# All is good
if (vehicle.mode == VehicleMode("MANUAL")):
#print('Clear of obstacle! Switching to {}'.format(last_vehicle_mode))
vehicle.mode = last_vehicle_mode
else:
if (vehicle.mode != VehicleMode("MANUAL")):
#print('Object detected! Obstacle avoidance engaged!')
last_vehicle_mode = vehicle.mode
vehicle.mode = VehicleMode("MANUAL")
# Use obstacle avoidance algorithm
nav_cmds = sonar_avoidance(sonar_ranges, sonar_angles, range_max)
# Use hybrid of O.A. and W.P.F. if obstacles are further out in the detection range
if min(sonar_ranges.values()) > hybrid_zone_cutoff:
w_p_f_cmds = sum(history_queue) / len(history_queue)
weight = min(sonar_ranges.values()) / range_max
# W.P and object are on the right
if (w_p_f_cmds > 1500 and nav_cmds['yaw'] < 1500):
nav_cmds['yaw'] = (400 * weight) + 1500
# W.P. and object are on the left
if (w_p_f_cmds < 1500 and nav_cmds['yaw'] > 1500):
nav_cmds['yaw'] = 1500 - (400 * weight)
weight = min(sonar_ranges.values()) / range_max
#nav_cmds['yaw'] = weight * w_p_f_cmds + (1 - weight) * nav_cmds['yaw']
#print('Use hybrid!')
#print('Yaw: {}'.format(nav_cmds['yaw']))
msg = OverrideRCIn()
msg.channels[0] = nav_cmds['yaw']
msg.channels[1] = 0
msg.channels[2] = nav_cmds['throttle']
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
obstacle_avoidance_cmds_pub.publish(msg)