Commit 0ef9a41a authored by Séverin Lemaignan's avatar Séverin Lemaignan
Browse files

Added a basic van controller as example

parent 1011fa6c
#! /usr/bin/env python3
"""
Template showing how to integrate the van's decision making process into the
simulation.
Note that this example uses Python for illustration purposes, but any language
supporting sockets, or alternatively, middlewares like ROS or YARP, can be used
instead.
"""
import math
from pymorse import Morse
speed=0.0 # initial speed, m/s
def on_camera_update(evt):
""" Analyse what is seen by the van, and adjust accordingly the speed.
"""
global speed
# see http://www.openrobots.org/morse/doc/latest/user/sensors/semantic_camera.html#data-fields
objects = evt["visible_objects"]
distances = []
for o in objects:
distance = math.sqrt(pow(o["position"][0],2) +
pow(o["position"][1],2))
print("Seen %s at %.1fm" % (o["name"], distance))
distances.append(distance)
speed = min(2.0, min(distances)/5.0)
print("Setting speed to %.1f" % speed)
with Morse() as sim:
# the object 'arrival_van' is automatically created by pymorse
# because the simulation contains a vehicle named 'arrival_van'
van = sim.arrival_van
pose = van.pose
print("The van's initial location is %s" % pose.get())
van.camera.subscribe(on_camera_update)
motion = van.motion
while True:
motion.publish({"v": speed,
"w": 0.0}) # straight line
sim.sleep(0.2)
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