MyRobot Tutorial¶
This tutorial explains how to create a simple simulated robot and a control
application using OSGAR. We will use the myrobot example as a reference.
The myrobot example consists of three main files:
myrobot.py: The robot driver, which simulates the robot’s behavior.myapp.py: The application, which controls the robot.myrobot.json: The configuration file, which connects the robot and the application.
The Robot Driver - myrobot.py¶
The myrobot.py file defines a simple simulated robot with differential
drive. It receives speed commands and simulates the robot’s movement,
publishing its new pose (position and orientation).
The driver inherits from osgar.node.Node and implements on_desired_speed
and on_tick methods to handle incoming messages. The update() method
is provided by the base class and automatically dispatches messages to the
corresponding on_<channel> handlers.
Here is the code for myrobot.py:
"""
Example of robot diver outside OSGAR package
- simulation of differential robot
"""
import math
from osgar.node import Node
class MyRobot(Node):
def __init__(self, config, bus):
super().__init__(config, bus)
bus.register('pose2d', 'emergency_stop')
self.pose = (0, 0, 0)
self.speed, self.angular_speed = 0, 0
self.desired_speed, self.desired_angular_speed = 0, 0
self.last_update = None
def send_pose(self):
x, y, heading = self.pose
self.publish('pose2d', [round(x*1000), round(y*1000),
round(math.degrees(heading)*100)])
def update_pose(self):
if self.last_update is not None:
t = (self.time - self.last_update).total_seconds()
self.speed = self.desired_speed # TODO motion model
self.angular_speed = self.desired_angular_speed
x, y, heading = self.pose
x += math.cos(heading) * self.speed * t
y += math.sin(heading) * self.speed * t
heading += self.angular_speed * t
self.pose = (x, y, heading)
self.last_update = self.time
def on_desired_speed(self, data):
self.update_pose()
self.desired_speed, self.desired_angular_speed = data[0]/1000.0, math.radians(data[1]/100.0)
self.send_pose()
def on_tick(self, data):
self.update_pose()
self.send_pose()
# vim: expandtab sw=4 ts=4
The Application - myapp.py¶
The myapp.py file defines an application that controls the robot. It sends
speed commands to make the robot move in a figure eight.
Here is the code for myapp.py:
"""
Example of a simple application controling robot to move in figure 8
"""
import math
from datetime import timedelta
from osgar.node import Node
from osgar.bus import BusShutdownException
def distance(pose1, pose2):
return math.hypot(pose1[0] - pose2[0], pose1[1] - pose2[1])
class MyApp(Node):
def __init__(self, config, bus):
super().__init__(config, bus)
bus.register('desired_speed')
self.max_speed = config.get('max_speed', 0.1)
self.max_angular_speed = math.radians(50) # TODO config
self.verbose = False
self.last_position = (0, 0, 0)
self.is_moving = False
self.pose2d = None # TODO should be defined by super().__init__()
# TODO refactor to common "serializer"
def send_speed_cmd(self, speed, angular_speed):
return self.publish('desired_speed', [round(speed*1000), round(math.degrees(angular_speed)*100)])
# TODO refactor to common driver (used from sick2018 example)
def go_straight(self, how_far):
print(self.time, "go_straight %.1f" % how_far, self.last_position)
start_pose = self.last_position
if how_far >= 0:
self.send_speed_cmd(self.max_speed, 0.0)
else:
self.send_speed_cmd(-self.max_speed, 0.0)
while distance(start_pose, self.last_position) < abs(how_far):
self.update()
self.send_speed_cmd(0.0, 0.0)
def turn(self, angle, with_stop=True):
print(self.time, "turn %.1f" % math.degrees(angle))
start_pose = self.last_position
if angle >= 0:
self.send_speed_cmd(0.0, self.max_angular_speed)
else:
self.send_speed_cmd(0.0, -self.max_angular_speed)
while abs(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 on_pose2d(self, data):
prev = self.pose2d
self.pose2d = data
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):
try:
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")
except BusShutdownException:
pass
if __name__ == "__main__":
from osgar.launcher import launch
launch(app=MyApp, description='Navigate figure eight', prefix='myapp-')
# vim: expandtab sw=4 ts=4
The Configuration - myrobot.json¶
The myrobot.json file defines the modules (the robot and the application)
and the connections between them. We use the built-in timer driver.
Here is the content of myrobot.json:
{
"version": 2,
"robot": {
"modules": {
"app": {
"driver": "myapp:MyApp",
"in": ["emergency_stop", "pose2d"],
"out": ["desired_speed"],
"init": {
"max_speed": 0.5
}
},
"myrobot": {
"driver": "myrobot:MyRobot",
"in": ["desired_speed", "tick"],
"out": ["emergency_stop", "pose2d"],
"init": {}
},
"timer": {
"driver": "timer",
"in": [],
"out": ["tick"],
"init": {
"sleep": 0.1
}
}
},
"links": [["app.desired_speed", "myrobot.desired_speed"],
["myrobot.emergency_stop", "app.emergency_stop"],
["myrobot.pose2d", "app.pose2d"],
["timer.tick", "myrobot.tick"]]
}
}
Running the Example¶
To run the example, you need to execute the osgar.record module with the
myrobot.json configuration file. You also need to add the current directory
and the examples/myrobot directory to your PYTHONPATH so that OSGAR
can find the modules.
PYTHONPATH=.:examples/myrobot python3 -m osgar.record examples/myrobot/myrobot.json --duration 10
This will create a log file with the recorded data from the simulation.