-
Notifications
You must be signed in to change notification settings - Fork 115
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #142 from ldania/turtle
Turtle
- Loading branch information
Showing
7 changed files
with
579 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,28 @@ | ||
############################################################################# | ||
# Copyright (c) Wolf Vollprecht, QuantStack # | ||
# # | ||
# Distributed under the terms of the BSD 3-Clause License. # | ||
# # | ||
# The full license is in the file LICENSE, distributed with this software. # | ||
############################################################################# | ||
|
||
## Modified by Luigi Dania for Jupyter-Ros2 | ||
import sys | ||
from threading import Thread | ||
|
||
import ipywidgets as widgets | ||
from IPython.core.magic import register_cell_magic | ||
|
||
def executor(cell, gbls, lcls): | ||
exec(cell, gbls, lcls) | ||
|
||
# @register_cell_magic is not available during jupyter nbextension enable ... | ||
try: | ||
@register_cell_magic | ||
def thread_cell2(line, cell, local_ns=None): | ||
t = Thread(target=executor, args=(cell, globals(), sys._getframe(2).f_locals)) | ||
out = widgets.Output(layout={'border': '1px solid gray'}) | ||
t.start() | ||
return out | ||
except: | ||
pass |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,123 @@ | ||
import os | ||
import time | ||
import math | ||
import random | ||
|
||
import ipycanvas | ||
import ipywidgets | ||
|
||
from ament_index_python.packages import get_package_share_directory | ||
|
||
|
||
class TurtleSim: | ||
def __init__(self, width=1600, height=1600, turtle_size=100, background_color="#4556FF"): | ||
self.turtles = {} | ||
self.turtle_size = turtle_size | ||
self.canvas_middle = {"x": width // 2, | ||
"y": height // 2, | ||
"theta": 0} | ||
|
||
# Three layers for the canvas: 0-background, 1-paths, 2-turtles | ||
self.canvas = ipycanvas.MultiCanvas(3, | ||
width=width, height=height, | ||
layout={"width": "100%"}) | ||
|
||
# Water background | ||
self.canvas[0].fill_style = background_color | ||
self.canvas[0].fill_rect(0, 0, width, height) | ||
|
||
# Turtle path width | ||
self.canvas[1].line_width = 8 | ||
|
||
self.last_move_time = time.time() | ||
self.spawn() | ||
|
||
def spawn(self, name=None, pose=None): | ||
|
||
if (name is None) or (name in self.turtles.keys()): | ||
name = "turtle" + str(len(self.turtles) + 1) | ||
|
||
self.turtles[name] = self.Turtle(name, self.turtle_size) | ||
|
||
if pose is None: | ||
# Spawn to middle of canvas | ||
self.turtles[name].pose = self.canvas_middle | ||
else: | ||
self.turtles[name].pose = pose | ||
|
||
with ipycanvas.hold_canvas(self.canvas): | ||
self.draw_turtle(name) | ||
|
||
print(name + " has spawned.") | ||
|
||
def move_turtles(self, new_poses): | ||
elapsed_time = time.time() - self.last_move_time | ||
|
||
|
||
|
||
if elapsed_time > 0.08: # seconds | ||
self.last_move_time = time.time() | ||
|
||
with ipycanvas.hold_canvas(self.canvas): | ||
self.canvas[2].clear() | ||
|
||
for name in self.turtles.keys(): | ||
# Draw line path | ||
self.canvas[1].stroke_style = self.turtles[name].path_color | ||
self.canvas[1].stroke_line(self.turtles[name].pose["x"], | ||
self.turtles[name].pose["y"], | ||
new_poses[name]["x"], | ||
new_poses[name]["y"]) | ||
# Update | ||
self.turtles[name].pose["x"] = new_poses[name]["x"] | ||
self.turtles[name].pose["y"] = new_poses[name]["y"] | ||
self.turtles[name].pose["theta"] = new_poses[name]["theta"] | ||
|
||
|
||
|
||
self.draw_turtle(name) | ||
|
||
def draw_turtle(self, name="turtle1", n=2): | ||
# Offsets for turtle center and orientation | ||
x_offset = - self.turtle_size / 2 | ||
y_offset = - self.turtle_size / 2 | ||
theta_offset = self.turtles[name].pose["theta"] - math.radians(90) # to face right side | ||
|
||
# Transform canvas | ||
self.canvas[n].save() | ||
self.canvas[n].translate(self.turtles[name].pose["x"], self.turtles[name].pose["y"]) | ||
self.canvas[n].rotate(-theta_offset) | ||
|
||
self.canvas[n].draw_image(self.turtles[name].canvas, | ||
x_offset, y_offset, | ||
self.turtle_size) | ||
|
||
# Revert transformation | ||
self.canvas[n].restore() | ||
|
||
class Turtle: | ||
def __init__(self, name, size=100): | ||
self.name = name | ||
self.size = size | ||
self.canvas = None | ||
self.randomize() | ||
self.pose = {"x": 0, | ||
"y": 0, | ||
"theta": 0} | ||
self.path_color = '#B3B8FF' # Light blue | ||
|
||
def randomize(self): | ||
img_path = str(get_package_share_directory("turtlesim")) + "/images/" | ||
images = os.listdir(img_path) | ||
turtle_pngs = [img for img in images if ('.png' in img and 'palette' not in img)] | ||
random_png = turtle_pngs[random.randint(0, len(turtle_pngs) - 1)] | ||
turtle_img = ipywidgets.Image.from_file(img_path + random_png) | ||
turtle_canvas = ipycanvas.Canvas(width=self.size, height=self.size) | ||
|
||
with ipycanvas.hold_canvas(turtle_canvas): | ||
turtle_canvas.draw_image(turtle_img, 0, 0, self.size) | ||
|
||
time.sleep(0.1) # Drawing time | ||
self.canvas = turtle_canvas | ||
|
||
return self |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,192 @@ | ||
{ | ||
"cells": [ | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "7259a8b6", | ||
"metadata": {}, | ||
"outputs": [], | ||
"source": [ | ||
"import rclpy as rp\n", | ||
"import jupyros.ros2 as jr2\n", | ||
"import jupyros.ros2.turtle_sim as turtle\n", | ||
"from turtlesim.srv import Spawn\n", | ||
"from turtlesim.msg import Pose\n", | ||
"import os\n", | ||
"from std_msgs.msg import String\n", | ||
"from geometry_msgs.msg import Twist\n", | ||
"from sidecar import Sidecar\n", | ||
"from time import time, sleep\n", | ||
"import math\n", | ||
"\n" | ||
] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "1964ecfc-67ee-47bf-aad3-824315c4418d", | ||
"metadata": {}, | ||
"outputs": [], | ||
"source": [ | ||
"# Initialize ROS communications for a given context\n", | ||
"if(rp.ok() == False):\n", | ||
" rp.init()\n", | ||
"else:\n", | ||
" print(\"rclpy already initiated\")" | ||
] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "47354d6d-8c92-47ce-9f85-c0c1e403d8bf", | ||
"metadata": {}, | ||
"outputs": [], | ||
"source": [ | ||
"superturtle = rp.create_node(\"superturtle\")\n", | ||
"moveNode = rp.create_node(\"moveNode\")" | ||
] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "2dbf024d", | ||
"metadata": { | ||
"tags": [] | ||
}, | ||
"outputs": [], | ||
"source": [ | ||
"turtlesim = turtle.TurtleSim(background_color=\"#0000FF\")" | ||
] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "2bfe401a-2a01-4f75-839a-411b221bac8e", | ||
"metadata": { | ||
"tags": [] | ||
}, | ||
"outputs": [], | ||
"source": [ | ||
"display(turtlesim.canvas)" | ||
] | ||
}, | ||
{ | ||
"cell_type": "markdown", | ||
"id": "ece1ece3-54f6-4df8-be79-42d7f37f6e08", | ||
"metadata": {}, | ||
"source": [ | ||
"**TIP:** When using JupyterLab, you can right-click on the canvas and select *Create New View from Output*" | ||
] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "497db1e0-8c21-4ec0-b620-1607ab34d685", | ||
"metadata": { | ||
"tags": [] | ||
}, | ||
"outputs": [], | ||
"source": [ | ||
"poses = {}\n", | ||
"\n", | ||
"for name in turtlesim.turtles.keys():\n", | ||
" poses[name] = turtlesim.turtles[name].pose\n", | ||
" \n", | ||
"print(poses[\"turtle1\"])" | ||
] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "ac63dbbb-b388-4b18-890c-e3bcada044a9", | ||
"metadata": {}, | ||
"outputs": [], | ||
"source": [] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "cd2e66dc", | ||
"metadata": {}, | ||
"outputs": [], | ||
"source": [ | ||
"topic_name = '/Pose'\n", | ||
"def move_turtles(msg):\n", | ||
" scale = 0.0015\n", | ||
" name = \"turtle1\"\n", | ||
" \n", | ||
" def angle_slope():\n", | ||
" d_y = (math.sin(msg.theta+1/180)*math.cos(msg.theta+1/180)-math.sin(msg.theta)*math.cos(msg.theta))\n", | ||
" d_x = (math.cos(msg.theta+1/180) - math.cos(msg.theta))\n", | ||
" return math.atan2(d_y,d_x)\n", | ||
" \n", | ||
" poses[name] = {\"x\": 1/scale * math.cos(msg.theta) + 800,\n", | ||
" \"y\": -1/scale * math.sin(msg.theta)*math.cos(msg.theta) + 800,\n", | ||
" \"theta\": angle_slope()}\n", | ||
" ##msg.theta - math.atan2((-1/scale * math.sin(msg.theta)*math.cos(msg.theta) + 800),(1/scale * math.cos(msg.theta) + 800))\n", | ||
" \n", | ||
" turtlesim.move_turtles(new_poses=poses)\n", | ||
"\n", | ||
"\n", | ||
"\n", | ||
"\n", | ||
"def cb(msg):\n", | ||
" move_turtles(msg)\n", | ||
"\n", | ||
"\n", | ||
"turtle_control = jr2.Subscriber(moveNode, Pose, topic_name, cb)\n", | ||
"turtle_control.display()" | ||
] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "c9e44409-1f96-426f-9826-95f2ddff5119", | ||
"metadata": {}, | ||
"outputs": [], | ||
"source": [ | ||
"%%thread_cell2\n", | ||
"run = True\n", | ||
"i = 90\n", | ||
"pub = jr2.Publisher(moveNode, Pose, topic_name)\n", | ||
"\n", | ||
"while run:\n", | ||
" msg = Pose()\n", | ||
" msg.theta = i / 180 * math.pi\n", | ||
" pub.send_msg( msg)\n", | ||
" sleep(0.01)\n", | ||
" i += 1\n", | ||
"print(\"Done\")" | ||
] | ||
}, | ||
{ | ||
"cell_type": "code", | ||
"execution_count": null, | ||
"id": "0fd093f5-122e-4006-8ad8-813428356fbe", | ||
"metadata": {}, | ||
"outputs": [], | ||
"source": [] | ||
} | ||
], | ||
"metadata": { | ||
"kernelspec": { | ||
"display_name": "Python 3 (ipykernel)", | ||
"language": "python", | ||
"name": "python3" | ||
}, | ||
"language_info": { | ||
"codemirror_mode": { | ||
"name": "ipython", | ||
"version": 3 | ||
}, | ||
"file_extension": ".py", | ||
"mimetype": "text/x-python", | ||
"name": "python", | ||
"nbconvert_exporter": "python", | ||
"pygments_lexer": "ipython3", | ||
"version": "3.8.10" | ||
} | ||
}, | ||
"nbformat": 4, | ||
"nbformat_minor": 5 | ||
} |
Oops, something went wrong.