def navigate_to_goal(self, goal, timeout):
start_time = self.time
self.last_position_angle = self.last_position
gps_angle = None
while geo_length(self.last_position,
goal) > 1.0 and self.time - start_time < timeout:
desired_heading = normalizeAnglePIPI(
geo_angle(self.last_position, goal))
step = geo_length(self.last_position, self.last_position_angle)
if step > 1.0:
gps_angle = normalizeAnglePIPI(
geo_angle(self.last_position_angle, self.last_position))
print('step', step, math.degrees(gps_angle))
self.last_position_angle = self.last_position
desired_wheel_heading = normalizeAnglePIPI(desired_heading -
self.last_imu_yaw)
self.send_speed_cmd(self.maxspeed, desired_wheel_heading)
prev_time = self.time
self.update()
if int(prev_time.total_seconds()) != int(
self.time.total_seconds()):
print(self.time, geo_length(self.last_position, goal),
math.degrees(self.last_imu_yaw))
print("STOP (3s)")
self.send_speed_cmd(0, 0)
start_time = self.time
while self.time - start_time < timedelta(seconds=3):
self.update()
def get_angle_diff(self, destination, direction=1):
angle_diff = normalizeAnglePIPI(
math.atan2(destination[1] - self.xyz[1], destination[0] -
self.xyz[0]) - self.yaw)
if direction < 0:
angle_diff = -math.pi + angle_diff
return normalizeAnglePIPI(angle_diff)
def on_rot(self):
temp_yaw, self.pitch, self.roll = [
normalizeAnglePIPI(math.radians(x / 100)) for x in self.rot
]
if self.yaw_offset is None:
self.yaw_offset = -temp_yaw
self.yaw = temp_yaw + self.yaw_offset
def turn(self, angle, with_stop=True, speed=0.0, timeout=None):
print(self.time, "turn %.1f" % math.degrees(angle))
if angle >= 0:
self.send_speed_cmd(speed, self.max_angular_speed)
else:
self.send_speed_cmd(speed, -self.max_angular_speed)
start_time = self.time
# problem with accumulated angle
sum_angle = 0.0
prev_angle = self.yaw
while abs(sum_angle) < abs(angle):
self.update()
sum_angle += normalizeAnglePIPI(self.yaw - prev_angle)
prev_angle = self.yaw
if timeout is not None and self.time - start_time > timeout:
print(self.time,
"turn - timeout at %.1fdeg" % math.degrees(sum_angle))
break
if with_stop:
self.send_speed_cmd(0.0, 0.0)
start_time = self.time
while self.time - start_time < timedelta(seconds=2):
self.update()
print(self.time, 'stop at', self.time - start_time)
def update(self, robot, channel, data):
# TODO use orientation with quaternion instead
if channel == 'rot':
yaw, pitch, roll = [
normalizeAnglePIPI(math.radians(x / 100)) for x in data
]
if pitch > 0.5:
raise PitchRollException()
def on_rot(self, data):
rot = data
rot[2] += 18000
(temp_yaw, self.pitch,
self.roll) = [normalizeAnglePIPI(math.radians(x / 100)) for x in rot]
if self.yaw_offset is None:
self.yaw_offset = -temp_yaw
self.yaw = temp_yaw + self.yaw_offset
def drive_around_rock(self, how_far, view_direction=None, timeout=None):
# go around a rock with 'how_far' clearance, if how_far positive, it goes around to the left, negative means right
# will keeping to look forward (i.e, movement is sideways)
# view_direction - if present, first turn in place to point in that direction
# TODO: use lidar to go around as much as needed
print(
self.sim_time, self.robot_name,
"go_around_a_rock %.1f (speed: %.1f)" % (how_far, self.max_speed))
if timeout is None:
timeout = timedelta(
seconds=4 +
2 * abs(how_far) / self.max_speed) # add 4 sec for wheel setup
if view_direction is not None:
self.turn(normalizeAnglePIPI(view_direction - self.yaw),
timeout=timedelta(seconds=10))
start_pose = self.xyz
start_time = self.sim_time
while distance(start_pose, self.xyz) < abs(how_far):
self.publish("desired_movement", [
GO_STRAIGHT, 7500,
-math.copysign(self.default_effort_level, how_far)
]) # 1000m radius is almost straight
self.wait(timedelta(milliseconds=100))
if timeout is not None and self.sim_time - start_time > timeout:
print(
self.sim_time, self.robot_name,
"go_around_a_rock - timeout at %.1fm" %
distance(start_pose, self.xyz))
break
self.send_speed_cmd(0.0, 0.0) # TEST
return # TEST TODO: this will end rock bypass routine right after side-step
self.go_straight(math.copysign(abs(how_far) + 5,
how_far)) # TODO: rock size estimate
# TODO: maybe going back not needed, hand over to main routine, we presumably already cleared the obstacle
start_pose = self.xyz
start_time = self.sim_time
while distance(start_pose, self.xyz) < abs(how_far):
self.publish("desired_movement", [
GO_STRAIGHT, -7500,
-math.copysign(self.default_effort_level, how_far)
]) # 1000m radius is almost straight
self.wait(timedelta(milliseconds=100))
if timeout is not None and self.sim_time - start_time > timeout:
print(
self.sim_time, self.robot_name,
"go_around_a_rock - timeout at %.1fm" %
distance(start_pose, self.xyz))
break
self.send_speed_cmd(0.0, 0.0)
print(
self.sim_time, self.robot_name,
"go_around_a_rock ended at [%.1f,%.1f]" %
(self.xyz[0], self.xyz[1]))
def update(self):
# print status periodically - location
if self.time is not None:
if self.last_status_timestamp is None:
self.last_status_timestamp = self.time
elif self.time - self.last_status_timestamp > timedelta(seconds=8):
self.last_status_timestamp = self.time
x, y, z = self.xyz
print(
self.time,
"Loc: [%f %f %f] [%f %f %f]; Driver: %s; Score: %d" %
(x, y, z, self.roll, self.pitch, self.yaw,
self.current_driver, self.score))
channel = super().update()
# handler = getattr(self, "on_" + channel, None)
# if handler is not None:
# handler(self.time, data)
if channel == 'pose2d':
self.on_pose2d(self.time, self.pose2d)
elif channel == 'artf':
self.on_artf(self.time, self.artf)
elif channel == 'score':
self.on_score(self.time, self.score)
elif channel == 'driving_control':
self.on_driving_control(self.time, self.driving_control)
elif channel == 'object_reached':
self.on_object_reached(self.time, self.object_reached)
elif channel == 'scan':
self.on_scan(self.time, self.scan)
self.local_planner.update(self.scan)
elif channel == 'rot':
temp_yaw, self.pitch, self.roll = [
normalizeAnglePIPI(math.radians(x / 100)) for x in self.rot
]
if self.yaw_offset is None:
self.yaw_offset = -temp_yaw
self.yaw = temp_yaw + self.yaw_offset
if self.use_gimbal:
# maintain camera level
cam_angle = self.camera_angle - self.pitch
self.send_request('set_cam_angle %f\n' % cam_angle)
if not self.inException and self.pitch > 0.6:
# TODO pitch can also go the other way if we back into an obstacle
# TODO: robot can also roll if it runs on a side of a rock while already on a slope
self.bus.publish('driving_recovery', True)
print(self.time, "app: Excess pitch, going back down")
raise VirtualBumperException()
for m in self.monitors:
m(self, channel)
return channel
def on_rot(self, data):
# use of on_rot is deprecated, will be replaced by on_orientation
# also, this filtering does not work when an outlier is presented while angles near singularity
# e.g., values 0, 1, 359, 358, 120 will return 120
temp_yaw, temp_pitch, temp_roll = [
normalizeAnglePIPI(math.radians(x / 100)) for x in data
]
self.yaw_history.append(temp_yaw)
self.pitch_history.append(temp_pitch)
self.roll_history.append(temp_roll)
if len(self.yaw_history) > 5:
self.yaw_history.pop(0)
self.pitch_history.pop(0)
self.roll_history.pop(0)
self.yaw = normalizeAnglePIPI(
median(self.yaw_history) - self.yaw_offset)
self.pitch = median(self.pitch_history)
self.roll = median(self.roll_history)
if self.use_gimbal:
# maintain camera level
cam_angle = min(
math.pi / 4.0,
max(-math.pi / 8.0, self.camera_angle + self.pitch))
self.publish('cmd', b'set_cam_angle %f' % cam_angle)
if self.virtual_bumper is not None and not self.inException and (
abs(self.pitch) > 0.9 or abs(self.roll) > math.pi / 4):
# TODO pitch can also go the other way if we back into an obstacle
# TODO: robot can also roll if it runs on a side of a rock while already on a slope
self.bus.publish('driving_recovery', True)
print(self.sim_time, self.robot_name,
"app: Excess pitch or roll, going back")
raise VirtualBumperException()
if abs(self.roll) > math.pi / 2 and (
self.last_reset_model is None or
self.sim_time - self.last_reset_model > timedelta(seconds=15)):
# if roll is more than 90deg, robot is upside down
self.last_reset_model = self.sim_time
def step(self, dt):
if abs(self.desired_speed) > 0:
dist = dt # i.e. 1m/s
if abs(self.desired_speed) <= 1.0:
dist = 0.0 # turn in place
x, y, heading = self.rel_pose
a = heading + self.wheel_angle
self.rel_pose = x + dist*math.cos(a), y + dist*math.sin(a), heading
diff = normalizeAnglePIPI(self.desired_wheel_angle - self.wheel_angle)
if abs(diff) > math.radians(1):
if diff > 0:
corr = math.radians(10) # 10deg/s
else:
corr = math.radians(-10)
self.wheel_angle += dt*corr
self.status ^= 0x8000
def turn(self, angle, with_stop=True, speed=0.0):
print(self.time, "turn %.1f" % math.degrees(angle))
start_pose = self.last_position
if angle >= 0:
self.send_speed_cmd(speed, self.max_angular_speed)
else:
self.send_speed_cmd(speed, -self.max_angular_speed)
while abs(normalizeAnglePIPI(start_pose[2] - self.last_position[2])) < abs(angle):
self.update()
if with_stop:
self.send_speed_cmd(0.0, 0.0)
start_time = self.time
while self.time - start_time < timedelta(seconds=2):
self.update()
if not self.is_moving:
break
print(self.time, 'stop at', self.time - start_time)
def turn(self, angle, with_stop=True, speed=0.0):
print(self.time, "turn %.1f" % math.degrees(angle))
start_pose = self.last_position
if angle >= 0:
self.send_speed_cmd(speed, self.max_angular_speed)
else:
self.send_speed_cmd(speed, -self.max_angular_speed)
while abs(normalizeAnglePIPI(start_pose[2] - self.last_position[2])) < abs(angle):
self.update()
if with_stop:
self.send_speed_cmd(0.0, 0.0)
start_time = self.time
while self.time - start_time < timedelta(seconds=2):
self.update()
if not self.is_moving:
break
print(self.time, 'stop at', self.time - start_time)
def set_speed(self, desired_speed, desired_wheel_heading):
# TODO split this for Car and Spider mode
speed = int(round(desired_speed))
desired_steering = int(-512 * math.degrees(desired_wheel_heading) /
360.0)
if speed != 0:
if self.wheel_heading is None:
speed = 1 # in in place
else:
d = math.degrees(
normalizeAnglePIPI(self.wheel_heading -
desired_wheel_heading))
if abs(d) > 20.0:
speed = 1 # turn in place (II.)
self.cmd = [speed, desired_steering]
def approach_box(self, at_dist):
print(self.time, 'approach_box')
speed = 0.2
box_pos = None
while self.last_scan is None:
self.update()
if min_dist(self.last_scan[270:-270]) > at_dist:
self.send_speed_cmd(speed, 0.0)
prev_count = self.scan_count
while min_dist(self.last_scan[270:-270]) > at_dist:
self.update()
if prev_count != self.scan_count:
box_i = detect_box(self.last_scan)
if box_i is not None:
angle = math.radians(
(len(self.last_scan) // 2 - box_i) / 3)
dist = self.last_scan[box_i] / 1000.0
pose = combine(self.last_position, self.laser_pose)
pose = combine(pose,
(0, 0.1, 0)) # correct for hand balls
box_pos = (pose[0] + dist * math.cos(pose[2] + angle),
pose[1] + dist * math.sin(pose[2] + angle))
# TODO laser position offset??
# print('%.3f\t %.3f' % box_pos)
prev_count = self.scan_count
angular_speed = 0.0
if box_pos is not None:
x, y, heading = self.last_position
diff = normalizeAnglePIPI(
math.atan2(box_pos[1] - y, box_pos[0] - x) -
heading)
if math.hypot(x - box_pos[0], y - box_pos[1]) > 0.5:
angular_speed = diff # turn to goal in 1s
t0 = self.time
t1 = self.send_speed_cmd(speed, angular_speed)
dt = t1 - t0
if dt > timedelta(microseconds=100000):
print('Queue delay:', dt)
self.wait_for_new_scan() # skip old one
self.wait_for_new_scan(
) # take every 2nd + sometimes even 3rd
self.send_speed_cmd(0.0, 0.0) # or it should stop always??
def step(self, dt):
if abs(self.desired_speed) > 0:
dist = dt # i.e. 1m/s
if abs(self.desired_speed) <= 1.0:
dist = 0.0 # turn in place
x, y, heading = self.rel_pose
a = heading + self.wheel_angle
self.rel_pose = x + dist * math.cos(a), y + dist * math.sin(
a), heading
diff = normalizeAnglePIPI(self.desired_wheel_angle -
self.wheel_angle)
if abs(diff) > math.radians(1):
if diff > 0:
corr = math.radians(10) # 10deg/s
else:
corr = math.radians(-10)
self.wheel_angle += dt * corr
self.status ^= 0x8000
def update(self):
channel = super().update()
assert channel in ["depth", "scan", "rot"], channel
if channel == 'depth':
assert self.depth.shape == (360, 640), self.depth.shape
elif channel == 'scan':
assert len(self.scan) == 720, len(self.scan)
if self.depth is None:
self.publish('scan', self.scan)
return channel # when no depth data are available ...
depth_scan = depth2dist(self.depth, self.pitch, self.roll)
new_scan = adjust_scan(self.scan, depth_scan)
self.publish('scan', new_scan.tolist())
elif channel == 'rot':
self.yaw, self.pitch, self.roll = [
normalizeAnglePIPI(math.radians(x / 100)) for x in self.rot
]
else:
assert False, channel # unsupported channel
return channel
def turn(self,
angle,
with_stop=True,
speed=0.0,
ang_speed=None,
timeout=None):
# positive turn - counterclockwise
print(
self.sim_time, self.robot_name, "turn %.1f deg from %.1f deg" %
(math.degrees(angle), math.degrees(self.yaw)))
self.send_speed_cmd(
speed,
math.copysign(
ang_speed if ang_speed is not None else self.max_angular_speed,
angle))
start_time = self.sim_time
# problem with accumulated angle
sum_angle = 0.0
prev_angle = self.yaw
slowdown_happened = False
while abs(sum_angle) < abs(angle):
if with_stop and not slowdown_happened and abs(sum_angle) > abs(
angle
) - 0.35: # slow rotation down for the last 20deg to improve accuracy
self.send_speed_cmd(
speed, math.copysign(self.max_angular_speed / 2.0, angle))
slowdown_happened = True
self.update()
sum_angle += normalizeAnglePIPI(self.yaw - prev_angle)
prev_angle = self.yaw
if timeout is not None and self.sim_time - start_time > timeout:
print(self.sim_time, self.robot_name,
"turn - timeout at %.1fdeg" % math.degrees(sum_angle))
break
if with_stop:
self.send_speed_cmd(0.0, 0.0)
def approach_box(self, at_dist):
print(self.time, 'approach_box')
speed = 0.2
box_pos = None
while self.last_scan is None:
self.update()
if min_dist(self.last_scan[270:-270]) > at_dist:
self.send_speed_cmd(speed, 0.0)
prev_count = self.scan_count
while min_dist(self.last_scan[270:-270]) > at_dist:
self.update()
if prev_count != self.scan_count:
box_i = detect_box(self.last_scan)
if box_i is not None:
angle = math.radians((len(self.last_scan)//2 - box_i)/3)
dist = self.last_scan[box_i]/1000.0
pose = combine(self.last_position, self.laser_pose)
pose = combine(pose, (0, 0.1, 0)) # correct for hand balls
box_pos = (pose[0] + dist * math.cos(pose[2] + angle),
pose[1] + dist * math.sin(pose[2] + angle))
# TODO laser position offset??
# print('%.3f\t %.3f' % box_pos)
prev_count = self.scan_count
angular_speed = 0.0
if box_pos is not None:
x, y, heading = self.last_position
diff = normalizeAnglePIPI(math.atan2(box_pos[1] - y, box_pos[0] - x) - heading)
if math.hypot(x - box_pos[0], y - box_pos[1]) > 0.5:
angular_speed = diff # turn to goal in 1s
t0 = self.time
t1 = self.send_speed_cmd(speed, angular_speed)
dt = t1 - t0
if dt > timedelta(microseconds=100000):
print('Queue delay:', dt)
self.wait_for_new_scan() # skip old one
self.wait_for_new_scan() # take every 2nd + sometimes even 3rd
self.send_speed_cmd(0.0, 0.0) # or it should stop always??
def move_sideways(
self,
how_far,
view_direction=None,
timeout=None): # how_far: left is positive, right is negative
print(self.sim_time, self.robot_name,
"move_sideways %.1f (speed: %.1f)" % (how_far, self.max_speed),
self.last_position)
if timeout is None:
timeout = timedelta(
seconds=4 +
2 * abs(how_far) / self.max_speed) # add 4 sec for wheel setup
if view_direction is not None:
angle_diff = normalizeAnglePIPI(view_direction - self.yaw)
if abs(angle_diff) > math.radians(
10): # only turn if difference more than 10deg
self.turn(angle_diff, timeout=timedelta(seconds=10))
start_pose = self.xyz
start_time = self.sim_time
while distance(start_pose, self.xyz) < abs(how_far):
self.publish("desired_movement", [
GO_STRAIGHT, -9000,
-math.copysign(self.default_effort_level, how_far)
])
self.wait(timedelta(milliseconds=100))
if timeout is not None and self.sim_time - start_time > timeout:
print(
self.sim_time, self.robot_name,
"go_sideways - timeout at %.1fm" %
distance(start_pose, self.xyz))
break
self.send_speed_cmd(0.0, 0.0)
print(
self.sim_time, self.robot_name,
"move sideways ended at [%.1f,%.1f]" % (self.xyz[0], self.xyz[1]))
def on_scan(self, data):
assert len(data) == 180
super().on_scan(data)
if len(self.median_scan) > 0:
self.min_scan_distance = min(self.median_scan) / 1000.0
delete_in_view = [
artf for artf in self.objects_in_view
if self.objects_in_view[artf]['expiration'] < self.sim_time
]
for artf in delete_in_view:
del self.objects_in_view[artf]
if "rover" not in self.objects_in_view.keys(
) and "excavator_arm" not in self.objects_in_view.keys(
) and self.rover_distance < 15:
self.rover_distance = 15
if (self.debug):
print(self.sim_time, self.robot_name,
"Expiring rover view and resetting distance to 15m")
if not self.arrived_message_sent:
raise ExcavatorLostException()
if not self.finish_visually:
return
# once distance reached, go on another 200ms to get real tight
if self.arrival_send_requested_at is not None and self.sim_time - self.arrival_send_requested_at > timedelta(
milliseconds=200):
self.publish("desired_movement", [0, 0, 0])
if self.arrival_send_requested_at is not None and self.sim_time - self.arrival_send_requested_at > timedelta(
milliseconds=1500):
self.arrival_send_requested_at = None
if self.set_yaw is not None:
print(
self.sim_time, self.robot_name,
"Internal yaw: %.2f, set yaw: %.2f, yaw offset: %.2f" %
(self.yaw, self.set_yaw, self.yaw - self.set_yaw))
self.yaw_offset = self.yaw - self.set_yaw
self.set_yaw = None
print(self.sim_time, self.robot_name,
"Sending arrived message to excavator")
self.send_request('external_command excavator_1 arrived %.2f' %
self.yaw)
# TODO: avoid obstacles when following (too tight turns)
if ('rover' in self.objects_in_view.keys()
or 'excavator_arm' in self.objects_in_view.keys()) and (
self.driving_mode == "approach" or self.driving_mode
== "align" or self.driving_mode == "turnto"
) and not self.arrived_message_sent and not self.inException:
# TODO: sometimes we are near rover but don't see its distance
# other times, we see a rover and its distance but stuck behind a rock
if self.excavator_yaw is None: # within requested distance and no outstanding yaw, report
if min(self.rover_distance, self.min_scan_distance
) < self.target_excavator_distance:
self.set_brakes(True)
self.arrival_send_requested_at = self.sim_time
self.arrived_message_sent = True
self.excavator_yaw = None
self.scan_driving = False
if self.debug:
print(
self.sim_time, self.robot_name,
"Arrival handling: distance %.1f, yaw: %.2f, rover angle: %.2f; stopping"
%
(self.rover_distance, self.yaw, self.rover_angle))
if self.scan_driving or (
abs(self.rover_distance - self.target_excavator_distance) <
DISTANCE_TOLERANCE and self.excavator_yaw is not None
): # within 1m from target, start adjusting angles
if not self.scan_driving:
if self.set_yaw is None: # do not stop when doing initial excavator approach to bump and align better
self.publish("desired_movement", [0, 0, 0])
self.scan_driving = True
diff = normalizeAnglePIPI(self.yaw - self.excavator_yaw)
if (self.debug):
print(
self.sim_time, self.robot_name,
"Arrival handling: yaw difference: (%.2f - %.2f)= %.2f, rover angle: %.2f"
%
(self.yaw, self.excavator_yaw, diff, self.rover_angle))
if self.scan_driving_phase == "yaw":
if self.scan_driving_phase_start is None:
self.scan_driving_phase_start = self.sim_time
if abs(
diff
) > 0.12 and self.set_yaw is None and self.sim_time - self.scan_driving_phase_start < timedelta(
seconds=30
): # 10deg, don't bother otherwise; also, don't attempt to align before true pose is known
if (self.debug):
print(self.sim_time, self.robot_name,
"Arrival handling: moving on a curve")
self.publish("desired_movement", [
-8, -9000,
math.copysign(0.5 * self.default_effort_level,
diff)
])
#TODO: keep doing until diff=0; however we are bound to overshoot so maybe leave as is
else:
self.scan_driving_phase = "behind"
if self.scan_driving_phase == "behind":
if abs(
self.rover_angle
) > 0.12 and self.set_yaw is None and self.sim_time - self.scan_driving_phase_start < timedelta(
seconds=50):
if (self.debug):
print(self.sim_time, self.robot_name,
"Arrival handling: moving sideways")
self.publish("desired_movement", [
GO_STRAIGHT, -9000,
-math.copysign(0.5 * self.default_effort_level,
self.rover_angle)
])
#TODO: keep doing until angle=0
else:
self.scan_driving_phase = "waiting"
if self.scan_driving_phase == "waiting":
if (self.debug):
print(
self.sim_time, self.robot_name,
"Arrival handling: final wait to straighten wheels"
)
if self.aligned_at is None:
self.aligned_at = self.sim_time
if self.sim_time - self.aligned_at > timedelta(
milliseconds=2500) or self.set_yaw is not None:
if self.driving_mode == "approach":
self.target_excavator_distance = EXCAVATOR_DIGGING_GAP
self.excavator_yaw = None
self.scan_driving = False
self.aligned_at = None
self.scan_driving_phase = "yaw"
self.scan_driving_phase_start = None
else:
self.publish(
"desired_movement", [0, 0, 0]
) # going straight doesn't wait for steering so we have to wait here
def run(self):
try:
self.wait_for_init()
#self.wait(timedelta(seconds=5))
self.set_light_intensity("0.2")
self.set_cam_angle(-0.1)
self.use_gimbal = True
self.set_brakes(True)
if False:
# point wheels downwards and wait until on flat terrain or timeout
start_drifting = self.sim_time
while self.sim_time - start_drifting < timedelta(
seconds=20) and (abs(self.pitch) > 0.05
or abs(self.roll) > 0.05):
try:
attitude = math.asin(
math.sin(self.roll) / math.sqrt(
math.sin(self.pitch)**2 +
math.sin(self.roll)**2))
if self.debug:
print(self.sim_time, self.robot_name,
"Vehicle attitude: %.2f" % attitude)
self.publish(
"desired_movement",
[GO_STRAIGHT,
math.degrees(attitude * 100), 0.1])
self.wait(timedelta(milliseconds=100))
except MoonException as e:
print(
self.sim_time, self.robot_name,
"Exception while waiting for excavator to settle: ",
str(e))
self.publish("desired_movement", [0, 0, 0])
while True:
while self.driving_mode is None: # waiting for align command
try:
self.wait(timedelta(seconds=1))
except MoonException as e:
print(
self.sim_time, self.robot_name,
"Exception while waiting to be invited to look for excavator",
str(e))
break
self.set_brakes(False)
self.finish_visually = True
# FIND EXCAVATOR
while True:
try:
self.excavator_waiting = True
with LidarCollisionMonitor(self):
while True:
self.turn(math.radians(self.rand.randrange(
90, 270)),
timeout=timedelta(seconds=20))
self.turn(math.radians(360),
timeout=timedelta(seconds=40))
self.go_straight(20.0,
timeout=timedelta(minutes=2))
except ChangeDriverException as e:
print(self.sim_time, self.robot_name,
"Excavator found, following")
self.publish("desired_movement", [0, 0, 0])
except (VirtualBumperException, LidarCollisionException) as e:
self.inException = True
print(self.sim_time, self.robot_name,
"Lidar or Virtual Bumper!")
try:
self.go_straight(-3, timeout=timedelta(seconds=20))
deg_angle = self.rand.randrange(-180, -90)
self.turn(math.radians(deg_angle),
timeout=timedelta(seconds=10))
except:
self.publish("desired_movement", [0, 0, 0])
self.inException = False
continue
except MoonException as e:
print(
self.sim_time, self.robot_name,
"MoonException while looking for rover, restarting turns"
)
traceback.print_exc(file=sys.stdout)
continue
# follow to ONHOLD distance
self.driving_mode = "onhold"
self.target_excavator_distance = EXCAVATOR_ONHOLD_GAP
# driving towards excavator, wait until hauler is in position
while abs(self.target_excavator_distance -
self.rover_distance) > DISTANCE_TOLERANCE:
try:
self.wait(timedelta(milliseconds=200))
except ExcavatorLostException:
print(
self.sim_time, self.robot_name,
"Excavator lost while waiting to reach desired location, starting to look again"
)
break # look again
except MoonException:
print(
self.sim_time, self.robot_name,
"Exception while waiting to reach desired location, waiting on"
)
if abs(self.target_excavator_distance -
self.rover_distance) <= DISTANCE_TOLERANCE:
break # distance reached
self.publish("desired_movement", [0, 0, 0])
self.set_brakes(True)
print(self.sim_time, self.robot_name,
"Sending arrived message to excavator")
self.arrived_message_sent = True
self.send_request('external_command excavator_1 arrived %.2f' %
0.0)
# turn lights off while waiting for excavator to rotate away from hauler
# prevents potentially illuminating nearby processing plant too much and causing mis-detections
self.set_light_intensity("0.0")
while self.set_yaw is None:
try:
self.wait(timedelta(seconds=1))
except MoonException:
print(
self.sim_time, self.robot_name,
"Exception while waiting for yaw alignment readiness, waiting on"
)
self.set_brakes(False)
self.set_light_intensity("0.2")
print(self.sim_time, self.robot_name,
"Sending arrived message to excavator")
self.send_request('external_command excavator_1 arrived %.2f' %
self.set_yaw)
#self.set_yaw = None
# self.finish_visually = True
# self.excavator_waiting = True
self.virtual_bumper = VirtualBumper(
timedelta(seconds=30), 0.1
) # TODO: need generous timeout because turning around or sideways moves are not sensed by bumper
# 3 modes: 1) going to location 2) following rover visually 3) waiting for instructions to go to location
while True:
if self.goto is not None:
try:
with LidarCollisionMonitor(self, 1000):
angle_diff = self.get_angle_diff(self.goto, 1)
self.turn(angle_diff,
timeout=timedelta(seconds=40))
self.go_to_location(
self.goto,
self.default_effort_level,
full_turn=False,
avoid_obstacles_close_to_destination=True,
timeout=timedelta(minutes=2))
self.turn(normalizeAnglePIPI(self.goto[2] -
self.yaw),
timeout=timedelta(seconds=40))
self.send_request(
'external_command excavator_1 arrived %.2f' %
self.yaw)
self.goto = None
self.finish_visually = True
self.set_cam_angle(-0.1)
self.set_light_intensity("0.2")
except ChangeDriverException as e:
print(self.sim_time, self.robot_name,
"Driver changed during goto?")
except LidarCollisionException as e: #TODO: long follow of obstacle causes loss, go along under steeper angle
print(self.sim_time, self.robot_name, "Lidar")
self.inException = True
self.lidar_drive_around()
self.inException = False
continue
except VirtualBumperException as e:
self.send_speed_cmd(0.0, 0.0)
print(self.sim_time, self.robot_name, "Bumper")
self.inException = True
self.go_straight(-1) # go 1m in opposite direction
self.drive_around_rock(
math.copysign(
4, 1 if self.rand.getrandbits(1) == 0 else
-1)) # assume 6m the most needed
self.inException = False
continue
try:
if self.excavator_waiting:
self.turn(math.copysign(
math.radians(self.rand.randrange(90, 270)),
self.rover_angle),
timeout=timedelta(seconds=20))
self.turn(math.radians(360),
timeout=timedelta(seconds=40))
self.go_straight(20.0, timeout=timedelta(seconds=40))
else:
if self.rover_distance > 5 and self.driving_mode != "approach":
with LidarCollisionMonitor(self, 1000):
self.wait(timedelta(seconds=1))
else:
self.wait(timedelta(seconds=1))
except LidarCollisionException as e: #TODO: long follow of obstacle causes loss, go along under steeper angle
print(
self.sim_time, self.robot_name,
"Lidar while following/waiting, distance: %.1f" %
self.rover_distance)
self.excavator_waiting = True
self.send_request('external_command excavator_1 wait')
self.inException = True
try:
self.lidar_drive_around(
) # TODO: if we lose visual of excavator, will keep going and never find, maybe turn 360
except ExcavatorLostException as e:
self.excavator_waiting = True
self.send_request('external_command excavator_1 wait')
except MoonException as e:
print(self.sim_time, self.robot_name,
"Exception while handling Lidar", str(e))
traceback.print_exc(file=sys.stdout)
self.inException = False
self.send_speed_cmd(0.0, 0.0)
except ChangeDriverException as e:
print(self.sim_time, self.robot_name,
"Excavator found, continuing visual driving")
except VirtualBumperException as e: # if bumper while following (e.g, blocked by a rock)
if self.arrived_message_sent: # if while waiting for loading, ignore
continue
self.send_speed_cmd(0.0, 0.0)
print(self.sim_time, self.robot_name, "Bumper")
self.inException = True
try:
self.go_straight(-1) # go 1m in opposite direction
self.drive_around_rock(
math.copysign(
4, 1 if self.rand.getrandbits(1) == 0 else
-1)) # assume 6m the most needed
except ExcavatorLostException as e:
self.excavator_waiting = True
self.send_request('external_command excavator_1 wait')
self.inException = False
except ExcavatorLostException as e:
self.excavator_waiting = True
self.send_request('external_command excavator_1 wait')
except BusShutdownException:
pass
print("HAULER END")
def update(self):
packet = self.bus.listen()
if packet is not None:
# print('SubT', packet)
timestamp, channel, data = packet
if self.time is None or int(self.time.seconds)//60 != int(timestamp.seconds)//60:
self.stdout(timestamp, '(%.1f %.1f %.1f)' % self.xyz, sorted(self.stat.items()))
print(timestamp, list(('%.1f' % (v/100)) for v in self.voltage))
self.stat.clear()
self.time = timestamp
if not self.is_virtual:
self.sim_time_sec = self.time.total_seconds()
self.stat[channel] += 1
handler = getattr(self, "on_" + channel, None)
if handler is not None:
handler(timestamp, data)
elif channel == 'scan' and not self.flipped:
if self.last_send_time is not None and self.last_send_time - self.time > timedelta(seconds=0.1):
print('queue delay', self.last_send_time - self.time)
self.scan = data
if self.ref_scan is None or not equal_scans(self.scan, self.ref_scan, 200):
self.ref_scan = self.scan
self.ref_count = 0
else:
self.ref_count += 1
if self.ref_count > 300:
print('Robot is stuck!', self.ref_count)
if self.collision_detector_enabled:
self.collision_detector_enabled = False
raise Collision()
self.ref_count = 0
if self.local_planner is not None:
self.local_planner.update(data)
elif channel == 'scan_back' and self.flipped:
self.scan = data
if self.local_planner is not None:
self.local_planner.update(data)
elif channel == 'scan360':
# reduce original 360 degrees scan to 270 degrees oriented forward or backward
index45deg = int(round(len(data)/8))
if self.flipped:
mid = len(data)//2
self.scan = (data[mid:]+data[:mid])[index45deg:-index45deg]
else:
self.scan = data[index45deg:-index45deg]
if self.local_planner is not None:
self.local_planner.update(data)
elif channel == 'rot':
temp_yaw, self.pitch, self.roll = [normalizeAnglePIPI(math.radians(x/100)) for x in data]
if self.yaw_offset is None:
self.yaw_offset = -temp_yaw
self.yaw = temp_yaw + self.yaw_offset
elif channel == 'orientation':
self.orientation = data
elif channel == 'sim_time_sec':
self.sim_time_sec = data
elif channel == 'origin':
if self.origin is None: # accept only initial offset
self.robot_name = data[0].decode('ascii')
if len(data) == 8:
self.origin = data[1:4]
qx, qy, qz, qw = data[4:]
self.origin_quat = qx, qy, qz, qw # quaternion
else:
self.stdout('Origin ERROR received')
self.origin_error = True
elif channel == 'voltage':
self.voltage = data
elif channel == 'emergency_stop':
self.emergency_stop = data
elif channel == 'cmd':
self.lora_cmd = data
for m in self.monitors:
m(self)
return channel
def equal_poses(pose1, pose2, dist_limit, angle_limit=None):
x1, y1, heading1 = pose1
x2, y2, heading2 = pose2
return (math.hypot(x2 - x1, y2 - y1) < dist_limit and
(angle_limit is None
or abs(normalizeAnglePIPI(heading1 - heading2)) < angle_limit))
def rad_close(a, b):
return [abs(normalizeAnglePIPI(x - y)) < 0.1 for x, y in zip(a, b)]
