Merge pull request #2893 from JdeRobot/montecarlo_visual_loc

Montecarlo Visual Loc

database/exercises/db.sql

@@ -95,6 +95,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, templat
 9	3d_reconstruction	3D Reconstruction	3D Reconstruction exercise using React and RAM	{"tags": ["ROS2","COMPUTER VISION"]}	ACTIVE	RoboticsAcademy/exercises/static/exercises/3d_reconstruction/python_template/
 10	amazon_warehouse	Amazon Warehouse	Control an amazon-like robot to organize a warehouse	{"tags": "ROS2"}	ACTIVE	RoboticsAcademy/exercises/static/exercises/amazon_warehouse/python_template/
 11	montecarlo_laser_loc	Montecarlo Laser Loc	Calculate the position of the robot based on the	{"tags": "ROS2"}	ACTIVE	RoboticsAcademy/exercises/static/exercises/montecarlo_laser_loc/python_template/
+12	montecarlo_visual_loc	Montecarlo Visual Loc	Calculate the position of the robot based on the	{"tags": "ROS2"}	ACTIVE	RoboticsAcademy/exercises/static/exercises/montecarlo_visual_loc/python_template/
 \.
 
 
@@ -129,6 +130,7 @@ COPY public.exercises_universes (id, exercise_id, universe_id) FROM stdin;
 24	1	23
 25	1	24
 26	1	25
+27	12	27
 \.
 
 

exercises/static/exercises/montecarlo_visual_loc/python_template/ros2_humble/GUI.py

@@ -0,0 +1,137 @@
+import json
+#import math
+#import matplotlib.pyplot as plt
+import threading
+import cv2
+import base64
+
+from gui_interfaces.general.measuring_threading_gui import MeasuringThreadingGUI
+from console_interfaces.general.console import start_console
+from map import Map
+from HAL import getPose3d
+
+# Graphical User Interface Class
+
+class GUI(MeasuringThreadingGUI):
+
+    def __init__(self, host="ws://127.0.0.1:2303"):
+        super().__init__(host)
+
+        # Payload vars
+        self.payload = {'image': '', 'map': '', 'user': '', 'particles': ''}
+        self.init_coords = (171, 63)
+        self.start_coords = (201, 85.5)
+        self.map = Map(getPose3d)
+
+        # Particles
+        self.particles = []
+        # Image
+        self.image = None
+        self.image_lock = threading.Lock()
+        self.image_updated = False
+        # User position
+        self.user_position = (0, 0)
+        self.user_angle = (0, 0)
+
+        self.start()
+
+    # Prepares and sends a map to the websocket server
+    def update_gui(self):
+
+        # Payload Image Message
+        payload_image = self.payloadImage()
+        self.payload["image"] = json.dumps(payload_image)
+
+        # Payload Map Message
+        pos_message = self.map.getRobotCoordinates()
+        if pos_message == self.init_coords:
+            pos_message = self.start_coords
+        ang_message = self.map.getRobotAngle()
+        pos_message = str(pos_message + ang_message)
+        self.payload["map"] = pos_message
+
+        # Payload User Message
+        pos_message_user = self.user_position
+        ang_message_user = self.user_angle
+        pos_message_user = pos_message_user + ang_message_user
+        pos_message_user = str(pos_message_user)
+        self.payload["user"] = pos_message_user
+
+        # Payload Particles Message
+        if self.particles:
+            self.payload["particles"] = json.dumps(self.particles)
+        else:
+            self.payload["particles"] = json.dumps([])
+
+        message = json.dumps(self.payload)
+        self.send_to_client(message)
+
+    # Function to prepare image payload
+    # Encodes the image as a JSON string and sends through the WS
+    def payloadImage(self):
+        with self.image_lock:
+            image_updated = self.image_updated
+            image_to_be_shown = self.image
+
+        image = image_to_be_shown
+        payload = {'image': '', 'shape': ''}
+
+        if not image_updated:
+            return payload
+
+        shape = image.shape
+        frame = cv2.imencode('.JPEG', image)[1]
+        encoded_image = base64.b64encode(frame)
+
+        payload['image'] = encoded_image.decode('utf-8')
+        payload['shape'] = shape
+
+        with self.image_lock:
+            self.image_updated = False
+
+        return payload
+
+    def showPosition(self, x, y, angle):
+        scale_y = 15; offset_y = 63
+        y = scale_y * y + offset_y
+
+        scale_x = -30; offset_x = 171
+        x = scale_x * x + offset_x
+
+        self.user_position = x, y
+        self.user_angle = angle,
+
+    def showParticles(self, particles):
+        if particles:
+            self.particles = particles
+            scale_y = 15
+            offset_y = 63
+            scale_x = -30
+            offset_x = 171
+            for particle in self.particles:
+                particle[1] = scale_y * particle[1] + offset_y
+                particle[0] = scale_x * particle[0] + offset_x
+        else:
+            self.particles = []
+
+    # Function to set the next image to be sent
+    def setImage(self, image):
+        with self.image_lock:
+            self.image = image
+            self.image_updated = True
+
+host = "ws://127.0.0.1:2303"
+gui = GUI(host)
+
+# Redirect the console
+start_console()
+
+# Expose to the user
+def showImage(img):
+    gui.setImage(img)
+
+def showPosition(x, y, angle):
+    gui.showPosition(x, y, angle)
+
+def showParticles(particles):
+    gui.showParticles(particles)

