[WIP] 360 lidar autoparking

common/hal_interfaces/hal_interfaces/general/lidar.py

@@ -0,0 +1,89 @@
+from rclpy.node import Node
+import sensor_msgs.msg
+from math import pi as PI
+
+
+### AUXILIARY FUNCTIONS
+class LidarData:
+
+    def __init__(self):
+
+        self.values = [] # Actual point data, size is (row_step*height)
+
+        # 2D structure of the point cloud. If the cloud is unordered, height is
+        # 1 and width is the length of the point cloud.
+        self.height = 0
+        self.width = 0
+
+        self.point_step = 0 # Length of a point in bytes
+        self.row_step = 0 # Length of a row in bytes
+        self.fields = [] # Describes the channels and their layout in the binary data blob.
+        self.is_dense = False # True if there are no invalid points
+        self.timeStamp = 0  # seconds
+
+    def __str__(self):
+        s = (
+            "LidarData: {\n   height: "
+            + str(self.height)
+            + "\n   width: "
+            + str(self.width)
+        )
+        s = (
+            s
+            + "\n   point_step: "
+            + str(self.point_step)
+            + "\n   row_step: "
+            + str(self.row_step)
+        )
+        s = (
+            s
+            + "\n   fields: "
+            + str(self.fields)
+            + "\n   is_dense: "
+            + str(self.is_dense)
+        )
+        s = (
+            s
+            + "\n   timeStamp: "
+            + str(self.timeStamp)
+            + "\n   values: "
+            + str(self.values)
+            + "\n}"
+        )
+        return s
+
+
+def lidarScan2LidarData(scan):
+    """
+    Translates from ROS LidarScan to JderobotTypes LidarData.
+    @param scan: ROS LidarScan to translate
+    @type scan: LidarScan
+    @return a LidarData translated from scan
+    """
+    lidar = LidarData()
+    lidar.values = scan.data
+    lidar.height = scan.height
+    lidar.width = scan.width
+    lidar.point_step = scan.point_step
+    lidar.row_step = scan.row_step
+    lidar.fields = scan.fields
+    lidar.is_dense = scan.is_dense
+    lidar.timeStamp = scan.header.stamp.sec + (scan.header.stamp.nanosec * 1e-9)
+    return lidar
+
+
+### HAL INTERFACE ###
+class LidarNode(Node):
+
+    def __init__(self, topic):
+        super().__init__("lidar_node")
+        self.sub = self.create_subscription(
+            sensor_msgs.msg.PointCloud2, topic, self.listener_callback, 10
+        )
+        self.last_scan_ = sensor_msgs.msg.PointCloud2()
+
+    def listener_callback(self, scan):
+        self.last_scan_ = scan
+
+    def getLidarData(self):
+        return lidarScan2LidarData(self.last_scan_)

common/hal_interfaces/hal_interfaces/specific/threed_reconstruction/parameters_camera.py

@@ -7,7 +7,7 @@ def __init__(self, configFile, cam):
 
         if os.getcwd() == "/":
             f = open(
-                "/RoboticsAcademy/exercises/static/exercises/3d_reconstruction_newmanager/python_template/ros2_humble/" + configFile, "r")
+                "/RoboticsAcademy/exercises/static/exercises/3d_reconstruction/python_template/ros2_humble/" + configFile, "r")
         else:
             f = open(configFile, "r")
 

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	autoparking_lidar	Autoparking with Lidar	Autoparking with Lidar exercise	{"tags": ["AUTONOMOUS DRIVING","SERVICE ROBOTS","ROS2"] }	ACTIVE	RoboticsAcademy/exercises/static/exercises/autoparking_lidar/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	26
 \.
 
 

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

@@ -0,0 +1,38 @@
+import json
+
+from gui_interfaces.general.measuring_threading_gui import MeasuringThreadingGUI
+from console_interfaces.general.console import start_console
+from map import Map
+from HAL import getLidarData
+
+# Graphical User Interface Class
+
+class GUI(MeasuringThreadingGUI):
+
+    def __init__(self, host="ws://127.0.0.1:2303"):
+        super().__init__(host)
+
+        # Payload vars
+        self.payload = {'map': ''}
+        self.map = Map(getLidarData)
+
+        self.start()
+
+    # Prepares and sends a map to the websocket server
+    def update_gui(self):
+
+        map_message = self.map.get_json_data()
+        self.payload["map"] = map_message
+        message = json.dumps(self.payload)
+        self.send_to_client(message)
+
+
+    def reset_gui(self):
+        """Resets the GUI to its initial state."""
+        self.map.reset()
+
+host = "ws://127.0.0.1:2303"
+gui = GUI(host)
+
+# Redirect the console
+start_console()

