[WIP] Marker based visual loc

database/exercises/db.sql

@@ -96,6 +96,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, templat
 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/
+13	marker_visual_loc	Marker Visual Loc	Calculate the position of the robot based on the	{"tags": "ROS2"}	ACTIVE	RoboticsAcademy/exercises/static/exercises/marker_visual_loc/python_template/
 \.
 
 
@@ -131,6 +132,7 @@ COPY public.exercises_universes (id, exercise_id, universe_id) FROM stdin;
 25	1	24
 26	1	25
 27	12	27
+28	13	29
 \.
 
 

exercises/models.py

@@ -17,6 +17,8 @@
     ('console', "Console"),
     ('gazebo_gra', "Gazebo GRA"),
     ('gazebo_rae', "Gazebo RAE"),
+    ('gzsim_gra', "Gz Sim GRA"),
+    ('gzsim_rae', "Gz Sim RAE"),
     ('physic_gra', "Physic GRA"),
     ('physic_rae', "Physic RAE")
 )

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

@@ -0,0 +1,95 @@
+import json
+import cv2
+import base64
+import threading
+import time
+import numpy as np
+
+from map import Map
+
+from gui_interfaces.general.measuring_threading_gui import MeasuringThreadingGUI
+from console_interfaces.general.console import start_console
+
+from HAL import getPose3d, getOdom
+
+class GUI(MeasuringThreadingGUI):
+
+    def __init__(self, host="ws://127.0.0.1:2303", freq=30.0):
+        super().__init__(host)
+
+        self.image = None
+        self.image_lock = threading.Lock()
+
+        self.predict_pose = None
+        self.map = Map(getPose3d, getOdom)
+
+        # Payload vars
+        self.payload = {"image": "", "real_pose": "","noisy_pose": "", "estimate_pose": "" }
+
+        self.start()
+
+    # Prepares and send image to the websocket server
+    def update_gui(self):
+        pos_message = self.map.getRobotCoordinates()
+        pos_message = str(pos_message)
+        self.payload["real_pose"] = pos_message
+
+        n_pos_message = self.map.getRobotCoordinatesWithNoise()
+        n_pos_message = str(n_pos_message)
+        self.payload["noisy_pose"] = n_pos_message
+
+        if np.any(self.predict_pose):
+            p_pos_message = str(self.predict_pose)
+            self.payload["estimate_pose"] = p_pos_message
+
+        if np.any(self.image):
+            _, encoded_image = cv2.imencode(".JPEG", self.image)
+            b64 = base64.b64encode(encoded_image).decode("utf-8")
+            shape = self.image.shape
+        else:
+            b64 = None
+            shape = 0
+
+        payload_img = {
+            "image": b64,
+            "shape": shape,
+        }
+
+        self.payload["image"] = json.dumps(payload_img)
+        message = json.dumps(self.payload)
+        self.send_to_client(message)
+
+    # Functions to set the next image to be sent
+    def setImage(self, image):
+        with self.image_lock:
+            self.image = image
+
+    def setEstimatedRobotPose(self, pose):
+        self.predict_pose = pose
+
+
+host = "ws://127.0.0.1:2303"
+gui = GUI(host)
+
+# Redirect the console
+start_console()
+
+# Expose the user functions
+def showImage(image):
+    gui.setImage(image)
+
+def showEstimatedPose(pose):
+    """Pose must be (x, y, yaw)"""
+    x, y, yaw = pose
+
+    scale_y = -85
+    offset_y = -6.88
+    y = scale_y * (offset_y - y)
+
+    scale_x =  83
+    offset_x = 8
+    x = scale_x * (offset_x - x)
+
+    transformed_pose = (x, y, yaw)
+
+    gui.setEstimatedRobotPose(transformed_pose)

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

