def run(self):
try:
print('Wait for definition of last_position, yaw and orientation')
while self.last_position is None or self.yaw is None or self.orientation is None:
self.update() # define self.time
print('done at', self.time)
start_time = self.time
while self.time - start_time < timedelta(minutes=40):
try:
virtual_bumper = VirtualBumper(timedelta(seconds=2), 0.1)
with LidarCollisionMonitor(self), VirtualBumperMonitor(self, virtual_bumper), \
PitchRollMonitor(self):
self.go_straight(100.0, timeout=timedelta(minutes=2))
except (VirtualBumperException, LidarCollisionException,
PitchRollException) as e:
print(self.time, repr(e))
self.go_straight(-1.0, timeout=timedelta(seconds=10))
deg_angle = self.rand.randrange(90, 180)
deg_sign = self.rand.randint(0, 1)
if deg_sign:
deg_angle = -deg_angle
try:
virtual_bumper = VirtualBumper(timedelta(seconds=2), 0.1)
with VirtualBumperMonitor(self, virtual_bumper):
self.turn(math.radians(deg_angle),
timeout=timedelta(seconds=30))
except VirtualBumperException:
print(self.time, "Turn Virtual Bumper!")
self.turn(math.radians(-deg_angle),
timeout=timedelta(seconds=30))
self.wait(timedelta(seconds=10))
except BusShutdownException:
pass
def test_real_data(self):
# taken from Eduro
poses = [(7.213, -1.344, -1.55439023182615),
(7.213, -1.344, -1.5563100940033436),
(7.213, -1.344, -1.556833692778942),
(7.213, -1.344, -1.555611962302546),
(7.213, -1.344, -1.556833692778942),
(7.213, -1.344, -1.556833692778942),
(7.213, -1.344, -1.55439023182615),
(7.213, -1.344, -1.5538666330505517),
(7.213, -1.344, -1.5580554232553379),
(7.213, -1.344, -1.55439023182615),
(7.213, -1.344, -1.556833692778942),
(7.213, -1.344, -1.556833692778942)]
vb = VirtualBumper(timedelta(seconds=2), dist_radius_limit=0.1)
t0 = timedelta(seconds=0)
t_step = timedelta(seconds=0.5)
vb.update_desired_speed(0.5, 0.0)
for i, pose in enumerate(poses):
vb.update_pose(t0 + i * t_step, pose)
self.assertTrue(vb.collision())
def stripe_to_obstacle_pattern(self):
# sweep the hilly center in stripes (~10 mins), circle would include too many turns
sweep_steps = []
current_sweep_step = 0
sweep_steps.append(
[[-23, -42], "goto", -self.default_effort_level]
) # TODO: go to first point more aggressively, do not skip to next step
for i in range(-42, 36, 3):
sweep_steps.append([[35, i], "goto", self.default_effort_level])
sweep_steps.append([[-23, i], "goto", -self.default_effort_level])
sweep_steps.append([[-23, i + 3], "side",
self.default_effort_level])
start_time = self.sim_time
last_exception = self.sim_time
wait_for_mapping = False
while current_sweep_step < len(
sweep_steps) and self.sim_time - start_time < timedelta(
minutes=60):
ex = None
angle_diff = 0
speed = 0
pos = None
try:
while wait_for_mapping:
self.wait(timedelta(seconds=1))
self.virtual_bumper = VirtualBumper(
timedelta(seconds=3), 0.2
) # radius of "stuck" area; a little more as the robot flexes
with LidarCollisionMonitor(
self, 1500
): # some distance needed not to lose tracking when seeing only obstacle up front
pos, op, speed = sweep_steps[current_sweep_step]
if op == "goto":
angle_diff = self.get_angle_diff(pos, speed)
if current_sweep_step > 0 and abs(
angle_diff) > math.radians(
90): # past target, move to next
print(
self.sim_time,
"Target angle diff is more than 90deg..., already past the target, go to next step"
)
current_sweep_step += 1
continue
if abs(angle_diff) > math.radians(
10): # only turn if difference more than 10deg
ex = "turn"
self.turn(angle_diff,
timeout=timedelta(seconds=15))
ex = "goto"
self.go_to_location(pos, speed)
elif op == "side":
self.move_sideways(
3, view_direction=0) # look towards 0deg
current_sweep_step += 1
except VSLAMEnabledException as e:
print(self.sim_time, "VSLAM: mapping re-enabled")
wait_for_mapping = False
except VSLAMDisabledException as e:
print(self.sim_time, "VSLAM: mapping disabled, waiting")
self.send_speed_cmd(0.0, 0.0)
wait_for_mapping = True
except VSLAMLostException as e:
print(self.sim_time, "VSLAM lost")
self.inException = True
if ex == "turn":
self.turn(-angle_diff, timeout=timedelta(seconds=15))
else:
self.go_straight(math.copysign(
3, -speed)) # go 3m in opposite direction
self.inException = False
self.returning_to_base = True # continuing plan based on odometry, VSLAM will hopefully catch up
if not self.vslam_valid:
current_sweep_step += 1
except VSLAMFoundException as e:
self.returning_to_base = False
print(self.sim_time, "VSLAM found")
except LidarCollisionException as e:
print(self.sim_time,
"Lidar: stepping back from the obstacle...")
self.inException = True
self.go_straight(-2)
self.inException = False
if speed > 0: # if bump going forward, go backward to next leg
print(self.sim_time,
"...and switching to backwards towards next leg")
current_sweep_step += 1
else:
print(self.sim_time,
"...and continuing backwards to existing target")
except VirtualBumperException as e:
if distance(self.xyz, pos) < 5:
print(self.sim_time,
"Bumper: Close enough, continuing to next leg")
current_sweep_step += 1
continue
print(self.sim_time, "Bumper: trying to reverse")
self.inException = True
self.go_straight(math.copysign(2, -speed))
self.inException = False
if self.sim_time - last_exception > timedelta(seconds=5):
last_exception = self.sim_time
print(self.sim_time,
"- timeout, giving up on this leg, moving to next")
current_sweep_step += 1
else:
print(self.sim_time, "- will try the same leg again")
def circular_pattern(self):
current_sweep_step = 0
ARRIVAL_TOLERANCE = 5
CIRCLE_SEGMENTS = 10
sweep_steps = []
#HOMEPOINT = self.xyz # where the drive started and we know the location well
CENTERPOINT = [3, -3] # where to start the circle from
HOMEPOINT = [19, 5] # central area, good for regrouping
print(self.sim_time,
"Home coordinates: %.1f, %.1f" % (HOMEPOINT[0], HOMEPOINT[1]))
#sweep_steps.append(["turn", [math.radians(360)]])
sweep_steps.append(
["goto", False, [HOMEPOINT, self.default_effort_level, False]])
# sweep_steps.append(["turn", False, [math.radians(360)]]) # more likely to break vslam than help
# go around the main crater in circles
for rad in range(13, 55,
2): # create a little overlap not to miss anything
for i in range(CIRCLE_SEGMENTS):
x, y = pol2cart(
rad, 2 * math.pi - i * 2 * math.pi / CIRCLE_SEGMENTS)
sweep_steps.append([
"goto", False,
[[x + CENTERPOINT[0], y + CENTERPOINT[1]],
self.default_effort_level, False]
])
# sweep the hilly center in stripes (~10 mins), circle would include too many turns
# for i in range(-13, 13, 3):
# sweep_steps.append([[-15,i], -self.default_effort_level, False])
# sweep_steps.append([[15,i], self.default_effort_level, False])
#sweep_steps.append([[-7,-52], -self.default_effort_level, False]) # go backwards to starting point of right stripe
# sweep right strip
#for i in range(-65, -40, 3):
# sweep_steps.append([[-55,i], -self.default_effort_level, False])
# sweep_steps.append([[43,i], self.default_effort_level, False])
start_time = self.sim_time
wait_for_mapping = False
self.virtual_bumper = VirtualBumper(
timedelta(seconds=5), 0.2, angle_limit=math.pi /
16) # radius of "stuck" area; a little more as the robot flexes
while current_sweep_step < len(
sweep_steps) and self.sim_time - start_time < timedelta(
minutes=60):
ex = None
try:
while wait_for_mapping:
self.wait(timedelta(seconds=1))
if not self.vslam_valid:
try:
with LidarCollisionMonitor(self, 1200):
try:
if self.last_processing_plant_follow is None or self.sim_time - self.last_processing_plant_follow > timedelta(
seconds=60):
self.processing_plant_found = False
self.last_processing_plant_follow = self.sim_time
self.turn(math.radians(
self.rand.randrange(90, 270)),
timeout=timedelta(seconds=20))
self.turn(math.radians(360),
timeout=timedelta(seconds=40))
except ChangeDriverException as e:
pass
finally:
self.processing_plant_found = True
self.go_straight(20.0,
timeout=timedelta(seconds=40))
except (VirtualBumperException,
LidarCollisionException) as e:
self.inException = True
print(self.sim_time, self.robot_name,
"Lidar or Virtual Bumper in random walk")
try:
self.go_straight(-3, timeout=timedelta(seconds=20))
except:
pass
self.inException = False
continue
with LidarCollisionMonitor(
self, 1200
): # some distance needed not to lose tracking when seeing only obstacle up front
op, self.inException, params = sweep_steps[
current_sweep_step]
if op == "goto":
pos, speed, self.returning_to_base = params
print(
self.sim_time, "Driving radius: %.1f" %
distance(CENTERPOINT, pos))
self.go_to_location(
pos,
speed,
full_turn=True,
with_stop=False,
tolerance=ARRIVAL_TOLERANCE,
avoid_obstacles_close_to_destination=True)
elif op == "turn":
angle, self.returning_to_base = params
self.turn(angle, timeout=timedelta(seconds=20))
elif op == "straight": # TODO: if interrupted, it will repeat the whole distance when recovered
dist, self.returning_to_base = params
self.go_straight(dist)
elif op == "rock":
self.drive_around_rock(6) # assume 6m the most needed
elif op == "lidar":
self.lidar_drive_around()
else:
assert False, "Invalid command"
current_sweep_step += 1
except VSLAMLostException as e:
print("VSLAM lost")
self.returning_to_base = True
#sweep_steps.insert(current_sweep_step, ["goto", False, [HOMEPOINT, self.default_effort_level, True]])
continue
self.inException = True
try:
self.go_straight(-3) # go 3m in opposite direction
except VSLAMDisabledException as e:
print(self.sim_time, "VSLAM: mapping disabled, waiting")
self.send_speed_cmd(0.0, 0.0)
wait_for_mapping = True
except VSLAMEnabledException as e:
print(self.sim_time, "VSLAM: mapping re-enabled")
wait_for_mapping = False
except VSLAMFoundException as e:
print("VSLAM found on way to base")
self.returning_to_base = False
current_sweep_step += 1
except BusShutdownException:
raise
except:
pass
self.inException = False
if not self.vslam_valid: # if vslam still not valid after backtracking, go towards center
self.returning_to_base = True
# sweep_steps.insert(current_sweep_step, ["goto", [HOMEPOINT, self.default_effort_level, True]])
anglediff = self.get_angle_diff(HOMEPOINT)
# queue in reverse order
sweep_steps.insert(current_sweep_step, [
"straight", False,
[distance(self.xyz, HOMEPOINT), True]
])
sweep_steps.insert(current_sweep_step,
["turn", False, [anglediff, True]])
#TODO: to go to homebase, measure angle and distance and then go straight instead of following localization, ignore all vslam info
else:
print("VSLAM found after stepping back")
except VSLAMFoundException as e:
print("VSLAM found on way to base")
self.returning_to_base = False
current_sweep_step += 1
except VSLAMEnabledException as e:
print(self.sim_time, "VSLAM: mapping re-enabled")
# rover often loses precision after pausing, go back to center to re-localize
# sweep_steps.insert(current_sweep_step, ["goto", [HOMEPOINT, self.default_effort_level, False]]) # can't go every time
wait_for_mapping = False
except VSLAMDisabledException as e:
print(self.sim_time, "VSLAM: mapping disabled, waiting")
self.send_speed_cmd(0.0, 0.0)
wait_for_mapping = True
except LidarCollisionException as e: #TODO: long follow of obstacle causes loss, go along under steeper angle
print(self.sim_time, "Lidar")
if distance(self.xyz, pos) < ARRIVAL_TOLERANCE:
current_sweep_step += 1
continue
sweep_steps.insert(current_sweep_step, ["lidar", True, []])
except VirtualBumperException as e:
print(self.sim_time, "Bumper")
if distance(self.xyz, pos) < ARRIVAL_TOLERANCE:
current_sweep_step += 1
continue
sweep_steps.insert(current_sweep_step, ["rock", True, []])
sweep_steps.insert(current_sweep_step,
["straight", True, [-1, False]])
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 run(self):
try:
print('Wait for definition of last_position and yaw')
while self.last_position is None or self.yaw is None:
self.update() # define self.time
print('done at', self.time)
last_walk_start = 0.0
start_time = self.time
while self.time - start_time < timedelta(minutes=40):
additional_turn = 0
last_walk_start = self.time
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=4), 0.1)
with LidarCollisionMonitor(self):
if self.current_driver is None and not self.brakes_on:
self.go_straight(50.0,
timeout=timedelta(minutes=2))
else:
self.wait(timedelta(minutes=2)
) # allow for self driving, then timeout
self.update()
except ChangeDriverException as e:
continue
except (VirtualBumperException, LidarCollisionException) as e:
self.inException = True
# TODO: crashes if an exception (e.g., excess pitch) occurs while handling an exception (e.g., virtual/lidar bump)
print(self.time, repr(e))
last_walk_end = self.time
self.virtual_bumper = None
self.go_straight(-2.0, timeout=timedelta(seconds=10))
if last_walk_end - last_walk_start > timedelta(
seconds=20
): # if we went more than 20 secs, try to continue a step to the left
self.try_step_around()
else:
self.bus.publish('driving_recovery', False)
self.inException = False
print("Time elapsed since start of previous leg: %d sec" %
(last_walk_end.total_seconds() -
last_walk_start.total_seconds()))
if last_walk_end - last_walk_start > timedelta(seconds=20):
# if last step only ran short time before bumper, time for a large random turn
# if it ran long time, maybe worth trying going in the same direction
continue
additional_turn = 30
# next random walk direction should be between 30 and 150 degrees
# (no need to go straight back or keep going forward)
# if part of virtual bumper handling, add 30 degrees to avoid the obstacle more forcefully
deg_angle = self.rand.randrange(30 + additional_turn,
150 + additional_turn)
deg_sign = self.rand.randint(0, 1)
if deg_sign:
deg_angle = -deg_angle
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=20), 0.1)
self.turn(math.radians(deg_angle),
timeout=timedelta(seconds=30))
except ChangeDriverException as e:
continue
except VirtualBumperException:
self.inException = True
print(self.time, "Turn Virtual Bumper!")
self.virtual_bumper = None
if self.current_driver is not None:
# probably didn't throw in previous turn but during self driving
self.go_straight(-2.0, timeout=timedelta(seconds=10))
self.try_step_around()
self.turn(math.radians(-deg_angle),
timeout=timedelta(seconds=30))
self.inException = False
self.bus.publish('driving_recovery', False)
except BusShutdownException:
pass
def test_usage(self):
vb = VirtualBumper(timedelta(seconds=2.5), dist_radius_limit=0.1)
t0 = timedelta(seconds=12)
t_step = timedelta(seconds=1)
pose = [1.0, 2.0, math.pi]
vb.update_desired_speed(0.5, 0.0)
self.assertFalse(vb.collision())
vb.update_pose(t0, pose)
vb.update_pose(t0 + t_step, pose)
vb.update_pose(t0 + 2 * t_step, pose)
vb.update_pose(t0 + 3 * t_step, pose)
self.assertTrue(vb.collision())
vb.update_desired_speed(0.0, 0.0)
self.assertFalse(vb.collision())
def test_false_trigger_via_lora_pause(self):
# bug shown during LoRa command "Pause"
vb = VirtualBumper(timedelta(seconds=2), dist_radius_limit=0.1)
vb.update_desired_speed(0.5, 0.0)
vb.update_pose(timedelta(seconds=2.479321), (0.0, 0.0, 0.0))
vb.update_pose(timedelta(seconds=27.612698), (2.503, -0.013, -0.00977))
vb.update_desired_speed(0.0, 0.0)
vb.update_pose(timedelta(seconds=27.767734),
(2.507, -0.013, -0.0095993))
vb.update_pose(timedelta(seconds=30.458526),
(2.525, -0.014, -0.0076794487))
vb.update_pose(timedelta(seconds=37.808329),
(2.525, -0.014, -0.0089011791851))
self.assertFalse(vb.collision())
def test_motion(self):
vb = VirtualBumper(timedelta(seconds=2), dist_radius_limit=0.1)
t0 = timedelta(seconds=0)
t_step = timedelta(seconds=1)
pose = [0.0, 0.0, 0.0]
vb.update_pose(t0, pose)
vb.update_desired_speed(0.5, 0.0)
self.assertFalse(vb.collision())
pose = [0.1, 0.0, 0.0]
vb.update_pose(t0 + t_step, pose)
pose = [0.2, 0.0, 0.0]
vb.update_pose(t0 + 2 * t_step, pose)
self.assertFalse(vb.collision())
def __init__(self, config, bus):
self.bus = bus
bus.register("desired_speed", "pose2d", "artf_xyz", "pose3d", "stdout", "request_origin")
self.start_pose = None
self.traveled_dist = 0.0
self.time = None
self.max_speed = config['max_speed']
self.max_angular_speed = math.radians(60)
self.walldist = config['walldist']
self.timeout = timedelta(seconds=config['timeout'])
self.symmetric = config['symmetric'] # is robot symmetric?
self.dangerous_dist = config.get('dangerous_dist', 0.3)
self.min_safe_dist = config.get('min_safe_dist', 0.75)
self.safety_turning_coeff = config.get('safety_turning_coeff', 0.8)
virtual_bumper_sec = config.get('virtual_bumper_sec')
self.virtual_bumper = None
if virtual_bumper_sec is not None:
virtual_bumper_radius = config.get('virtual_bumper_radius', 0.1)
self.virtual_bumper = VirtualBumper(timedelta(seconds=virtual_bumper_sec), virtual_bumper_radius)
self.last_position = (0, 0, 0) # proper should be None, but we really start from zero
self.xyz = (0, 0, 0) # 3D position for mapping artifacts
self.xyz_quat = [0, 0, 0]
self.orientation = quaternion.identity()
self.yaw, self.pitch, self.roll = 0, 0, 0
self.yaw_offset = None # not defined, use first IMU reading
self.is_moving = None # unknown
self.scan = None # I should use class Node instead
self.flipped = False # by default use only front part
self.joint_angle_rad = [] # optinal angles, needed for articulated robots flip
self.stat = defaultdict(int)
self.voltage = []
self.artifacts = []
self.trace = Trace()
self.collision_detector_enabled = False
self.sim_time_sec = 0
self.lora_cmd = None
self.emergency_stop = None
self.monitors = [] # for Emergency Stop Exception
self.use_right_wall = config['right_wall']
self.use_center = False # navigate into center area (controlled by name ending by 'C')
self.is_virtual = config.get('virtual_world', False) # workaround to handle tunnel differences
self.front_bumper = False
self.rear_bumper = False
self.last_send_time = None
self.origin = None # unknown initial position
self.origin_quat = quaternion.identity()
self.offset = (0, 0, 0)
if 'init_offset' in config:
x, y, z = [d/1000.0 for d in config['init_offset']]
self.offset = (x, y, z)
self.init_path = None
if 'init_path' in config:
pts_s = [s.split(',') for s in config['init_path'].split(';')]
self.init_path = [(float(x), float(y)) for x, y in pts_s]
self.origin_error = False
self.robot_name = None # received with origin
scan_subsample = config.get('scan_subsample', 1)
obstacle_influence = config.get('obstacle_influence', 0.8)
direction_adherence = math.radians(config.get('direction_adherence', 90))
self.local_planner = LocalPlanner(
obstacle_influence=obstacle_influence,
direction_adherence=direction_adherence,
max_obstacle_distance=2.5,
scan_subsample=scan_subsample,
max_considered_obstacles=100)
self.use_return_trace = config.get('use_return_trace', True)
self.ref_scan = None
self.pause_start_time = None
if config.get('start_paused', False):
self.pause_start_time = timedelta() # paused from the very beginning
class SubTChallenge:
def __init__(self, config, bus):
self.bus = bus
bus.register("desired_speed", "pose2d", "artf_xyz", "pose3d", "stdout", "request_origin")
self.start_pose = None
self.traveled_dist = 0.0
self.time = None
self.max_speed = config['max_speed']
self.max_angular_speed = math.radians(60)
self.walldist = config['walldist']
self.timeout = timedelta(seconds=config['timeout'])
self.symmetric = config['symmetric'] # is robot symmetric?
self.dangerous_dist = config.get('dangerous_dist', 0.3)
self.min_safe_dist = config.get('min_safe_dist', 0.75)
self.safety_turning_coeff = config.get('safety_turning_coeff', 0.8)
virtual_bumper_sec = config.get('virtual_bumper_sec')
self.virtual_bumper = None
if virtual_bumper_sec is not None:
virtual_bumper_radius = config.get('virtual_bumper_radius', 0.1)
self.virtual_bumper = VirtualBumper(timedelta(seconds=virtual_bumper_sec), virtual_bumper_radius)
self.last_position = (0, 0, 0) # proper should be None, but we really start from zero
self.xyz = (0, 0, 0) # 3D position for mapping artifacts
self.xyz_quat = [0, 0, 0]
self.orientation = quaternion.identity()
self.yaw, self.pitch, self.roll = 0, 0, 0
self.yaw_offset = None # not defined, use first IMU reading
self.is_moving = None # unknown
self.scan = None # I should use class Node instead
self.flipped = False # by default use only front part
self.joint_angle_rad = [] # optinal angles, needed for articulated robots flip
self.stat = defaultdict(int)
self.voltage = []
self.artifacts = []
self.trace = Trace()
self.collision_detector_enabled = False
self.sim_time_sec = 0
self.lora_cmd = None
self.emergency_stop = None
self.monitors = [] # for Emergency Stop Exception
self.use_right_wall = config['right_wall']
self.use_center = False # navigate into center area (controlled by name ending by 'C')
self.is_virtual = config.get('virtual_world', False) # workaround to handle tunnel differences
self.front_bumper = False
self.rear_bumper = False
self.last_send_time = None
self.origin = None # unknown initial position
self.origin_quat = quaternion.identity()
self.offset = (0, 0, 0)
if 'init_offset' in config:
x, y, z = [d/1000.0 for d in config['init_offset']]
self.offset = (x, y, z)
self.init_path = None
if 'init_path' in config:
pts_s = [s.split(',') for s in config['init_path'].split(';')]
self.init_path = [(float(x), float(y)) for x, y in pts_s]
self.origin_error = False
self.robot_name = None # received with origin
scan_subsample = config.get('scan_subsample', 1)
obstacle_influence = config.get('obstacle_influence', 0.8)
direction_adherence = math.radians(config.get('direction_adherence', 90))
self.local_planner = LocalPlanner(
obstacle_influence=obstacle_influence,
direction_adherence=direction_adherence,
max_obstacle_distance=2.5,
scan_subsample=scan_subsample,
max_considered_obstacles=100)
self.use_return_trace = config.get('use_return_trace', True)
self.ref_scan = None
self.pause_start_time = None
if config.get('start_paused', False):
self.pause_start_time = timedelta() # paused from the very beginning
def send_speed_cmd(self, speed, angular_speed):
if self.virtual_bumper is not None:
self.virtual_bumper.update_desired_speed(speed, angular_speed)
self.bus.publish('desired_speed', [round(speed*1000), round(math.degrees(angular_speed)*100)])
# Corresponds to gc.disable() in __main__. See a comment there for more details.
gc.collect()
def maybe_remember_artifact(self, artifact_data, artifact_xyz):
for stored_data, (x, y, z) in self.artifacts:
if distance3D((x, y, z), artifact_xyz) < 4.0:
# in case of uncertain type, rather report both
if stored_data == artifact_data:
return False
self.artifacts.append((artifact_data, artifact_xyz))
return True
def go_straight(self, how_far, timeout=None):
print(self.time, "go_straight %.1f (speed: %.1f)" % (how_far, self.max_speed), self.last_position)
start_pose = self.last_position
if how_far >= 0:
self.send_speed_cmd(self.max_speed, 0.0)
else:
self.send_speed_cmd(-self.max_speed, 0.0)
start_time = self.time
while distance(start_pose, self.last_position) < abs(how_far):
self.update()
if timeout is not None and self.time - start_time > timeout:
print("go_straight - TIMEOUT!")
break
self.send_speed_cmd(0.0, 0.0)
def go_safely(self, desired_direction):
if self.local_planner is None:
safety, safe_direction = 1.0, desired_direction
else:
safety, safe_direction = self.local_planner.recommend(desired_direction)
#print(self.time,"safety:%f desired:%f safe_direction:%f"%(safety, desired_direction, safe_direction))
#desired_angular_speed = 1.2 * safe_direction
desired_angular_speed = 0.9 * safe_direction
size = len(self.scan)
dist = min_dist(self.scan[size//3:2*size//3])
if dist < self.min_safe_dist: # 2.0:
# desired_speed = self.max_speed * (1.2/2.0) * (dist - 0.4) / 1.6
desired_speed = self.max_speed * (dist - self.dangerous_dist) / (self.min_safe_dist - self.dangerous_dist)
else:
desired_speed = self.max_speed # was 2.0
desired_speed = desired_speed * (1.0 - self.safety_turning_coeff * min(self.max_angular_speed, abs(desired_angular_speed)) / self.max_angular_speed)
if self.flipped:
self.send_speed_cmd(-desired_speed, desired_angular_speed) # ??? angular too??!
else:
self.send_speed_cmd(desired_speed, desired_angular_speed)
return safety
def turn(self, angle, with_stop=True, speed=0.0, timeout=None):
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)
start_time = self.time
while abs(normalizeAnglePIPI(start_pose[2] - self.last_position[2])) < abs(angle):
self.update()
if timeout is not None and self.time - start_time > timeout:
print(self.time, "turn - TIMEOUT!")
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()
if not self.is_moving:
break
print(self.time, 'stop at', self.time - start_time)
def stop(self):
self.send_speed_cmd(0.0, 0.0)
start_time = self.time
while self.time - start_time < timedelta(seconds=20):
self.update()
if not self.is_moving:
break
print(self.time, 'stop at', self.time - start_time, self.is_moving)
def follow_wall(self, radius, right_wall=False, timeout=timedelta(hours=3), dist_limit=None, flipped=False,
pitch_limit=None, roll_limit=None):
# make sure that we will start with clean data
if flipped:
self.scan = None
self.flipped = True
reason = None # termination reason is not defined yet
start_dist = self.traveled_dist
start_time = self.sim_time_sec
last_pause_time = timedelta() # for multiple Pause
current_pause_time = timedelta()
while self.sim_time_sec - start_time < (timeout + last_pause_time + current_pause_time).total_seconds():
try:
channel = self.update()
if (channel == 'scan' and not self.flipped) or (channel == 'scan_back' and self.flipped) or channel == 'scan360':
if self.pause_start_time is None:
if self.use_center:
desired_direction = 0
else:
desired_direction = follow_wall_angle(self.scan, radius=radius, right_wall=right_wall)
self.go_safely(desired_direction)
if dist_limit is not None:
if dist_limit < abs(self.traveled_dist - start_dist): # robot can return backward -> abs()
print(self.time, 'Distance limit reached! At', self.traveled_dist, self.traveled_dist - start_dist)
reason = REASON_DIST_REACHED
break
if pitch_limit is not None and self.pitch is not None:
if abs(self.pitch) > pitch_limit:
print(self.time, 'Pitch limit triggered termination: (pitch %.1f)' % math.degrees(self.pitch))
reason = REASON_PITCH_LIMIT
break
if roll_limit is not None and self.roll is not None:
if abs(self.roll) > roll_limit:
print(self.time, 'Roll limit triggered termination: (roll %.1f)' % math.degrees(self.roll))
reason = REASON_ROLL_LIMIT
break
if self.virtual_bumper is not None and self.virtual_bumper.collision():
print(self.time, "VIRTUAL BUMPER - collision")
self.go_straight(-0.3, timeout=timedelta(seconds=10))
reason = REASON_VIRTUAL_BUMPER
break
if self.front_bumper and not flipped:
print(self.time, "FRONT BUMPER - collision")
self.go_straight(-0.3, timeout=timedelta(seconds=10))
reason = REASON_FRONT_BUMPER
break
if self.rear_bumper and flipped:
print(self.time, "REAR BUMPER - collision")
self.go_straight(-0.3, timeout=timedelta(seconds=10))
reason = REASON_REAR_BUMPER
break
if self.lora_cmd is not None:
# the "GoHome" command must be accepted only on the way there and not on the return home
if dist_limit is None and self.lora_cmd == LORA_GO_HOME_CMD:
print(self.time, 'LoRa cmd - GoHome')
self.lora_cmd = None
reason = REASON_LORA
break
if self.lora_cmd == LORA_PAUSE_CMD:
print(self.time, 'LoRa cmd - Pause')
self.send_speed_cmd(0, 0)
if self.pause_start_time is None:
# ignore repeated Pause
self.pause_start_time = self.time
self.lora_cmd = None
elif self.lora_cmd == LORA_CONTINUE_CMD:
print(self.time, 'LoRa cmd - Continue')
if self.pause_start_time is not None:
# ignore Continue without Pause
last_pause_time += self.time - self.pause_start_time
self.pause_start_time = None
self.lora_cmd = None
if self.pause_start_time is not None:
current_pause_time = self.time - self.pause_start_time
else:
current_pause_time = timedelta()
except Collision:
assert not self.collision_detector_enabled # collision disables further notification
before_stop = self.xyz
self.stop()
after_stop = self.xyz
print("Pose Jump:", before_stop, after_stop)
self.xyz = before_stop
self.go_straight(-1)
self.stop()
if right_wall:
turn_angle = math.pi / 2
else:
turn_angle = -math.pi / 2
self.turn(turn_angle, with_stop=True)
self.go_straight(1.5)
self.stop()
self.turn(-turn_angle, with_stop=True)
self.go_straight(1.5)
self.stop()
self.collision_detector_enabled = True
self.scan = None
self.flipped = False
return self.traveled_dist - start_dist, reason
def return_home(self, timeout, home_threshold=None):
if home_threshold is None:
HOME_THRESHOLD = 5.0
else:
HOME_THRESHOLD = home_threshold
SHORTCUT_RADIUS = 2.3
MAX_TARGET_DISTANCE = 5.0
MIN_TARGET_DISTANCE = 1.0
assert(MAX_TARGET_DISTANCE > SHORTCUT_RADIUS) # Because otherwise we could end up with a target point more distant from home than the robot.
print('Wait and get ready for return')
self.send_speed_cmd(0, 0)
self.wait(dt=timedelta(seconds=3.0))
original_trace = copy.deepcopy(self.trace)
self.trace.prune(SHORTCUT_RADIUS)
self.wait(dt=timedelta(seconds=2.0))
print('done.')
start_time = self.sim_time_sec
target_distance = MAX_TARGET_DISTANCE
count_down = 0
while distance3D(self.xyz, (0, 0, 0)) > HOME_THRESHOLD and self.sim_time_sec - start_time < timeout.total_seconds():
channel = self.update()
if (channel == 'scan' and not self.flipped) or (channel == 'scan_back' and self.flipped) or (channel == 'scan360'):
if target_distance == MIN_TARGET_DISTANCE:
target_x, target_y = original_trace.where_to(self.xyz, target_distance)[:2]
else:
target_x, target_y = self.trace.where_to(self.xyz, target_distance)[:2]
# print(self.time, self.xyz, (target_x, target_y), math.degrees(self.yaw))
x, y = self.xyz[:2]
desired_direction = math.atan2(target_y - y, target_x - x) - self.yaw
if self.flipped:
desired_direction += math.pi # symmetry
for angle in self.joint_angle_rad:
desired_direction -= angle
safety = self.go_safely(desired_direction)
if safety < 0.2:
print(self.time, "Safety low!", safety, desired_direction)
target_distance = MIN_TARGET_DISTANCE
count_down = 300
if count_down > 0:
count_down -= 1
if count_down == 0:
target_distance = MAX_TARGET_DISTANCE
print(self.time, "Recovery to original", target_distance)
print('return_home: dist', distance3D(self.xyz, (0, 0, 0)), 'time(sec)', self.sim_time_sec - start_time)
def follow_trace(self, trace, timeout, max_target_distance=5.0, safety_limit=None):
print('Follow trace')
END_THRESHOLD = 2.0
start_time = self.sim_time_sec
print('MD', self.xyz, distance3D(self.xyz, trace.trace[0]), trace.trace)
while distance3D(self.xyz, trace.trace[0]) > END_THRESHOLD and self.sim_time_sec - start_time < timeout.total_seconds():
if self.update() == 'scan':
target_x, target_y = trace.where_to(self.xyz, max_target_distance)[:2]
x, y = self.xyz[:2]
# print((x, y), (target_x, target_y))
desired_direction = math.atan2(target_y - y, target_x - x) - self.yaw
safety = self.go_safely(desired_direction)
if safety_limit is not None and safety < safety_limit:
print('Danger! Safety limit for follow trace reached!', safety, safety_limit)
break
print('End of follow trace(sec)', self.sim_time_sec - start_time)
def register(self, callback):
self.monitors.append(callback)
return callback
def unregister(self, callback):
assert callback in self.monitors
self.monitors.remove(callback)
def on_pose2d(self, timestamp, data):
x, y, heading = data
pose = (x / 1000.0, y / 1000.0, math.radians(heading / 100.0))
if self.last_position is not None:
self.is_moving = (self.last_position != pose)
dist = math.hypot(pose[0] - self.last_position[0], pose[1] - self.last_position[1])
direction = ((pose[0] - self.last_position[0]) * math.cos(self.last_position[2]) +
(pose[1] - self.last_position[1]) * math.sin(self.last_position[2]))
if direction < 0:
dist = -dist
else:
dist = 0.0
self.last_position = pose
if self.start_pose is None:
self.start_pose = pose
self.traveled_dist += dist
x, y, z = self.xyz
x += math.cos(self.pitch) * math.cos(self.yaw) * dist
y += math.cos(self.pitch) * math.sin(self.yaw) * dist
z += math.sin(self.pitch) * dist
x0, y0, z0 = self.offset
self.last_send_time = self.bus.publish('pose2d', [round((x + x0) * 1000), round((y + y0) * 1000),
round(math.degrees(self.yaw) * 100)])
if self.virtual_bumper is not None:
if self.is_virtual:
self.virtual_bumper.update_pose(timedelta(seconds=self.sim_time_sec), pose)
else:
self.virtual_bumper.update_pose(self.time, pose)
self.xyz = x, y, z
self.trace.update_trace(self.xyz)
# pose3d
dist3d = quaternion.rotate_vector([dist, 0, 0], self.orientation)
self.xyz_quat = [a + b for a, b in zip(self.xyz_quat, dist3d)]
xyz_quat = [p + o for p, o in zip(self.xyz_quat, self.offset)]
self.bus.publish('pose3d', [xyz_quat, self.orientation])
def on_acc(self, timestamp, data):
acc = [x / 1000.0 for x in data]
gacc = np.matrix([[0., 0., 9.80]]) # Gravitational acceleration.
cos_pitch = math.cos(self.pitch)
sin_pitch = math.sin(self.pitch)
# TODO: Once roll is correct, incorporate it here too.
egacc = np.matrix([ # Expected gravitational acceleration given known pitch.
[cos_pitch, 0., sin_pitch],
[0., 1., 0.],
[-sin_pitch, 0., cos_pitch]
]) * gacc.T
cacc = np.asarray(acc) - egacc.T # Corrected acceleration (without gravitational acceleration).
magnitude = math.hypot(cacc[0, 0], cacc[0, 1])
# used to be 12 - see https://bitbucket.org/osrf/subt/issues/166/expected-x2-acceleration
if magnitude > 200: #18.0:
print(self.time, 'Collision!', acc, 'reported:', self.collision_detector_enabled)
if self.collision_detector_enabled:
self.collision_detector_enabled = False
raise Collision()
def on_artf(self, timestamp, data):
artifact_data, deg_100th, dist_mm = data
x, y, z = self.xyz
x0, y0, z0 = self.offset
angle, dist = self.yaw + math.radians(deg_100th / 100.0), dist_mm / 1000.0
ax = x0 + x + math.cos(angle) * dist
ay = y0 + y + math.sin(angle) * dist
az = z0 + z
if -20 < ax < 0 and -10 < ay < 10:
# filter out elements on staging area
self.stdout(self.time, 'Robot at:', (ax, ay, az))
else:
if self.maybe_remember_artifact(artifact_data, (ax, ay, az)):
self.bus.publish('artf_xyz', [[artifact_data, round(ax*1000), round(ay*1000), round(az*1000)]])
def on_joint_angle(self, timestamp, data):
# angles for articulated robot in 1/100th of degree
self.joint_angle_rad = [math.radians(a/100) for a in data]
def on_bumpers_front(self, timestamp, data):
self.front_bumper = max(data) # array of boolean values where True means collision
def on_bumpers_rear(self, timestamp, data):
self.rear_bumper = max(data) # array of boolean values where True means collision
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 wait(self, dt, use_sim_time=False): # TODO refactor to some common class
if use_sim_time:
start_sim_time_sec = self.sim_time_sec
while self.sim_time_sec - start_sim_time_sec < dt.total_seconds():
self.update()
else:
if self.time is None:
self.update()
start_time = self.time
while self.time - start_time < dt:
self.update()
def stdout(self, *args, **kwargs):
output = StringIO()
print(*args, file=output, **kwargs)
contents = output.getvalue().strip()
output.close()
self.bus.publish('stdout', contents)
print(contents)
#############################################
def system_nav_trace(self, path=None):
"""
Navigate along line
"""
dx, dy, __ = self.offset
trace = Trace()
trace.add_line_to((-dx, -dy, 0))
if path is not None:
for x, y in path:
trace.add_line_to((x - dx, y - dy, 0))
trace.reverse()
self.follow_trace(trace, timeout=timedelta(seconds=120), max_target_distance=2.5, safety_limit=0.2)
def robust_follow_wall(self, radius, right_wall=False, timeout=timedelta(hours=3), dist_limit=None, flipped=False,
pitch_limit=None, roll_limit=None):
"""
Handle multiple re-tries with increasing distance from the wall if necessary
"""
allow_virtual_flip = self.symmetric
walldist = self.walldist
total_dist = 0.0
start_time = self.sim_time_sec
overall_timeout = timeout
while self.sim_time_sec - start_time < overall_timeout.total_seconds():
if self.sim_time_sec - start_time > overall_timeout.total_seconds():
print('Total Timeout Reached', overall_timeout.total_seconds())
break
timeout = timedelta(seconds=overall_timeout.total_seconds() - (self.sim_time_sec - start_time))
print('Current timeout', timeout)
dist, reason = self.follow_wall(radius=walldist, right_wall=right_wall, timeout=timeout, flipped=flipped,
pitch_limit=LIMIT_PITCH, roll_limit=LIMIT_ROLL)
total_dist += dist
if reason is None or reason in [REASON_LORA,]:
break
walldist += 0.2
if not allow_virtual_flip:
# Eduro not supported yet
if reason in [REASON_VIRTUAL_BUMPER,]:
# virtual bumper already tried to backup a bit
continue
# for large slope rather return home
break
# re-try with bigger distance
print(self.time, "Re-try because of", reason)
for repeat in range(2):
self.follow_wall(radius=walldist, right_wall=not right_wall, timeout=timedelta(seconds=30), dist_limit=2.0,
flipped=not flipped, pitch_limit=RETURN_LIMIT_PITCH, roll_limit=RETURN_LIMIT_ROLL)
dist, reason = self.follow_wall(radius=walldist, right_wall=right_wall, timeout=timedelta(seconds=40), dist_limit=4.0,
pitch_limit=LIMIT_PITCH, roll_limit=LIMIT_ROLL, flipped=flipped)
total_dist += dist
if reason is None:
break
if reason in [REASON_LORA, REASON_DIST_REACHED]:
break
walldist += 0.2
walldist = self.walldist
if reason in [REASON_LORA,]:
break
def play_system_track(self):
print("SubT Challenge Ver1!")
try:
with EmergencyStopMonitor(self):
allow_virtual_flip = self.symmetric
if distance(self.offset, (0, 0)) > 0.1 or self.init_path is not None:
self.system_nav_trace(self.init_path)
# self.go_straight(2.5) # go to the tunnel entrance - commented our for testing
walldist = self.walldist
total_dist = 0.0
start_time = self.sim_time_sec
while self.sim_time_sec - start_time < self.timeout.total_seconds():
if self.sim_time_sec - start_time > self.timeout.total_seconds():
print('Total Timeout Reached', self.timeout.total_seconds())
break
timeout = timedelta(seconds=self.timeout.total_seconds() - (self.sim_time_sec - start_time))
print('Current timeout', timeout)
dist, reason = self.follow_wall(radius=walldist, right_wall=self.use_right_wall, timeout=timeout,
pitch_limit=LIMIT_PITCH, roll_limit=LIMIT_ROLL)
total_dist += dist
if reason is None or reason in [REASON_LORA,]:
break
walldist += 0.2
if not allow_virtual_flip:
# Eduro not supported yet
if reason in [REASON_VIRTUAL_BUMPER,]:
# virtual bumper already tried to backup a bit
continue
# for large slope rather return home
break
# re-try with bigger distance
print(self.time, "Re-try because of", reason)
for repeat in range(2):
self.follow_wall(radius=walldist, right_wall=not self.use_right_wall, timeout=timedelta(seconds=30), dist_limit=2.0,
flipped=allow_virtual_flip, pitch_limit=RETURN_LIMIT_PITCH, roll_limit=RETURN_LIMIT_ROLL)
dist, reason = self.follow_wall(radius=walldist, right_wall=self.use_right_wall, timeout=timedelta(seconds=40), dist_limit=4.0,
pitch_limit=LIMIT_PITCH, roll_limit=LIMIT_ROLL)
total_dist += dist
if reason is None:
break
if reason in [REASON_LORA, REASON_DIST_REACHED]:
break
walldist += 0.2
walldist = self.walldist
if reason in [REASON_LORA,]:
break
if self.use_return_trace and self.local_planner is not None:
self.stdout(self.time, "Going HOME %.3f" % dist, reason)
if allow_virtual_flip:
self.flipped = True # return home backwards
self.scan = None
self.return_home(2 * self.timeout, home_threshold=1.0)
self.send_speed_cmd(0, 0)
else:
print(self.time, "Going HOME", reason)
if not allow_virtual_flip:
self.turn(math.radians(90), timeout=timedelta(seconds=20))
self.turn(math.radians(90), timeout=timedelta(seconds=20))
self.robust_follow_wall(radius=self.walldist, right_wall=not self.use_right_wall, timeout=3*self.timeout, dist_limit=3*total_dist,
flipped=allow_virtual_flip, pitch_limit=RETURN_LIMIT_PITCH, roll_limit=RETURN_LIMIT_ROLL)
if self.artifacts:
self.bus.publish('artf_xyz', [[artifact_data, round(x*1000), round(y*1000), round(z*1000)]
for artifact_data, (x, y, z) in self.artifacts])
except EmergencyStopException:
print(self.time, "EMERGENCY STOP - terminating")
self.send_speed_cmd(0, 0)
self.wait(timedelta(seconds=3))
#############################################
def go_to_entrance(self):
"""
Navigate to the base station tile end
"""
dx, dy, __ = self.offset
trace = Trace() # starts by default at (0, 0, 0) and the robots are placed X = -7.5m (variable Y)
trace.add_line_to((-4.5 - dx, -dy, 0)) # in front of the tunnel/entrance
trace.add_line_to((2.5 - dx, -dy, 0)) # 2.5m inside
trace.reverse()
self.follow_trace(trace, timeout=timedelta(seconds=30), max_target_distance=2.5, safety_limit=0.2)
def play_virtual_part(self):
self.stdout("Waiting for origin ...")
self.origin = None # invalidate origin
self.origin_error = False
self.bus.publish('request_origin', True)
while self.origin is None and not self.origin_error:
self.update()
self.stdout('Origin:', self.origin, self.robot_name)
if self.origin is not None:
x, y, z = self.origin
x1, y1, z1 = self.xyz
self.offset = x - x1, y - y1, z - z1
self.stdout('Offset:', self.offset)
heading = quaternion.heading(self.origin_quat)
self.stdout('heading', math.degrees(heading), 'angle', math.degrees(math.atan2(-y, -x)), 'dist', math.hypot(x, y))
self.go_to_entrance()
else:
# lost in tunnel
self.stdout('Lost in tunnel:', self.origin_error, self.offset)
start_time = self.sim_time_sec
for loop in range(100):
self.collision_detector_enabled = True
if self.sim_time_sec - start_time > self.timeout.total_seconds():
print('Total Timeout Reached', self.timeout.total_seconds())
break
timeout = timedelta(seconds=self.timeout.total_seconds() - (self.sim_time_sec - start_time))
print('Current timeout', timeout)
dist, reason = self.follow_wall(radius=self.walldist, right_wall=self.use_right_wall, # was radius=0.9
timeout=timeout, pitch_limit=LIMIT_PITCH, roll_limit=None)
self.collision_detector_enabled = False
if reason == REASON_VIRTUAL_BUMPER:
assert self.virtual_bumper is not None
self.virtual_bumper.reset_counters()
# the robot ended in cycle probably
self.return_home(timedelta(seconds=10))
# try something crazy if you do not have other ideas ...
before_center = self.use_center
self.use_center = True
dist, reason = self.follow_wall(radius=self.walldist, right_wall=self.use_right_wall, # was radius=0.9
timeout=timedelta(seconds=60), pitch_limit=LIMIT_PITCH, roll_limit=None)
self.use_center = before_center
if reason is None or reason != REASON_PITCH_LIMIT:
continue
if reason is None or reason != REASON_PITCH_LIMIT:
break
self.stdout(self.time, "Microstep HOME %d %.3f" % (loop, dist), reason)
self.go_straight(-0.3, timeout=timedelta(seconds=10))
self.return_home(timedelta(seconds=10))
self.stdout("Artifacts:", self.artifacts)
self.stdout(self.time, "Going HOME %.3f" % dist, reason)
self.return_home(2 * self.timeout)
self.send_speed_cmd(0, 0)
if self.artifacts:
self.bus.publish('artf_xyz', [[artifact_data, round(x*1000), round(y*1000), round(z*1000)]
for artifact_data, (x, y, z) in self.artifacts])
self.wait(timedelta(seconds=10), use_sim_time=True)
def dumplog(self):
import os
filename = self.bus.logger.filename # deep hack
self.stdout("Dump Log:", filename)
size = statinfo = os.stat(filename).st_size
self.stdout("Size:", size)
with open(filename, 'rb') as f:
for i in range(0, size, 100):
self.stdout(i, f.read(100))
self.stdout("Dump END")
def play_virtual_track(self):
self.stdout("SubT Challenge Ver64!")
self.stdout("Waiting for robot_name ...")
while self.robot_name is None:
self.update()
self.stdout('robot_name:', self.robot_name)
if self.use_right_wall == 'auto':
self.use_right_wall = self.robot_name.endswith('R')
self.use_center = self.robot_name.endswith('C')
self.stdout('Use right wall:', self.use_right_wall)
times_sec = [int(x) for x in self.robot_name[1:-1].split('F')]
self.stdout('Using times', times_sec)
# add extra sleep to give a chance to the other robot (based on name)
self.wait(timedelta(seconds=times_sec[0]), use_sim_time=True)
# potential wrong artifacts:
self.stdout('Artifacts before start:', self.artifacts)
for timeout_sec in times_sec[1:]:
self.timeout = timedelta(seconds=timeout_sec)
self.play_virtual_part()
self.stdout('Final xyz:', self.xyz)
x, y, z = self.xyz
x0, y0, z0 = self.offset
self.stdout('Final xyz (DARPA coord system):', (x + x0, y + y0, z + z0))
self.wait(timedelta(seconds=30), use_sim_time=True)
# self.dumplog()
# self.wait(timedelta(seconds=10), use_sim_time=True)
#############################################
def play(self):
if self.is_virtual:
return self.play_virtual_track()
else:
return self.play_system_track()
def start(self):
self.thread = threading.Thread(target=self.play)
self.thread.start()
def is_alive(self):
return self.thread.is_alive()
def request_stop(self):
self.bus.shutdown()
def join(self, timeout=None):
self.thread.join(timeout)
def test_turn_in_place(self):
# first current version without checking angle
vb = VirtualBumper(timedelta(seconds=2), dist_radius_limit=0.1)
vb.update_desired_speed(0.0, math.pi/2)
self.assertTrue(vb.should_be_moving)
vb.update_pose(timedelta(seconds=0), (0.0, 0.0, 0.0))
vb.update_pose(timedelta(seconds=1), (0.0, 0.0, math.pi/2))
vb.update_pose(timedelta(seconds=2), (0.0, 0.0, math.pi))
self.assertTrue(vb.collision())
# now with angle
vb = VirtualBumper(timedelta(seconds=2), dist_radius_limit=0.1, angle_limit=math.pi/2)
vb.update_desired_speed(0.0, math.pi/2)
self.assertTrue(vb.should_be_moving)
vb.update_pose(timedelta(seconds=0), (0.0, 0.0, 0.0))
vb.update_pose(timedelta(seconds=1), (0.0, 0.0, math.pi/2))
vb.update_pose(timedelta(seconds=2), (0.0, 0.0, math.pi))
self.assertFalse(vb.collision())
def run(self):
try:
self.wait_for_init()
self.set_light_intensity("1.0")
self.set_brakes(False)
# some random manual starting moves to choose from
# self.go_straight(-0.1, timeout=timedelta(seconds=20))
# self.go_straight(-2, timeout=timedelta(seconds=20))
# self.turn(math.radians(45), timeout=timedelta(seconds=20))
# self.set_cam_angle(CAMERA_ANGLE_HOMEBASE)
if SKIP_CUBESAT_SUCCESS:
# skip cubesat
self.cubesat_success = True
self.follow_object(['homebase'])
else:
# regular launch
self.follow_object(['cubesat', 'homebase'])
# self.homebase_arrival_success = True
# self.cubesat_success = True
# self.follow_object(['homebase'])
# self.follow_object(['basemarker'])
# self.current_driver = 'basemarker'
# self.publish("desired_movement", [10, 9000, 10])
# self.wait(timedelta(seconds=10))
last_walk_start = 0.0
start_time = self.sim_time
while self.sim_time - start_time < timedelta(minutes=45):
additional_turn = 0
last_walk_start = self.sim_time
# TURN 360
# TODO:
# if looking for multiple objects, sweep multiple times, each looking only for one object (objects listed in order of priority)
# this way we won't start following homebase when cubesat was also visible, but later in the sweep
# alternatively, sweep once without immediately reacting to matches but note match angles
# then turn back to the direction of the top priority match
if self.current_driver is None and not self.brakes_on:
if len(self.objects_to_follow) > 1:
self.full_360_scan = True
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=20), 0.1)
with LidarCollisionMonitor(self):
# start each straight stretch by looking around first
# if cubesat already found, we are looking for homebase, no need to lift camera as much
self.set_cam_angle(
CAMERA_ANGLE_DRIVING if self.
cubesat_success else CAMERA_ANGLE_LOOKING)
self.turn(math.radians(360),
timeout=timedelta(seconds=20))
except ChangeDriverException as e:
print(self.sim_time,
"Turn interrupted by driver: %s" % e)
self.full_360_scan = False
self.full_360_objects = {}
continue
# proceed to straight line drive where we wait; straight line exception handling is better applicable for follow-object drive
except (VirtualBumperException,
LidarCollisionException) as e:
self.inException = True
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
print(self.sim_time,
"Turn 360 degrees Virtual Bumper!")
self.virtual_bumper = None
# recovery from an exception while turning CCW is to turn CW somewhat
deg_angle = self.rand.randrange(-180, -90)
self.turn(math.radians(deg_angle),
timeout=timedelta(seconds=10))
self.inException = False
self.bus.publish('driving_recovery', False)
if len(self.objects_to_follow) > 1:
print(
self.sim_time, "app: 360deg scan: " +
str([[a, median(self.full_360_objects[a])]
for a in self.full_360_objects.keys()]))
for o in self.objects_to_follow:
if o in self.full_360_objects.keys():
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=20), 0.1)
with LidarCollisionMonitor(self):
self.turn(
median(self.full_360_objects[o]) -
self.yaw,
timeout=timedelta(seconds=20))
except BusShutdownException:
raise
except:
# in case it gets stuck turning, just hand over driving to main without completing the desired turn
pass
break
self.full_360_scan = False
self.full_360_objects = {}
else:
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=20), 0.1)
with LidarCollisionMonitor(self):
self.wait(timedelta(minutes=2)
) # allow for self driving, then timeout
except ChangeDriverException as e:
# if driver lost, start by turning 360; if driver changed, wait here again
continue
except (VirtualBumperException,
LidarCollisionException) as e:
print(self.sim_time, "Follow-object Virtual Bumper")
self.inException = True
print(self.sim_time, repr(e))
last_walk_end = self.sim_time
self.virtual_bumper = None
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
self.go_straight(
-2.0, timeout=timedelta(seconds=10)
) # this should be reasonably safe, we just came from there
self.try_step_around()
self.inException = False
self.bus.publish('driving_recovery', False)
# GO STRAIGHT
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=4), 0.1)
with LidarCollisionMonitor(self):
if self.current_driver is None and not self.brakes_on:
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
self.go_straight(30.0,
timeout=timedelta(minutes=2))
else:
self.wait(timedelta(minutes=2)
) # allow for self driving, then timeout
self.update()
except ChangeDriverException as e:
continue
except (VirtualBumperException, LidarCollisionException) as e:
print(self.sim_time,
"Go Straight or Follow-object Virtual Bumper")
self.inException = True
# TODO: crashes if an exception (e.g., excess pitch) occurs while handling an exception (e.g., virtual/lidar bump)
print(self.sim_time, repr(e))
last_walk_end = self.sim_time
self.virtual_bumper = None
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
self.go_straight(
-2.0, timeout=timedelta(seconds=10)
) # this should be reasonably safe, we just came from there
if last_walk_end - last_walk_start > timedelta(
seconds=20
): # if we went more than 20 secs, try to continue a step to the left
# TODO: this is not necessarily safe, need to protect against pitch, etc again
self.try_step_around()
else:
self.bus.publish('driving_recovery', False)
self.inException = False
print("Time elapsed since start of previous leg: %d sec" %
(last_walk_end.total_seconds() -
last_walk_start.total_seconds()))
if last_walk_end - last_walk_start > timedelta(seconds=40):
# if last step only ran short time before bumper (this includes bumper timeout time), time for a large random turn
# if it ran long time, maybe worth trying going in the same direction
continue
additional_turn = 30
# next random walk direction should be between 30 and 150 degrees
# (no need to go straight back or keep going forward)
# if part of virtual bumper handling, add 30 degrees to avoid the obstacle more forcefully
# TURN RANDOMLY
deg_angle = self.rand.randrange(30 + additional_turn,
150 + additional_turn)
deg_sign = self.rand.randint(0, 1)
if deg_sign:
deg_angle = -deg_angle
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=20), 0.1)
with LidarCollisionMonitor(self):
self.turn(math.radians(deg_angle),
timeout=timedelta(seconds=30))
except ChangeDriverException as e:
continue
except (VirtualBumperException, LidarCollisionException):
self.inException = True
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
print(self.sim_time, "Random Turn Virtual Bumper!")
self.virtual_bumper = None
if self.current_driver is not None:
# probably didn't throw in previous turn but during self driving
self.go_straight(-2.0, timeout=timedelta(seconds=10))
self.try_step_around()
self.turn(math.radians(-deg_angle),
timeout=timedelta(seconds=30))
self.inException = False
self.bus.publish('driving_recovery', False)
except BusShutdownException:
pass
def run(self):
try:
print('Wait for definition of last_position and yaw')
while self.last_position is None or self.yaw is None:
self.update() # define self.time
print('done at', self.time)
self.set_brakes(False)
# some random manual starting moves to choose from
# self.go_straight(-0.1, timeout=timedelta(seconds=20))
# self.go_straight(-2, timeout=timedelta(seconds=20))
# self.turn(math.radians(45), timeout=timedelta(seconds=20))
# self.set_cam_angle(CAMERA_ANGLE_HOMEBASE)
# self.follow_object(['basemarker'])
# self.current_driver = 'basemarker'
if SKIP_CUBESAT_SUCCESS:
# skip cubesat
self.cubesat_success = True
self.follow_object(['homebase'])
else:
# regular launch
self.follow_object(['cubesat', 'homebase'])
# self.publish("desired_movement", [10, 9000, 10])
# self.wait(timedelta(seconds=10))
last_walk_start = 0.0
start_time = self.time
while self.time - start_time < timedelta(minutes=40):
additional_turn = 0
last_walk_start = self.time
# TURN 360
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=20), 0.1)
with LidarCollisionMonitor(self):
if self.current_driver is None and not self.brakes_on:
# start each straight stretch by looking around first
self.set_cam_angle(CAMERA_ANGLE_LOOKING)
self.turn(math.radians(360),
timeout=timedelta(seconds=20))
else:
self.wait(timedelta(minutes=2)
) # allow for self driving, then timeout
except ChangeDriverException as e:
print(self.time, "Turn interrupted by driver: %s" % e)
continue
except (VirtualBumperException, LidarCollisionException):
self.inException = True
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
print(self.time, "Turn Virtual Bumper!")
# TODO: if detector takes over driving within initial turn, rover may be actually going straight at this moment
# also, it may be simple timeout, not a crash
self.virtual_bumper = None
# recovery from an exception while turning CCW is to turn CW somewhat
deg_angle = self.rand.randrange(-180, -90)
self.turn(math.radians(deg_angle),
timeout=timedelta(seconds=10))
self.inException = False
self.bus.publish('driving_recovery', False)
# GO STRAIGHT
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=4), 0.1)
with LidarCollisionMonitor(self):
if self.current_driver is None and not self.brakes_on:
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
self.go_straight(50.0,
timeout=timedelta(minutes=2))
else:
self.wait(timedelta(minutes=2)
) # allow for self driving, then timeout
self.update()
except ChangeDriverException as e:
continue
except (VirtualBumperException, LidarCollisionException) as e:
self.inException = True
# TODO: crashes if an exception (e.g., excess pitch) occurs while handling an exception (e.g., virtual/lidar bump)
print(self.time, repr(e))
last_walk_end = self.time
self.virtual_bumper = None
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
self.go_straight(
-2.0, timeout=timedelta(seconds=10)
) # this should be reasonably safe, we just came from there
if last_walk_end - last_walk_start > timedelta(
seconds=20
): # if we went more than 20 secs, try to continue a step to the left
# TODO: this is not necessarily safe, need to protect against pitch, etc again
self.try_step_around()
else:
self.bus.publish('driving_recovery', False)
self.inException = False
print("Time elapsed since start of previous leg: %d sec" %
(last_walk_end.total_seconds() -
last_walk_start.total_seconds()))
if last_walk_end - last_walk_start > timedelta(seconds=40):
# if last step only ran short time before bumper (this includes bumper timeout time), time for a large random turn
# if it ran long time, maybe worth trying going in the same direction
continue
additional_turn = 30
# next random walk direction should be between 30 and 150 degrees
# (no need to go straight back or keep going forward)
# if part of virtual bumper handling, add 30 degrees to avoid the obstacle more forcefully
# TURN RANDOMLY
deg_angle = self.rand.randrange(30 + additional_turn,
150 + additional_turn)
deg_sign = self.rand.randint(0, 1)
if deg_sign:
deg_angle = -deg_angle
try:
self.virtual_bumper = VirtualBumper(
timedelta(seconds=20), 0.1)
with LidarCollisionMonitor(self):
self.turn(math.radians(deg_angle),
timeout=timedelta(seconds=30))
except ChangeDriverException as e:
continue
except (VirtualBumperException, LidarCollisionException):
self.inException = True
self.set_cam_angle(CAMERA_ANGLE_DRIVING)
print(self.time, "Turn Virtual Bumper!")
self.virtual_bumper = None
if self.current_driver is not None:
# probably didn't throw in previous turn but during self driving
self.go_straight(-2.0, timeout=timedelta(seconds=10))
self.try_step_around()
self.turn(math.radians(-deg_angle),
timeout=timedelta(seconds=30))
self.inException = False
self.bus.publish('driving_recovery', False)
except BusShutdownException:
pass