Skip to content

Commit b7ca8d7

Browse files
hbcarloshbcarlos
authored
Merge pull request #142 from ldania/turtle
Turtle
2 parents 6366c5b + 5171864 commit b7ca8d7

File tree

7 files changed

+579
-2
lines changed

7 files changed

+579
-2
lines changed

‎jupyros/__init__.py

+1
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
from .ros2.ros_widgets import *
2626
from .ros2.subscriber import *
2727
from .ros2.key_input import *
28+
from .ros2.turtle_sim import *
2829

2930
else:
3031
# Default to ROS1

‎jupyros/ros2/__init__.py

+2
Original file line numberDiff line numberDiff line change
@@ -17,3 +17,5 @@
1717
from ..ros2.ros_widgets import *
1818
from ..ros2.subscriber import *
1919
from ..ros2.key_input import *
20+
from ..ros2.turtle_sim import *
21+
from ..ros2.ipy import *

‎jupyros/ros2/ipy.py

+28
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
#############################################################################
2+
# Copyright (c) Wolf Vollprecht, QuantStack #
3+
# #
4+
# Distributed under the terms of the BSD 3-Clause License. #
5+
# #
6+
# The full license is in the file LICENSE, distributed with this software. #
7+
#############################################################################
8+
9+
## Modified by Luigi Dania for Jupyter-Ros2
10+
import sys
11+
from threading import Thread
12+
13+
import ipywidgets as widgets
14+
from IPython.core.magic import register_cell_magic
15+
16+
def executor(cell, gbls, lcls):
17+
exec(cell, gbls, lcls)
18+
19+
# @register_cell_magic is not available during jupyter nbextension enable ...
20+
try:
21+
@register_cell_magic
22+
def thread_cell2(line, cell, local_ns=None):
23+
t = Thread(target=executor, args=(cell, globals(), sys._getframe(2).f_locals))
24+
out = widgets.Output(layout={'border': '1px solid gray'})
25+
t.start()
26+
return out
27+
except:
28+
pass

‎jupyros/ros2/publisher.py

+3-2
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,7 @@ def __init__(self, node: Node, msg_type: MsgType, topic: str, rate = None ) -> N
8686
"send_btn": widgets.Button(description="Send Message"),
8787
"txt_input": widgets.Text(description="Message", value="Something")
8888
}
89+
self.vbox = None
8990
if(rate):
9091
self.node.create_timer(rate, self.__send_msg)
9192
self.widget_dict, self.widget_list = add_widgets(self.msg_type, self.__widget_dict, self.__widget_list)
@@ -126,7 +127,7 @@ def display(self) -> widgets.VBox:
126127
))
127128
self.__widget_list.append(btm_box)
128129
vbox = widgets.VBox(children=self.__widget_list)
129-
130+
self.vbox = vbox
130131
return vbox
131132

132133
def send_msg(self, args):
@@ -139,7 +140,7 @@ def __send_msg(self, args):
139140

140141
""" Generic call to send message. """
141142
self.msg_inst = self.msg_type()
142-
if(self.widget_list):
143+
if(self.vbox):
143144
self.widget_dict_to_msg()
144145
self.__publisher.publish(self.msg_inst)
145146
else:

‎jupyros/ros2/turtle_sim.py

