|
| 1 | +MyRobot Tutorial |
| 2 | +================ |
| 3 | + |
| 4 | +This tutorial explains how to create a simple simulated robot and a control |
| 5 | +application using OSGAR. We will use the ``myrobot`` example as a reference. |
| 6 | + |
| 7 | +The ``myrobot`` example consists of three main files: |
| 8 | + |
| 9 | +- ``myrobot.py``: The robot driver, which simulates the robot's behavior. |
| 10 | +- ``myapp.py``: The application, which controls the robot. |
| 11 | +- ``myrobot.json``: The configuration file, which connects the robot and the |
| 12 | + application. |
| 13 | + |
| 14 | +The Robot Driver - ``myrobot.py`` |
| 15 | +--------------------------------- |
| 16 | + |
| 17 | +The ``myrobot.py`` file defines a simple simulated robot with differential |
| 18 | +drive. It receives speed commands and simulates the robot's movement, |
| 19 | +publishing its new pose (position and orientation). |
| 20 | + |
| 21 | +The driver inherits from ``osgar.node.Node`` and implements ``on_desired_speed`` |
| 22 | +and ``on_tick`` methods to handle incoming messages. The ``update()`` method |
| 23 | +is provided by the base class and automatically dispatches messages to the |
| 24 | +corresponding ``on_<channel>`` handlers. |
| 25 | + |
| 26 | +Here is the code for ``myrobot.py``: |
| 27 | + |
| 28 | +.. code-block:: python |
| 29 | +
|
| 30 | + """ |
| 31 | + Example of robot diver outside OSGAR package |
| 32 | + - simulation of differential robot |
| 33 | + """ |
| 34 | + import math |
| 35 | +
|
| 36 | + from osgar.node import Node |
| 37 | +
|
| 38 | +
|
| 39 | + class MyRobot(Node): |
| 40 | + def __init__(self, config, bus): |
| 41 | + super().__init__(config, bus) |
| 42 | + bus.register('pose2d', 'emergency_stop') |
| 43 | + self.pose = (0, 0, 0) |
| 44 | + self.speed, self.angular_speed = 0, 0 |
| 45 | + self.desired_speed, self.desired_angular_speed = 0, 0 |
| 46 | + self.last_update = None |
| 47 | +
|
| 48 | + def send_pose(self): |
| 49 | + x, y, heading = self.pose |
| 50 | + self.publish('pose2d', [round(x*1000), round(y*1000), |
| 51 | + round(math.degrees(heading)*100)]) |
| 52 | +
|
| 53 | + def update_pose(self): |
| 54 | + if self.last_update is not None: |
| 55 | + t = (self.time - self.last_update).total_seconds() |
| 56 | + self.speed = self.desired_speed # TODO motion model |
| 57 | + self.angular_speed = self.desired_angular_speed |
| 58 | +
|
| 59 | + x, y, heading = self.pose |
| 60 | + x += math.cos(heading) * self.speed * t |
| 61 | + y += math.sin(heading) * self.speed * t |
| 62 | + heading += self.angular_speed * t |
| 63 | + self.pose = (x, y, heading) |
| 64 | + self.last_update = self.time |
| 65 | +
|
| 66 | + def on_desired_speed(self, data): |
| 67 | + self.update_pose() |
| 68 | + self.desired_speed, self.desired_angular_speed = data[0]/1000.0, math.radians(data[1]/100.0) |
| 69 | + self.send_pose() |
| 70 | +
|
| 71 | + def on_tick(self, data): |
| 72 | + self.update_pose() |
| 73 | + self.send_pose() |
| 74 | +
|
| 75 | + # vim: expandtab sw=4 ts=4 |
| 76 | +
|
| 77 | +
|
| 78 | +The Application - ``myapp.py`` |
| 79 | +------------------------------ |
| 80 | + |
| 81 | +The ``myapp.py`` file defines an application that controls the robot. It sends |
| 82 | +speed commands to make the robot move in a figure eight. |
| 83 | + |
| 84 | +Here is the code for ``myapp.py``: |
| 85 | + |
| 86 | +.. code-block:: python |
| 87 | +
|
| 88 | + """ |
| 89 | + Example of a simple application controling robot to move in figure 8 |
| 90 | + """ |
| 91 | + import math |
| 92 | + from datetime import timedelta |
| 93 | +
|
| 94 | + from osgar.node import Node |
| 95 | + from osgar.bus import BusShutdownException |
| 96 | +
|
| 97 | +
|
| 98 | + def distance(pose1, pose2): |
| 99 | + return math.hypot(pose1[0] - pose2[0], pose1[1] - pose2[1]) |
| 100 | +
|
| 101 | +
|
| 102 | + class MyApp(Node): |
| 103 | + def __init__(self, config, bus): |
| 104 | + super().__init__(config, bus) |
| 105 | + bus.register('desired_speed') |
| 106 | + self.max_speed = config.get('max_speed', 0.1) |
| 107 | + self.max_angular_speed = math.radians(50) # TODO config |
| 108 | + self.verbose = False |
| 109 | + self.last_position = (0, 0, 0) |
| 110 | + self.is_moving = False |
| 111 | + self.pose2d = None # TODO should be defined by super().__init__() |
| 112 | +
|
| 113 | + # TODO refactor to common "serializer" |
| 114 | + def send_speed_cmd(self, speed, angular_speed): |
| 115 | + return self.publish('desired_speed', [round(speed*1000), round(math.degrees(angular_speed)*100)]) |
| 116 | +
|
| 117 | + # TODO refactor to common driver (used from sick2018 example) |
| 118 | + def go_straight(self, how_far): |
| 119 | + print(self.time, "go_straight %.1f" % how_far, self.last_position) |
| 120 | + start_pose = self.last_position |
| 121 | + if how_far >= 0: |
| 122 | + self.send_speed_cmd(self.max_speed, 0.0) |
| 123 | + else: |
| 124 | + self.send_speed_cmd(-self.max_speed, 0.0) |
| 125 | + while distance(start_pose, self.last_position) < abs(how_far): |
| 126 | + self.update() |
| 127 | + self.send_speed_cmd(0.0, 0.0) |
| 128 | +
|
| 129 | + def turn(self, angle, with_stop=True): |
| 130 | + print(self.time, "turn %.1f" % math.degrees(angle)) |
| 131 | + start_pose = self.last_position |
| 132 | + if angle >= 0: |
| 133 | + self.send_speed_cmd(0.0, self.max_angular_speed) |
| 134 | + else: |
| 135 | + self.send_speed_cmd(0.0, -self.max_angular_speed) |
| 136 | + while abs(start_pose[2] - self.last_position[2]) < abs(angle): |
| 137 | + self.update() |
| 138 | + if with_stop: |
| 139 | + self.send_speed_cmd(0.0, 0.0) |
| 140 | + start_time = self.time |
| 141 | + while self.time - start_time < timedelta(seconds=2): |
| 142 | + self.update() |
| 143 | + if not self.is_moving: |
| 144 | + break |
| 145 | + print(self.time, 'stop at', self.time - start_time) |
| 146 | +
|
| 147 | + def on_pose2d(self, data): |
| 148 | + prev = self.pose2d |
| 149 | + self.pose2d = data |
| 150 | + x_mm, y_mm, heading_mdeg = self.pose2d |
| 151 | + self.last_position = (x_mm/1000.0, y_mm/1000.0, |
| 152 | + math.radians(heading_mdeg/100.0)) |
| 153 | + self.is_moving = (prev != self.pose2d) |
| 154 | +
|
| 155 | + def run(self): |
| 156 | + try: |
| 157 | + print("MyApp example - figure 8!") |
| 158 | + step_size = 0.5 # meters |
| 159 | + deg90 = math.radians(90) |
| 160 | +
|
| 161 | + for i in range(4): |
| 162 | + self.go_straight(step_size) |
| 163 | + self.turn(deg90) |
| 164 | +
|
| 165 | + for i in range(4): |
| 166 | + self.go_straight(step_size) |
| 167 | + self.turn(-deg90) |
| 168 | +
|
| 169 | + print("END") |
| 170 | + except BusShutdownException: |
| 171 | + pass |
| 172 | +
|
| 173 | +
|
| 174 | + if __name__ == "__main__": |
| 175 | + from osgar.launcher import launch |
| 176 | +
|
| 177 | + launch(app=MyApp, description='Navigate figure eight', prefix='myapp-') |
| 178 | +
|
| 179 | + # vim: expandtab sw=4 ts=4 |
| 180 | +
|
| 181 | +The Configuration - ``myrobot.json`` |
| 182 | +------------------------------------ |
| 183 | + |
| 184 | +The ``myrobot.json`` file defines the modules (the robot and the application) |
| 185 | +and the connections between them. We use the built-in ``timer`` driver. |
| 186 | + |
| 187 | +Here is the content of ``myrobot.json``: |
| 188 | + |
| 189 | +.. code-block:: json |
| 190 | +
|
| 191 | + { |
| 192 | + "version": 2, |
| 193 | + "robot": { |
| 194 | + "modules": { |
| 195 | + "app": { |
| 196 | + "driver": "myapp:MyApp", |
| 197 | + "in": ["emergency_stop", "pose2d"], |
| 198 | + "out": ["desired_speed"], |
| 199 | + "init": { |
| 200 | + "max_speed": 0.5 |
| 201 | + } |
| 202 | + }, |
| 203 | + "myrobot": { |
| 204 | + "driver": "myrobot:MyRobot", |
| 205 | + "in": ["desired_speed", "tick"], |
| 206 | + "out": ["emergency_stop", "pose2d"], |
| 207 | + "init": {} |
| 208 | + }, |
| 209 | + "timer": { |
| 210 | + "driver": "timer", |
| 211 | + "in": [], |
| 212 | + "out": ["tick"], |
| 213 | + "init": { |
| 214 | + "sleep": 0.1 |
| 215 | + } |
| 216 | + } |
| 217 | + }, |
| 218 | + "links": [["app.desired_speed", "myrobot.desired_speed"], |
| 219 | + ["myrobot.emergency_stop", "app.emergency_stop"], |
| 220 | + ["myrobot.pose2d", "app.pose2d"], |
| 221 | + ["timer.tick", "myrobot.tick"]] |
| 222 | + } |
| 223 | + } |
| 224 | +
|
| 225 | +Running the Example |
| 226 | +------------------- |
| 227 | + |
| 228 | +To run the example, you need to execute the ``osgar.record`` module with the |
| 229 | +``myrobot.json`` configuration file. You also need to add the current directory |
| 230 | +and the ``examples/myrobot`` directory to your ``PYTHONPATH`` so that OSGAR |
| 231 | +can find the modules. |
| 232 | + |
| 233 | +.. code-block:: bash |
| 234 | +
|
| 235 | + PYTHONPATH=.:examples/myrobot python3 -m osgar.record examples/myrobot/myrobot.json --duration 10 |
| 236 | +
|
| 237 | +This will create a log file with the recorded data from the simulation. |
0 commit comments