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