channel = super().update() # define self.time desired_speed = 0.0 start_time = self.time while self.time - start_time < timedelta(minutes=1): channel = super().update() if channel == 'scan': size = len(self.scan) dist = min_dist(self.scan[size//3:2*size//3]) tangent_angle = follow_wall_angle(self.scan, right_wall=self.right_wall, radius=WALL_DISTANCE) if dist < 2.0: desired_speed = (self.max_speed / 2) * (dist - 0.4) / 1.6 else: desired_speed = self.max_speed desired_angular_speed = 0.7 * tangent_angle # TODO better P-regulator based on angle and free space self.send_speed_cmd(desired_speed, desired_angular_speed) elif channel == 'emergency_stop': self.send_speed_cmd(0.0, 0.0) self.request_stop() # it should be "delayed" if __name__ == "__main__": from osgar.launcher import launch launch(app=FollowWall, description='Follow Wall', prefix='wall-') # vim: expandtab sw=4 ts=4
prev = self.pose2d channel = super().update() if channel == 'pose2d': x_mm, y_mm, heading_mdeg = self.pose2d self.last_position = (x_mm / 1000.0, y_mm / 1000.0, math.radians(heading_mdeg / 100.0)) self.is_moving = (prev != self.pose2d) def run(self): print("MyApp example - figure 8!") step_size = 0.5 # meters deg90 = math.radians(90) for i in range(4): self.go_straight(step_size) self.turn(deg90) for i in range(4): self.go_straight(step_size) self.turn(-deg90) print("END") if __name__ == "__main__": from osgar.launcher import launch launch(app=MyApp, description='Navigate figure eight', prefix='myapp-') # vim: expandtab sw=4 ts=4
angle = math.radians(270 * index / SCAN_SIZE - 135) + masterAngleOffset desiredAngle = 0 desiredDistance = 0.4 speed = 0.2 + 2 * (near - desiredDistance) rot = 1.5 * (angle - desiredAngle) if speed < 0: speed = 0 self.send_speed_cmd(speed, rot) self.last_scan = None self.update() def run(self): try: self.raise_exception_on_stop = True self.followme() except EmergencyStopException: print('!!!Emergency STOP!!!') self.raise_exception_on_stop = False self.send_speed_cmd(0.0, 0.0) self.wait(timedelta(seconds=1)) if __name__ == "__main__": from osgar.launcher import launch launch(app=FollowMe, description='Follow Me', prefix='followme-') # vim: expandtab sw=4 ts=4
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() if __name__ == "__main__": from osgar.launcher import launch launch(app=GPSNavigation, description='GPS Navigation', prefix='gpsnav-') # vim: expandtab sw=4 ts=4
if channel == 'pose2d': x, y, heading = self.pose2d pose = (x/1000.0, y/1000.0, math.radians(heading/100.0)) if self.start_pose is None: self.start_pose = pose self.traveled_dist = math.hypot(pose[0] - self.start_pose[0], pose[1] - self.start_pose[1]) def wait(self, dt): # TODO refactor to some common class if self.time is None: self.update() start_time = self.time while self.time - start_time < dt: self.update() def run(self): print("Go One Meter!") self.send_speed_cmd(0.5, 0.0) while self.traveled_dist < 1.0: self.update() print("STOP") self.send_speed_cmd(0.0, 0.0) self.wait(timedelta(seconds=1)) if __name__ == "__main__": from osgar.launcher import launch launch(app=GoOneMeter, description='Go One Meter', prefix='go1m-') # vim: expandtab sw=4 ts=4
else: angle = math.radians(270 * index/SCAN_SIZE - 135) + masterAngleOffset desiredAngle = 0 desiredDistance = 0.4 speed = 0.2 + 2 * (near - desiredDistance) rot = 1.5 * (angle - desiredAngle) if speed < 0: speed = 0 self.send_speed_cmd(speed, rot) self.last_scan = None self.update() def run(self): try: self.raise_exception_on_stop = True self.followme() except EmergencyStopException: print('!!!Emergency STOP!!!') self.raise_exception_on_stop = False self.send_speed_cmd(0.0, 0.0) self.wait(timedelta(seconds=1)) if __name__ == "__main__": from osgar.launcher import launch launch(app=FollowMe, description='Follow Me', prefix='followme-') # vim: expandtab sw=4 ts=4