@@ -0,0 +1,71 @@
+import rclpy
+import sys
+import threading
+import time
+
+from hal_interfaces.general.motors import MotorsNode
+from hal_interfaces.general.odometry import OdometryNode
+from hal_interfaces.general.noise_odometry import NoisyOdometryNode
+from hal_interfaces.general.laser import LaserNode
+from hal_interfaces.general.camera import CameraNode
+
+freq = 90.0
+
+def __auto_spin() -> None:
+    while rclpy.ok():
+        executor.spin_once(timeout_sec=0)
+        time.sleep(1/freq)
+
+
+if not rclpy.ok():
+    rclpy.init(args=sys.argv)
+
+### HAL INIT ###
+motor_node = MotorsNode("/turtlebot3/cmd_vel", 4, 0.3)
+camera_node = CameraNode("/turtlebot3/camera/image_raw")
+odometry_node = OdometryNode("/turtlebot3/odom")
+noisy_odometry_node = NoisyOdometryNode("/turtlebot3/odom")
+
+# Spin nodes so that subscription callbacks load topic data
+executor = rclpy.executors.MultiThreadedExecutor()
+executor.add_node(camera_node)
+executor.add_node(odometry_node)
+executor.add_node(noisy_odometry_node)
+
+executor_thread = threading.Thread(target=__auto_spin, daemon=True)
+executor_thread.start()
+
+
+### GETTERS ### 
+
+# Pose
+def getPose3d():
+    try:
+        return odometry_node.getPose3d()
+    except Exception as e:
+        print(f"Exception in hal getPose3d {repr(e)}")  
+
+# Pose
+def getOdom():
+    try:
+        return noisy_odometry_node.getPose3d()
+    except Exception as e:
+        print(f"Exception in hal getPose3d {repr(e)}")  
+
+# Camera
+def getImage():
+    image = camera_node.getImage()
+    while image == None:
+        image = camera_node.getImage()
+    return image.data
+
+
+### SETTERS ###
+
+# Linear speed
+def setV(v):
+    motor_node.sendV(float(v))
+
+# Angular speed
+def setW(w):
+    motor_node.sendW(float(w))

exercises/static/exercises/marker_visual_loc/python_template/ros2_humble/map.py

@@ -0,0 +1,83 @@
+import numpy as np
+import math
+import random
+from math import pi as pi
+import cv2
+
+
+class Map:
+    def __init__(self, pose_getter, noisy_pose_getter):
+        self.pose_getter = pose_getter
+        self.noisy_pose_getter = noisy_pose_getter
+
+    def RTx(self, angle, tx, ty, tz):
+        RT = np.matrix(
+            [
+                [1, 0, 0, tx],
+                [0, math.cos(angle), -math.sin(angle), ty],
+                [0, math.sin(angle), math.cos(angle), tz],
+                [0, 0, 0, 1],
+            ]
+        )
+        return RT
+
+    def RTy(self, angle, tx, ty, tz):
+        RT = np.matrix(
+            [
+                [math.cos(angle), 0, math.sin(angle), tx],
+                [0, 1, 0, ty],
+                [-math.sin(angle), 0, math.cos(angle), tz],
+                [0, 0, 0, 1],
+            ]
+        )
+        return RT
+
+    def RTz(self, angle, tx, ty, tz):
+        RT = np.matrix(
+            [
+                [math.cos(angle), -math.sin(angle), 0, tx],
+                [math.sin(angle), math.cos(angle), 0, ty],
+                [0, 0, 1, tz],
+                [0, 0, 0, 1],
+            ]
+        )
+        return RT
+
+    def RTVacuum(self):
+        RTz = self.RTz(pi / 2, 50, 70, 0)
+        return RTz
+
+    def getRobotCoordinates(self):
+        pose = self.pose_getter()
+        x = pose.x
+        y = pose.y
+
+        scale_y = -85
+        offset_y = -6.88
+        y = scale_y * (offset_y - y)
+
+        scale_x =  83
+        offset_x = 8
+        x = scale_x * (offset_x - x)
+
+        return x, y, pose.yaw
+
+    def getRobotCoordinatesWithNoise(self):
+        pose = self.noisy_pose_getter()
+        x = pose.x
+        y = pose.y
+
+        scale_y = -85
+        offset_y = -6.88
+        y = scale_y * (offset_y - y)
+
+        scale_x =  83
+        offset_x = 8
+        x = scale_x * (offset_x - x)
+
+        return x, y, pose.yaw
+
+    # Function to reset
+    def reset(self):
+        # Nothing to do, service takes care!
+        pass