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