diff --git a/jupyros/__init__.py b/jupyros/__init__.py index d988d59..6bb258c 100644 --- a/jupyros/__init__.py +++ b/jupyros/__init__.py @@ -25,6 +25,7 @@ from .ros2.ros_widgets import * from .ros2.subscriber import * from .ros2.key_input import * + from .ros2.turtle_sim import * else: # Default to ROS1 diff --git a/jupyros/ros2/__init__.py b/jupyros/ros2/__init__.py index 67a25ae..63f22db 100644 --- a/jupyros/ros2/__init__.py +++ b/jupyros/ros2/__init__.py @@ -17,3 +17,5 @@ from ..ros2.ros_widgets import * from ..ros2.subscriber import * from ..ros2.key_input import * +from ..ros2.turtle_sim import * +from ..ros2.ipy import * diff --git a/jupyros/ros2/ipy.py b/jupyros/ros2/ipy.py new file mode 100644 index 0000000..4ba033b --- /dev/null +++ b/jupyros/ros2/ipy.py @@ -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 \ No newline at end of file diff --git a/jupyros/ros2/publisher.py b/jupyros/ros2/publisher.py index 9d06d68..9c51cf9 100644 --- a/jupyros/ros2/publisher.py +++ b/jupyros/ros2/publisher.py @@ -86,6 +86,7 @@ def __init__(self, node: Node, msg_type: MsgType, topic: str, rate = None ) -> N "send_btn": widgets.Button(description="Send Message"), "txt_input": widgets.Text(description="Message", value="Something") } + self.vbox = None if(rate): self.node.create_timer(rate, self.__send_msg) 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: )) self.__widget_list.append(btm_box) vbox = widgets.VBox(children=self.__widget_list) - + self.vbox = vbox return vbox def send_msg(self, args): @@ -139,7 +140,7 @@ def __send_msg(self, args): """ Generic call to send message. """ self.msg_inst = self.msg_type() - if(self.widget_list): + if(self.vbox): self.widget_dict_to_msg() self.__publisher.publish(self.msg_inst) else: diff --git a/jupyros/ros2/turtle_sim.py b/jupyros/ros2/turtle_sim.py new file mode 100644 index 0000000..964ab77 --- /dev/null +++ b/jupyros/ros2/turtle_sim.py @@ -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 diff --git a/notebooks/ROS2_Turtlesim.ipynb b/notebooks/ROS2_Turtlesim.ipynb new file mode 100644 index 0000000..6facf1e --- /dev/null +++ b/notebooks/ROS2_Turtlesim.ipynb @@ -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 +} diff --git a/notebooks/ROS2_Turtlesim_KeyboardControl.ipynb b/notebooks/ROS2_Turtlesim_KeyboardControl.ipynb new file mode 100644 index 0000000..fd0fa0b --- /dev/null +++ b/notebooks/ROS2_Turtlesim_KeyboardControl.ipynb @@ -0,0 +1,230 @@ +{ + "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", + "from time import time\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", + "keyInput = rp.create_node(\"keyInput\")\n", + "com = rp.create_node(\"com\")\n", + "simple = rp.create_node(\"simple\")\n", + "translate = rp.create_node(\"translate\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3bdbfe05-c4d3-4fd3-abfa-f38b039d4541", + "metadata": {}, + "outputs": [], + "source": [ + "key_send = jr2.KeyInput(keyInput, String, '/keyboard_stream')\n", + "key_send.display_inputs()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2dbf024d", + "metadata": { + "tags": [] + }, + "outputs": [], + "source": [ + "\n", + "turtlesim = turtle.TurtleSim(background_color=\"#0000FF\")\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2bfe401a-2a01-4f75-839a-411b221bac8e", + "metadata": {}, + "outputs": [], + "source": [ + "\n", + "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": {}, + "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": [ + "turtlesim.turtles[name].pose" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "cd2e66dc", + "metadata": {}, + "outputs": [], + "source": [ + "topic_name = '/keyboard_stream'\n", + "def move_turtles(msg):\n", + " scale = 0.1\n", + " name = \"turtle1\"\n", + "\n", + " poses[name] = {\"x\": turtlesim.turtles[name].pose[\"x\"] + msg.linear.x*math.cos(turtlesim.turtles[name].pose[\"theta\"])/scale,\n", + " \"y\": turtlesim.turtles[name].pose[\"y\"] - msg.linear.x*math.sin(turtlesim.turtles[name].pose[\"theta\"] )/scale,\n", + " \"theta\": turtlesim.turtles[name].pose[\"theta\"] + msg.angular.z/ 180 * math.pi}\n", + " \n", + " \n", + " turtlesim.move_turtles(new_poses=poses)\n", + "\n", + "\n", + "\n", + "\n", + "def cb(msg):\n", + " # Receive string msg and translate\n", + " \n", + " key = msg.data\n", + " \n", + " #### General Keybindings \"key: (x, y, z, a (alpha), b (beta), g (gamma)\"\n", + " \n", + " moveBindings = {\n", + " 'ArrowLeft': (0,0,0,0, 0, 1),\n", + " 'ArrowRight':(0,0,0,0, 0, -1),\n", + " 'ArrowUp': (1,0,0,0, 0, 0, 0),\n", + " 'ArrowDown': (-1,0,0,0, 0, 0)\n", + " }\n", + "\n", + " \"\"\"\n", + " 'i': (1, 0, 0, 0, 0, 0),\n", + " 'o': (1, 0, 0, -1, 0, 0)),\n", + " \"\"\"\n", + " \n", + " ## General Keybinding Decoder\n", + " if key in moveBindings.keys():\n", + " x = float(moveBindings[key][0])\n", + " y = float(moveBindings[key][1])\n", + " z = float(moveBindings[key][2])\n", + " a = float(moveBindings[key][3])\n", + " b = float(moveBindings[key][4])\n", + " g = float(moveBindings[key][5])\n", + " \n", + " else:\n", + " x = 0.0\n", + " y = 0.0\n", + " z = 0.0\n", + " a = 0.0\n", + " b = 0.0\n", + " g = 0.0\n", + " \n", + "\n", + " \n", + " twist = Twist()\n", + " twist.linear.x = x\n", + " twist.angular.z = g \n", + "\n", + " move_turtles(twist)\n", + "\n", + "\n", + "turtle_control = jr2.Subscriber(keyInput, String, topic_name, cb)\n", + "turtle_control.display()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "c0cd916b-41a7-4832-9470-328c1d30689b", + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "69fbd5a8-f709-4782-9d46-f7dfc5a27fbc", + "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 +}