Cicular Movement

ldania · IsabelParedes · commit fed4ae48efe6 · 2023-03-03T17:10:56.000+01:00
diff --git a/jupyros/ros2/turtle_sim.py 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
@@ -12,7 +12,6 @@
     "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",
@@ -78,7 +77,7 @@
     {
      "data": {
       "application/vnd.jupyter.widget-view+json": {
-       "model_id": "77f04f8ef9634ee590f4c6d02026d8b8",
+       "model_id": "f7e9060702e74556b7652d4a783d9768",
        "version_major": 2,
        "version_minor": 0
       },
@@ -107,9 +106,6 @@
    "execution_count": 6,
    "id": "497db1e0-8c21-4ec0-b620-1607ab34d685",
    "metadata": {
-    "jupyter": {
-     "source_hidden": true
-    },
     "tags": []
    },
    "outputs": [
@@ -147,7 +143,7 @@
     {
      "data": {
       "application/vnd.jupyter.widget-view+json": {
-       "model_id": "2aff5baab18c49bb8af3faed876a372a",
+       "model_id": "9a96ef62b2b440e0a6edd111a08f2a22",
        "version_major": 2,
        "version_minor": 0
       },
@@ -160,23 +156,27 @@
     }
    ],
    "source": [
-    "topic_name = '/keyboard_stream'\n",
+    "topic_name = '/Pose'\n",
     "def move_turtles(msg):\n",
-    "    scale = 1\n",
+    "    scale = 0.0015\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",
+    "    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",
-    "    print(msg)\n",
     "    move_turtles(msg)\n",
     "\n",
     "\n",
@@ -193,7 +193,7 @@
     {
      "data": {
       "application/vnd.jupyter.widget-view+json": {
-       "model_id": "1f92a4ad257146fca78c363671b8181b",
+       "model_id": "b9d3b46917c34d3097c23523095f14df",
        "version_major": 2,
        "version_minor": 0
       },
@@ -208,26 +208,17 @@
    "source": [
     "%%thread_cell2\n",
     "run = True\n",
-    "i = 0\n",
+    "i = 90\n",
     "pub = jr2.Publisher(moveNode, Pose, topic_name)\n",
     "\n",
     "while run:\n",
     "    msg = Pose()\n",
-    "    msg.x = (i/1800*5)*math.sin(i / 180 * math.pi) + 11.08 / 2\n",
-    "    msg.y = (i/1800*5)*math.cos(i / 180 * math.pi) + 11.08 / 2\n",
-    "    msg.theta = - i / 180 * math.pi\n",
-    "    pub.send_msg(msg)\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": "606db6b1-f505-41b7-99d1-525b09cf662e",
-   "metadata": {},
-   "outputs": [],
-   "source": []
   }
  ],
  "metadata": {
diff --git a/notebooks/ROS2_Turtlesim_KeyboardControl.ipynb b/notebooks/ROS2_Turtlesim_KeyboardControl.ipynb
@@ -59,7 +59,7 @@
     {
      "data": {
       "application/vnd.jupyter.widget-view+json": {
-       "model_id": "fc8ee6a25ba64a3bb7d88184717a5672",
+       "model_id": "33726261d3ca48cfb41d862e9386ed5e",
        "version_major": 2,
        "version_minor": 0
       },
@@ -73,7 +73,7 @@
     {
      "data": {
       "application/vnd.jupyter.widget-view+json": {
-       "model_id": "4b231f376d4145c3a3086ae7ea55bb62",
+       "model_id": "62f02b8ea884432a85024d49449446dd",
        "version_major": 2,
        "version_minor": 0
       },
@@ -120,7 +120,7 @@
     {
      "data": {
       "application/vnd.jupyter.widget-view+json": {
-       "model_id": "d4a4365909444d9495776ffc4f282e02",
+       "model_id": "32c59818a7cd4fdfa129eca3b0cb238f",
        "version_major": 2,
        "version_minor": 0
       },
@@ -137,18 +137,6 @@
     "display(turtlesim.canvas)"
    ]
   },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "id": "347b508d-4568-4cc2-b8da-c05f26b6f606",
-   "metadata": {
-    "tags": []
-   },
-   "outputs": [],
-   "source": [
-    "!jupyter labextension install js"
-   ]
-  },
   {
    "cell_type": "markdown",
    "id": "ece1ece3-54f6-4df8-be79-42d7f37f6e08",
@@ -159,10 +147,18 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 7,
    "id": "497db1e0-8c21-4ec0-b620-1607ab34d685",
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "{'x': 800, 'y': 800, 'theta': 0}\n"
+     ]
+    }
+   ],
    "source": [
     "poses = {}\n",
     "\n",
@@ -174,20 +170,46 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 8,
    "id": "ac63dbbb-b388-4b18-890c-e3bcada044a9",
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "data": {
+      "text/plain": [
+       "{'x': 800, 'y': 800, 'theta': 0}"
+      ]
+     },
+     "execution_count": 8,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
    "source": [
     "turtlesim.turtles[name].pose"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 9,
    "id": "cd2e66dc",
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "data": {
+      "application/vnd.jupyter.widget-view+json": {
+       "model_id": "7806c8d4a3f04f5f8f6f64d0043ca113",
+       "version_major": 2,
+       "version_minor": 0
+      },
+      "text/plain": [
+       "VBox(children=(HBox(children=(Button(description='Start', style=ButtonStyle()), Button(description='Stop', sty…"
+      ]
+     },
+     "metadata": {},
+     "output_type": "display_data"
+    }
+   ],
    "source": [
     "topic_name = '/keyboard_stream'\n",
     "def move_turtles(msg):\n",
@@ -255,18 +277,10 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": null,
    "id": "c0cd916b-41a7-4832-9470-328c1d30689b",
    "metadata": {},
-   "outputs": [
-    {
-     "name": "stderr",
-     "output_type": "stream",
-     "text": [
-      "UsageError: %%thread_cell is a cell magic, but the cell body is empty.\n"
-     ]
-    }
-   ],
+   "outputs": [],
    "source": []
   },
   {
