Merge pull request #142 from ldania/turtle

Turtle

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

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 *

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

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:

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

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
+}