+123
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,123 @@
1+
import os
2+
import time
3+
import math
4+
import random
5+
6+
import ipycanvas
7+
import ipywidgets
8+
9+
from ament_index_python.packages import get_package_share_directory
10+
11+
12+
class TurtleSim:
13+
def __init__(self, width=1600, height=1600, turtle_size=100, background_color="#4556FF"):
14+
self.turtles = {}
15+
self.turtle_size = turtle_size
16+
self.canvas_middle = {"x": width // 2,
17+
"y": height // 2,
18+
"theta": 0}
19+
20+
# Three layers for the canvas: 0-background, 1-paths, 2-turtles
21+
self.canvas = ipycanvas.MultiCanvas(3,
22+
width=width, height=height,
23+
layout={"width": "100%"})
24+
25+
# Water background
26+
self.canvas[0].fill_style = background_color
27+
self.canvas[0].fill_rect(0, 0, width, height)
28+
29+
# Turtle path width
30+
self.canvas[1].line_width = 8
31+
32+
self.last_move_time = time.time()
33+
self.spawn()
34+
35+
def spawn(self, name=None, pose=None):
36+
37+
if (name is None) or (name in self.turtles.keys()):
38+
name = "turtle" + str(len(self.turtles) + 1)
39+
40+
self.turtles[name] = self.Turtle(name, self.turtle_size)
41+
42+
if pose is None:
43+
# Spawn to middle of canvas
44+
self.turtles[name].pose = self.canvas_middle
45+
else:
46+
self.turtles[name].pose = pose
47+
48+
with ipycanvas.hold_canvas(self.canvas):
49+
self.draw_turtle(name)
50+
51+
print(name + " has spawned.")
52+
53+
def move_turtles(self, new_poses):
54+
elapsed_time = time.time() - self.last_move_time
55+
56+
57+
58+
if elapsed_time > 0.08: # seconds
59+
self.last_move_time = time.time()
60+
61+
with ipycanvas.hold_canvas(self.canvas):
62+
self.canvas[2].clear()
63+
64+
for name in self.turtles.keys():
65+
# Draw line path
66+
self.canvas[1].stroke_style = self.turtles[name].path_color
67+
self.canvas[1].stroke_line(self.turtles[name].pose["x"],
68+
self.turtles[name].pose["y"],
69+
new_poses[name]["x"],
70+
new_poses[name]["y"])
71+
# Update
72+
self.turtles[name].pose["x"] = new_poses[name]["x"]
73+
self.turtles[name].pose["y"] = new_poses[name]["y"]
74+
self.turtles[name].pose["theta"] = new_poses[name]["theta"]
75+
76+
77+
78+
self.draw_turtle(name)
79+
80+
def draw_turtle(self, name="turtle1", n=2):
81+
# Offsets for turtle center and orientation
82+
x_offset = - self.turtle_size / 2
83+
y_offset = - self.turtle_size / 2
84+
theta_offset = self.turtles[name].pose["theta"] - math.radians(90) # to face right side
85+
86+
# Transform canvas
87+
self.canvas[n].save()
88+
self.canvas[n].translate(self.turtles[name].pose["x"], self.turtles[name].pose["y"])
89+
self.canvas[n].rotate(-theta_offset)
90+
91+
self.canvas[n].draw_image(self.turtles[name].canvas,
92+
x_offset, y_offset,
93+
self.turtle_size)
94+
95+
# Revert transformation
96+
self.canvas[n].restore()
97+
98+
class Turtle:
99+
def __init__(self, name, size=100):
100+
self.name = name
101+
self.size = size
102+
self.canvas = None
103+
self.randomize()
104+
self.pose = {"x": 0,
105+
"y": 0,
106+
"theta": 0}
107+
self.path_color = '#B3B8FF' # Light blue
108+
109+
def randomize(self):
110+
img_path = str(get_package_share_directory("turtlesim")) + "/images/"
111+
images = os.listdir(img_path)
112+
turtle_pngs = [img for img in images if ('.png' in img and 'palette' not in img)]
113+
random_png = turtle_pngs[random.randint(0, len(turtle_pngs) - 1)]
114+
turtle_img = ipywidgets.Image.from_file(img_path + random_png)
115+
turtle_canvas = ipycanvas.Canvas(width=self.size, height=self.size)
116+
117+
with ipycanvas.hold_canvas(turtle_canvas):
118+
turtle_canvas.draw_image(turtle_img, 0, 0, self.size)
119+
120+
time.sleep(0.1) # Drawing time
121+
self.canvas = turtle_canvas
122+
123+
return self

‎notebooks/ROS2_Turtlesim.ipynb

