Commit 1711fd6c authored by Séverin Lemaignan's avatar Séverin Lemaignan
Browse files

Added a test controller for bikes

parent 40c41a27
#! /usr/bin/env python3
"""
This client controls the bikes in the simulation.
"""
import sys
try:
from pymorse import Morse
except ImportError:
print("you need first to install pymorse, the Python bindings for MORSE!")
sys.exit(1)
# waypoints: (x, y, z, tolerance, speed)
WAYPOINTS = [(-55.308, -53.882, 0.03, 0.5, 4.0),
(-30.2, -51.65, 0., 0.5, 4.0),
(-12.19, -48.78, 0., 0.5, 4.0)]
waypoint_idx=0
with Morse() as sim:
pose = sim.bike.pose
start_pose = pose.get() # blocks until we get the initial pose of the vehicle
print("My start location is %s" % start_pose)
motion = sim.bike.motion
#teleport = sim.bike.teleport
while True:
motion.goto(*WAYPOINTS[waypoint_idx]).result()
waypoint_idx += 1
if waypoint_idx >= len(WAYPOINTS):
print("Reached end of trajectory. Looping.")
teleport.publish(start_pose) # teleporting back to initial location
waypoint_idx = 0
else:
print("Reached waypoint %d" % waypoint_idx)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment