Merge pull request #3211 from JdeRobot/end_to_end_visual_control_exercise

[New Exercise] RoboticsAcademy: End-to-End Visual Control

Author: javizqh

database/exercises/db.sql

@@ -134,6 +134,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, url) FR
 19	human_detection	Human Detection	Deep learning-based human detection.	["Computer Vision","Deep Learning"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/human_detection
 20	drone_gymkhana	Drone Gymkhana	Drone Gymkhana	["ROS2","Drones"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/Drones/drone_gymkhana
 21	power_tower_inspection	Power Tower Inspection	Power Tower inspection using drones	["ROS2","Drones"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/Drones/power_tower_inspection
+22	end_to_end_visual_control	End to End Visual Control	End to end visual control using deep learning.	["Computer Vision","Deep Learning","AUTONOMOUS DRIVING"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises/AutonomousCars/end_to_end_visual_control
 \.
 
 --
@@ -174,8 +175,12 @@ COPY public.exercises_universes (id, exercise_id, universe_id) FROM stdin;
 30	16	32
 31	14	33
 32	17	34
-33	20	36
-34	21	37
+33	22 	5
+34	22	6
+35	22	20
+36	22	21
+37	20	36
+38	21	37
 \.
 -- 30  16  3
 
@@ -246,6 +251,9 @@ COPY public.exercises_tools (id, exercise_id, tool_id) FROM stdin;
 60	21	web_gui
 61	21	console
 62	21	simulator
+63	22	console
+64	22	simulator
+65	22	web_gui
 \.
 
 --
@@ -385,4 +393,4 @@ ADD CONSTRAINT exercises_w_exercise_id_e4e984bc_fk_exercises FOREIGN KEY (exerci
 
 --
 -- PostgreSQL database dump complete
---
+--

exercises/static/exercises/assets/img/end_to_end_visual_control_teaser.png

@@ -0,0 +1,22 @@
+import time
+from datetime import datetime
+
+start_time_internal_freq_control = None
+
+
+def tick(ideal_cycle: int = 50):
+    global start_time_internal_freq_control
+    finish_time = datetime.now()
+    ideal_ms = 1000 / ideal_cycle
+
+    if start_time_internal_freq_control == None:
+        start_time_internal_freq_control = finish_time
+        return
+
+    dt = finish_time - start_time_internal_freq_control
+    ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0
+
+    if ms < ideal_ms:
+        time.sleep((ideal_ms - ms) / 1000.0)
+
+    start_time_internal_freq_control = finish_time

exercises/static/exercises/end_to_end_visual_control/python_template/ros2_humble/Frequency.py

@@ -0,0 +1,65 @@
+import rclpy
+import sys
+import threading
+import time
+
+from hal_interfaces.general.motors import MotorsNode
+from hal_interfaces.general.camera import CameraNode
+
+
+IMG_WIDTH = 320
+IMG_HEIGHT = 240
+
+freq = 90.0  # Less than this wont work
+
+
+# Mutes exceptions
+def custom_thread_excepthook(args):
+    if "spin" in args.thread.name:
+        return
+    sys.__excepthook__(args.exc_type, args.exc_value, args.exc_traceback)
+
+
+threading.excepthook = custom_thread_excepthook
+
+
+def __auto_spin() -> None:
+    while rclpy.ok():
+        try:
+            executor.spin_once(timeout_sec=0)
+        except Exception:
+            pass
+        time.sleep(1 / freq)
+
+
+# ROS2 init
+if not rclpy.ok():
+    rclpy.init(args=sys.argv)
+
+# ROS2 Topics
+motor_node = MotorsNode("/cmd_vel", 4, 0.3)
+camera_node = CameraNode("/cam_f1_left/image_raw")
+
+# Spin nodes so that subscription callbacks load topic data
+executor = rclpy.executors.MultiThreadedExecutor()
+executor.add_node(camera_node)
+executor_thread = threading.Thread(target=__auto_spin, daemon=True)
+executor_thread.start()
+
+
+# Get Image from ROS Driver Camera
+def getImage():
+    image = camera_node.getImage()
+    while image == None:
+        image = camera_node.getImage()
+    return image.data
+
+
+# Set the velocity
+def setV(velocity):
+    motor_node.sendV(float(velocity))
+
+
+# Set the angular velocity
+def setW(velocity):
+    motor_node.sendW(float(velocity))

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

@@ -0,0 +1,116 @@
+import cv2
+import base64
+import json
+import threading
+import rclpy
+from gui_interfaces.general.measuring_threading_gui import MeasuringThreadingGUI
+from console_interfaces.general.console import start_console
+from hal_interfaces.general.odometry import OdometryNode
+from lap import Lap
+
+import sys
+
+sys.path.insert(0, "/RoboticsApplicationManager")
+
+from manager.ram_logging.log_manager import LogManager
+
+# Graphical User Interface Class
+
+
+class WebGUI(MeasuringThreadingGUI):
+    def __init__(self, host="ws://127.0.0.1:2303"):
+        super().__init__(host)
+
+        self.image_to_be_shown = None
+        self.image_to_be_shown_updated = False
+        self.image_show_lock = threading.Lock()
+
+        # Payload vars
+        self.payload = {"image": "", "lap": "", "map": ""}
+        # TODO: maybe move this to HAL and have it be hybrid
+        self.pose3d_object = OdometryNode("/odom")
+        executor = rclpy.executors.MultiThreadedExecutor()
+        executor.add_node(self.pose3d_object)
+        executor_thread = threading.Thread(target=executor.spin, daemon=True)
+        executor_thread.start()
+        self.lap = Lap(self.pose3d_object)
+
+        self.start()
+
+    # Process incoming messages to the GUI
+    def gui_in_thread(self, ws, message):
+
+        # In this case, incoming msgs can only be acks
+        if "ack" in message:
+            with self.ack_lock:
+                self.ack = True
+                self.ack_frontend = True
+        elif "start" in message:
+            self.lap.unpause()
+        elif "pause" in message:
+            self.lap.pause()
+        else:
+            LogManager.logger.error("Unsupported msg")
+
+    # Prepares and sends a map to the websocket server
+    def update_gui(self):
+
+        payload = self.payloadImage()
+        self.payload["image"] = json.dumps(payload)
+
+        # Payload Lap Message
+        lapped = self.lap.check_threshold()
+        self.payload["lap"] = ""
+        if lapped != None:
+            self.payload["lap"] = str(lapped)
+
+        # Payload Map Message
+        pose = self.pose3d_object.getPose3d()
+        pos_message = str((pose.x, pose.y))
+        self.payload["map"] = pos_message
+
+        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_show_lock:
+            image_to_be_shown_updated = self.image_to_be_shown_updated
+            image_to_be_shown = self.image_to_be_shown
+
+        image = image_to_be_shown
+        payload = {"image": "", "shape": ""}
+
+        if not image_to_be_shown_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_show_lock:
+            self.image_to_be_shown_updated = False
+
+        return payload
+
+    # Function for student to call
+    def showImage(self, image):
+        with self.image_show_lock:
+            self.image_to_be_shown = image
+            self.image_to_be_shown_updated = True
+
+
+host = "ws://127.0.0.1:2303"
+gui = WebGUI(host)
+
+# Redirect the console
+start_console()
+
+
+# Expose to the user
+def showImage(image):
+    gui.showImage(image)

exercises/static/exercises/end_to_end_visual_control/python_template/ros2_humble/WebGUI.py

@@ -0,0 +1,59 @@
+from datetime import datetime
+
+
+class Lap:
+    def __init__(self, pose3d):
+        self.pose3d = pose3d
+        self.reset()
+
+    # Function to check for threshold
+    # And incrementing the running time
+    def check_threshold(self):
+        pose3d = self.pose3d.getPose3d()
+
+        # Variable for pause
+        if self.pause_condition == False:
+            # Running condition to calculate the current time
+            # Time calculated by adding increments from each iteration
+            if self.start_time != 0 and self.lap_rest == False:
+                if self.lap_time == 0:
+                    self.lap_time = datetime.now() - self.start_time
+                else:
+                    self.lap_time += datetime.now() - self.start_time
+
+                self.start_time = datetime.now()
+
+            # Condition when the time starts running
+            if self.start_time == 0 and self.lap_rest == True:
+                self.start_time = datetime.now()
+                self.lap_rest = False
+
+        return self.lap_time
+
+    # Function to return lap time
+    def return_lap_time(self):
+        return self.lap_time
+
+    # Function to reset
+    def reset(self):
+        # Reset the initial variables
+        self.start_time = 0
+        self.lap_time = 0
+
+        self.lap_rest = True
+        self.buffer = False
+
+        self.pause_condition = False
+
+    # Function to pause
+    def pause(self):
+        self.pause_condition = True
+
+    # Function to unpause
+    def unpause(self):
+        # To enable unpause button to be used again and again
+        if self.pause_condition == True:
+            # Next time the time will be incremented accordingly
+            self.start_time = datetime.now()
+
+        self.pause_condition = False

exercises/static/exercises/end_to_end_visual_control/python_template/ros2_humble/lap.py

@@ -0,0 +1,9 @@
+model_dir_path = "/workspace/code/model.onnx"
+
+
+# This file is part of the Human Detection ROS2 package.
+def model_path_func() -> str:
+    return model_dir_path
+
+
+model_path = model_path_func()

exercises/static/exercises/end_to_end_visual_control/python_template/ros2_humble/model.py

@@ -0,0 +1,14 @@
+import * as React from "react";
+import {Fragment} from "react";
+
+import "./css/FollowLineRR.css";
+
+const FollowLineRR = (props) => {
+  return (
+    <Fragment>
+    {props.children}
+    </Fragment>
+  );
+};
+
+export default FollowLineRR;