exercises/static/exercises/autoparking_lidar/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.lidar import LidarNode
+
+# Hardware Abstraction Layer
+IMG_WIDTH = 320
+IMG_HEIGHT = 240
+freq = 30.0
+
+# ROS2 init
+
+print("HAL initializing", flush=True)
+if not rclpy.ok():
+    rclpy.init(args=None)
+
+    ### HAL INIT ###
+    motor_node = MotorsNode("/prius_autoparking/cmd_vel", 4, 0.3)
+    odometry_node = OdometryNode("/prius_autoparking/odom")
+    lidar_node = LidarNode("/prius_autoparking/cloud")
+
+    # Spin nodes so that subscription callbacks load topic data
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(odometry_node)
+    executor.add_node(lidar_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()
+
+
+def getPose3d():
+    return odometry_node.getPose3d()
+
+def getLidarData():
+    lidar = lidar_node.getLidarData()
+    timestamp = lidar.timeStamp
+    while timestamp == 0.0:
+        lidar = lidar_node.getLidarData()
+        timestamp = lidar.timeStamp
+    return lidar
+
+def setV(velocity):
+    motor_node.sendV(float(velocity))
+
+def setW(velocity):
+    motor_node.sendW(float(velocity))

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

@@ -0,0 +1,36 @@
+import json
+import math
+import numpy as np
+
+class Map:
+	def __init__(self, lidar_object):
+		# Define the object used for
+		# websocket communication
+		self.payload = {}
+
+		self.lidar_topic = lidar_object
+
+    # Get the JSON data as string
+	def get_json_data(self):
+		self.payload["lasers"], self.payload["ranges"] = self.setLaserValues()
+
+		message = json.dumps(self.payload)
+		return message
+
+	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  	
+
+	# Interpret the Laser values
+	def setLaserValues(self):
+		# Init laser array
+		self.lidar = self.lidar_topic()
+		return self.lidar

exercises/static/exercises/autoparking_lidar/react-components/AutoparkingRR.js

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

exercises/static/exercises/autoparking_lidar/react-components/Lasers.js

@@ -0,0 +1,63 @@
+import React from "react"
+import "./css/GUICanvas.css";
+import Car from "../resources/images/car-top-view.svg";
+
+const Lasers = (props) => {
+    const meter = 73; // 1m = 73px
+
+    const [laser, setLaser] = React.useState([])
+    const [maxRange, setMaxRange] = React.useState([])
+
+    React.useEffect(() => {
+        const callback = (message) => {
+            if(message.data.update.map){
+              const map_data = JSON.parse(message.data.update.map);
+              console.log(map_data)
+              setLaser (map_data.lasers[0])
+              setMaxRange (map_data.ranges[0])
+              console.log(map_data.ranges)
+            }
+            // Send the ACK of the msg
+            window.RoboticsExerciseComponents.commsManager.send("gui", "ack");
+        };
+        RoboticsExerciseComponents.commsManager.subscribe(
+          [RoboticsExerciseComponents.commsManager.events.UPDATE],
+          callback
+        );
+
+        return () => {
+          console.log("TestShowScreen unsubscribing from ['state-changed'] events");
+          RoboticsExerciseComponents.commsManager.unsubscribe(
+            [RoboticsExerciseComponents.commsManager.events.UPDATE],
+            callback
+          );
+        };
+      }, []);
+
+    return (
+      <div style={{display: "flex",   width: "100%",
+        height: "100%", backgroundColor: "#363233", position:"relative", overflow:"hidden"
+      }}>
+        <img src={Car} id="car"/>
+        {laser.map(element => {
+          var ang = -element[1]
+          var length = (element[0] / 75) * meter;
+          return (
+            <hr className="laser-beam" 
+              style={{
+              rotate: "z "+ ang +"rad",
+              width: length + "px",
+              position: "absolute",
+              background: "repeating-linear-gradient(to right,rgb(255, 112, 112),rgb(255, 112, 112) 73px,rgb(175, 29, 29)  73px,rgb(175, 29, 29) 146px)",
+              backgroundSize: "100% 1px",
+              bottom: "59%",
+              left: "50%",
+              transformOrigin: "0% 0%",
+              zIndex: "3"}}
+            />
+        )})
+        }
+	    </div>
+)};
+
+export default Lasers;

exercises/static/exercises/autoparking_lidar/react-components/css/AutoparkingRR.css

@@ -0,0 +1,28 @@
+* {
+    box-sizing: border-box;
+}
+
+body, html {
+    width: 100%; height: 100%;
+}
+
+#exercise-container {
+    position: absolute;
+    top: 0; left: 0; bottom: 0; right: 0;
+    overflow: hidden;
+    display: flex;
+    flex-direction: column;
+}
+
+#exercise-container #content {
+    width: 100%;
+    height: 100%;
+    overflow: hidden;
+}
+
+#exercise-container #content #content-exercise {
+    display: flex;
+    flex-direction: column;
+    width: 100%;
+    height: 100%;
+}

exercises/static/exercises/autoparking_lidar/react-components/css/GUICanvas.css

@@ -0,0 +1,24 @@
+#car {
+  position: absolute;
+  width: 91px;
+  height: 100px;
+  top: calc(50% - 68px);
+  left: calc(50% - 45.5px);
+  z-index: 2
+}
+
+/* .laser-beam {
+  position: absolute;
+  background: repeating-linear-gradient(
+    to right,
+    rgb(112, 138, 255),
+    rgb(112, 138, 255) 73px,
+    rgb(100, 198, 255) 73px,
+    rgb(100, 198, 255) 146px
+  );
+  background-size: 100% 1px;
+  bottom: 50%;
+  left: 50%;
+  transform-origin: 0% 0%;
+  z-index: 3;
+} */