Semana 26

docs/_posts/2024-06-03-Semana-26.md

@@ -0,0 +1,34 @@
+---
+title: "Semana 26 - Aumentado de dataset"
+categories:
+  - Registro Semanal
+tags:
+  - ROS2
+  - Aprendizaje Automático
+---
+
+Esta semana solo se basó en un aumentado bastante significativo del dataset generando varios mapas:
+
+
+
+<figure class="align-center" style="width:90%">
+  <img src="{{ site.url }}{{ site.baseurl }}/assets/images/post26/circuit1.png" alt="">
+  <figcaption>Circuito 1</figcaption>
+</figure>
+
+<figure class="align-center" style="width:90%">
+  <img src="{{ site.url }}{{ site.baseurl }}/assets/images/post26/circuit2.png" alt="">
+  <figcaption>Circuito 2</figcaption>
+</figure>
+
+<figure class="align-center" style="width:90%">
+  <img src="{{ site.url }}{{ site.baseurl }}/assets/images/post26/doors.png" alt="">
+  <figcaption>Circuito 2</figcaption>
+</figure>
+
+<figure class="align-center" style="width:90%">
+  <img src="{{ site.url }}{{ site.baseurl }}/assets/images/post26/spiral.png" alt="">
+  <figcaption>Spiral 2</figcaption>
+</figure>
+
+El resultado en crudo del dataset manual fue de 70,000 muestras. Sumando las 60,000 ya obtenidas anteriormente más las 40,000 del dataset automático, obtenemos un total de 170,000 muestras en crudo.

drone_sim_driver/src/dataset/remoteControl.py

@@ -83,7 +83,7 @@ def __init__(self, drone_id: str = "drone0", verbose: bool = False,
         self.min_angular = 0.0
         self.max_linear = 12.0
         self.min_linear = 0.0
-        self.max_z = 4.0
+        self.max_z = 20.0
         self.min_z = 0.5
 
         # Button controllers
@@ -302,7 +302,7 @@ def velocity_control(self, msg):
         # Z axis position control
         if abs(msg.axis_right_y) > 0.5 and (time.time() - self.lastCommanded) > self.joystickPeriod and not self.constZ:
             # Sets the correct sign
-            self.posZ += 0.1 * abs(msg.axis_right_y) / msg.axis_right_y
+            self.posZ += 0.3 * abs(msg.axis_right_y) / msg.axis_right_y
 
             # Security limits
             if self.posZ > self.max_z:
@@ -473,7 +473,7 @@ def remote_control(self):
 
             # Publish the info for training
             vels.x = float(linearVel)
-            vels.y = float(linearVelRaw)
+            vels.y = float(self.posZ)
             vels.z = float(angularVel)
             self.velPublisher_.publish(vels)
 

drone_sim_driver/src/features/rosbag2generalDataset.py

@@ -125,10 +125,10 @@ def main():
     # Instantiate rosbagDataset with the provided path
     dataset = rosbagDataset(args.rosbags_path)
 
-    img_topic = "/drone0/sensor_measurements/frontal_camera/image_raw"
-    vel_topic = "/drone0/commanded_vels"    
-    # img_topic = "/cf0/sensor_measurements/hd_camera/image_raw"
-    # vel_topic = "commanded_vels"
+    # img_topic = "/drone0/sensor_measurements/frontal_camera/image_raw"
+    # vel_topic = "/drone0/commanded_vels"    
+    img_topic = "/cf0/sensor_measurements/hd_camera/image_raw"
+    vel_topic = "commanded_vels"
 
     dataset.transform_data(img_topic, vel_topic)
 

drone_sim_driver/src/generateGateWorld.py

@@ -1,6 +1,8 @@
 import math
+import numpy as np
+import random
 
-EXTRA_ROTATION = math.pi
+EXTRA_ROTATION = 0#math.pi
 
 def add_circle(center_x, center_y, radius, num_gates, rad_ang):
   positions = []
@@ -41,30 +43,48 @@ def add_line(center_x, center_y, length, num_gates, orientation):
         positions.append((x, y, 0, 0, 0, orientation + EXTRA_ROTATION))
 
     return positions
-
-
-
-# Random poses
-poses = [
-    (5, 0, 0, 0, 0, 0),
-    (12, 30, 0, 0, 0, 0),
-    (9, 12, 0, 0, 0, 0),
-    (32, 13, 0, 0, 0, 0),
-    (25, -9, 0, 0, 0, 2),
-    (27, -25, 0, 0, 0, 1.8),
-    (7, -26, 0, 0, 0, 1),
-]
 
-# Center of the circle
-radius = 10
-num_gates = 20
+def ellipse():
+  # Random poses
+  num_gates = 50
+  theta = np.linspace(0, 2 * np.pi, num_gates, endpoint=False)  # Angle for each gate
+
+  # Define the radius and center for the oval path
+  a = 50  # Semi-major axis (horizontal radius)
+  b = 30  # Semi-minor axis (vertical radius)
+  center_x = 0
+  center_y = 0
+
+  xposes = center_x + a * np.cos(theta)
+  yposes = center_y + b * np.sin(theta)
+
+  # Calculate the orientation of each point
+  orientations = []
+  for i in range(num_gates):
+      next_i = (i + 1) % num_gates  # Next index, wrapping around for the last point
+      dx = xposes[next_i] - xposes[i]
+      dy = yposes[next_i] - yposes[i]
+      orientation = np.arctan2(dy, dx)
+      orientations.append(orientation)
+
+  poses = [(xposes[i], yposes[i], 0, 0, 0, orientations[i]) for i in range(num_gates)]
+
+  return poses
+
+
+
+n_gates = 50
+
+poses = [(3, 0, 0, 0, 0, math.pi)]
+
+for i in  range(n_gates):
+  x = random.randint(8, 20) + poses[-1][0]
+  y = random.randint(-10, 10) + poses[-1][1]
+  z = random.randint(-1, 4) 
+  orient = np.arctan2(poses[-1][1] - y, poses[-1][0] - x)
 
-# Adds the circle
-poses.extend(add_circle(-24, 0.17, radius, num_gates, 2* math.pi))
-poses.extend(add_circle(-24, -40.17, radius*1.5, num_gates/3, math.pi))
+  poses.append((x, y, z, 0, 0, orient))
 
-# Adds a line
-poses.extend(add_line(-45, 20,70, 25, -math.pi/2))
 
 
 xml_template = '''
@@ -84,7 +104,7 @@ def add_line(center_x, center_y, length, num_gates, orientation):
   <world name="ocean">
 
     <include>
-      <uri>model://ground_plane</uri>
+      <uri>model://grass_plane</uri>
     </include>
 	<scene>
       <grid>false</grid>