exercises/static/exercises/montecarlo_visual_loc/python_template/ros2_humble/HAL.py

@@ -0,0 +1,52 @@
+import rclpy
+import threading
+import time
+
+from hal_interfaces.general.motors import MotorsNode
+from hal_interfaces.general.odometry import OdometryNode
+from hal_interfaces.general.camera import CameraNode
+
+# Hardware Abstraction Layer
+IMG_WIDTH = 320
+IMG_HEIGHT = 240
+
+freq = 30.0
+
+print("HAL initializing", flush=True)
+if not rclpy.ok():
+    rclpy.init(args=None)
+
+    ### HAL INIT ###
+    motor_node = MotorsNode("/cmd_vel", 4, 0.3)
+    odometry_node = OdometryNode("/odom")
+    camera_node = CameraNode("/camera/image_raw")
+
+    # Spin nodes so that subscription callbacks load topic data
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(odometry_node)
+    executor.add_node(camera_node)
+    def __auto_spin() -> None:
+        while rclpy.ok():
+            executor.spin_once(timeout_sec=0)
+            time.sleep(1/freq)
+    executor_thread = threading.Thread(target=__auto_spin, daemon=True)
+    executor_thread.start()
+
+# Pose
+def getPose3d():
+    return odometry_node.getPose3d()
+
+# Image
+def getImage():
+    image = camera_node.getImage()
+    while image == None:
+        image = camera_node.getImage()
+    return image.data
+
+# Linear speed
+def setV(v):
+    motor_node.sendV(float(v))
+
+# Angular speed
+def setW(w):
+    motor_node.sendW(float(w))

exercises/static/exercises/montecarlo_visual_loc_newmanager/python_template/ros1_noetic/map.py → exercises/static/exercises/montecarlo_visual_loc/python_template/ros2_humble/map.py

@@ -27,7 +27,7 @@ def RTVacuum(self):
 		return RTz
 
 	def getRobotCoordinates(self):
-		pose = self.pose3d.getPose3d()
+		pose = self.pose3d()
 		x = pose.x
 		y = pose.y
 
@@ -40,16 +40,12 @@ def getRobotCoordinates(self):
 		return x, y
 
 	def getRobotAngle(self):
-		pose = self.pose3d.getPose3d()
-		rt = pose.yaw
+		pose = self.pose3d()
 
-		ty = math.cos(-rt) - math.sin(-rt)
-		tx = math.sin(-rt) + math.cos(-rt)
-
-		return tx, ty
+		return pose.yaw,
 
 	# Function to reset
 	def reset(self):
 		# Nothing to do, service takes care!
 		pass
-
+

exercises/static/exercises/montecarlo_visual_loc_newmanager/react-components/MontecarloVisualLoc.js → exercises/static/exercises/montecarlo_visual_loc/react-components/MontecarloVisualLoc.js

@@ -11,4 +11,4 @@ const MontecarloVisualLoc = (props) => {
   );
 };
 
-export default MontecarloVisualLoc;
+export default MontecarloVisualLoc;