+192
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,192 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "code",
5+
"execution_count": null,
6+
"id": "7259a8b6",
7+
"metadata": {},
8+
"outputs": [],
9+
"source": [
10+
"import rclpy as rp\n",
11+
"import jupyros.ros2 as jr2\n",
12+
"import jupyros.ros2.turtle_sim as turtle\n",
13+
"from turtlesim.srv import Spawn\n",
14+
"from turtlesim.msg import Pose\n",
15+
"import os\n",
16+
"from std_msgs.msg import String\n",
17+
"from geometry_msgs.msg import Twist\n",
18+
"from sidecar import Sidecar\n",
19+
"from time import time, sleep\n",
20+
"import math\n",
21+
"\n"
22+
]
23+
},
24+
{
25+
"cell_type": "code",
26+
"execution_count": null,
27+
"id": "1964ecfc-67ee-47bf-aad3-824315c4418d",
28+
"metadata": {},
29+
"outputs": [],
30+
"source": [
31+
"# Initialize ROS communications for a given context\n",
32+
"if(rp.ok() == False):\n",
33+
" rp.init()\n",
34+
"else:\n",
35+
" print(\"rclpy already initiated\")"
36+
]
37+
},
38+
{
39+
"cell_type": "code",
40+
"execution_count": null,
41+
"id": "47354d6d-8c92-47ce-9f85-c0c1e403d8bf",
42+
"metadata": {},
43+
"outputs": [],
44+
"source": [
45+
"superturtle = rp.create_node(\"superturtle\")\n",
46+
"moveNode = rp.create_node(\"moveNode\")"
47+
]
48+
},
49+
{
50+
"cell_type": "code",
51+
"execution_count": null,
52+
"id": "2dbf024d",
53+
"metadata": {
54+
"tags": []
55+
},
56+
"outputs": [],
57+
"source": [
58+
"turtlesim = turtle.TurtleSim(background_color=\"#0000FF\")"
59+
]
60+
},
61+
{
62+
"cell_type": "code",
63+
"execution_count": null,
64+
"id": "2bfe401a-2a01-4f75-839a-411b221bac8e",
65+
"metadata": {
66+
"tags": []
67+
},
68+
"outputs": [],
69+
"source": [
70+
"display(turtlesim.canvas)"
71+
]
72+
},
73+
{
74+
"cell_type": "markdown",
75+
"id": "ece1ece3-54f6-4df8-be79-42d7f37f6e08",
76+
"metadata": {},
77+
"source": [
78+
"**TIP:** When using JupyterLab, you can right-click on the canvas and select *Create New View from Output*"
79+
]
80+
},
81+
{
82+
"cell_type": "code",
83+
"execution_count": null,
84+
"id": "497db1e0-8c21-4ec0-b620-1607ab34d685",
85+
"metadata": {
86+
"tags": []
87+
},
88+
"outputs": [],
89+
"source": [
90+
"poses = {}\n",
91+
"\n",
92+
"for name in turtlesim.turtles.keys():\n",
93+
" poses[name] = turtlesim.turtles[name].pose\n",
94+
" \n",
95+
"print(poses[\"turtle1\"])"
96+
]
97+
},
98+
{
99+
"cell_type": "code",
100+
"execution_count": null,
101+
"id": "ac63dbbb-b388-4b18-890c-e3bcada044a9",
102+
"metadata": {},
103+
"outputs": [],
104+
"source": []
105+
},
106+
{
107+
"cell_type": "code",
108+
"execution_count": null,
109+
"id": "cd2e66dc",
110+
"metadata": {},
111+
"outputs": [],
112+
"source": [
113+
"topic_name = '/Pose'\n",
114+
"def move_turtles(msg):\n",
115+
" scale = 0.0015\n",
116+
" name = \"turtle1\"\n",
117+
" \n",
118+
" def angle_slope():\n",
119+
" d_y = (math.sin(msg.theta+1/180)*math.cos(msg.theta+1/180)-math.sin(msg.theta)*math.cos(msg.theta))\n",
120+
" d_x = (math.cos(msg.theta+1/180) - math.cos(msg.theta))\n",
121+
" return math.atan2(d_y,d_x)\n",
122+
" \n",
123+
" poses[name] = {\"x\": 1/scale * math.cos(msg.theta) + 800,\n",
124+
" \"y\": -1/scale * math.sin(msg.theta)*math.cos(msg.theta) + 800,\n",
125+
" \"theta\": angle_slope()}\n",
126+
" ##msg.theta - math.atan2((-1/scale * math.sin(msg.theta)*math.cos(msg.theta) + 800),(1/scale * math.cos(msg.theta) + 800))\n",
127+
" \n",
128+
" turtlesim.move_turtles(new_poses=poses)\n",
129+
"\n",
130+
"\n",
131+
"\n",
132+
"\n",
133+
"def cb(msg):\n",
134+
" move_turtles(msg)\n",
135+
"\n",
136+
"\n",
137+
"turtle_control = jr2.Subscriber(moveNode, Pose, topic_name, cb)\n",
138+
"turtle_control.display()"
139+
]
140+
},
141+
{
142+
"cell_type": "code",
143+
"execution_count": null,
144+
"id": "c9e44409-1f96-426f-9826-95f2ddff5119",
145+
"metadata": {},
146+
"outputs": [],
147+
"source": [
148+
"%%thread_cell2\n",
149+
"run = True\n",
150+
"i = 90\n",
151+
"pub = jr2.Publisher(moveNode, Pose, topic_name)\n",
152+
"\n",
153+
"while run:\n",
154+
" msg = Pose()\n",
155+
" msg.theta = i / 180 * math.pi\n",
156+
" pub.send_msg( msg)\n",
157+
" sleep(0.01)\n",
158+
" i += 1\n",
159+
"print(\"Done\")"
160+
]
161+
},
162+
{
163+
"cell_type": "code",
164+
"execution_count": null,
165+
"id": "0fd093f5-122e-4006-8ad8-813428356fbe",
166+
"metadata": {},
167+
"outputs": [],
168+
"source": []
169+
}
170+
],
171+
"metadata": {
172+
"kernelspec": {
173+
"display_name": "Python 3 (ipykernel)",
174+
"language": "python",
175+
"name": "python3"
176+
},
177+
"language_info": {
178+
"codemirror_mode": {
179+
"name": "ipython",
180+
"version": 3
181+
},
182+
"file_extension": ".py",
183+
"mimetype": "text/x-python",
184+
"name": "python",
185+
"nbconvert_exporter": "python",
186+
"pygments_lexer": "ipython3",
187+
"version": "3.8.10"
188+
}
189+
},
190+
"nbformat": 4,
191+
"nbformat_minor": 5
192+
}

0 commit comments

Comments
 (0)