Add Follow Person exercise and polished O3DE exercises

Author: HectorMartinezCabezas

RoboticsInfrastructure

@@ -0,0 +1,78 @@
+from rclpy.node import Node
+import sensor_msgs.msg
+from math import pi as PI
+import rclpy
+import numpy as np
+from sensor_msgs_py import point_cloud2
+from threading import Lock
+
+
+class LidarData:
+    def __init__(self):
+        self.points = []
+        self.intensities = []
+        self.timeStamp = 0.0
+        self.min_range = 0.1
+        self.max_range = 15.0
+        self.field_of_view = (2 * PI / 3, PI / 18)
+        self.is_dense = True
+
+    def __str__(self):
+        return (
+            f"LiDARData:\n"
+            f"  timestamp: {self.timeStamp}\n"
+            f"  points: {len(self.points)}\n"
+            f"  range: [{self.min_range}, {self.max_range}]\n"
+            f"  FOV: {self.field_of_view}\n"
+            f"  is_dense: {self.is_dense}"
+        )
+
+
+def pointCloud2LidarData(cloud):
+    lidar = LidarData()
+    if not cloud or cloud.width * cloud.height == 0:
+        return lidar
+
+    # Read XYZ points
+    lidar.points = list(
+        point_cloud2.read_points(cloud, field_names=("x", "y", "z"), skip_nans=False)
+    )
+
+    # Read intensities if available
+    if "intensity" in [f.name for f in cloud.fields]:
+        intensities = point_cloud2.read_points(
+            cloud, field_names=("intensity",), skip_nans=False
+        )
+        lidar.intensities = [i[0] for i in intensities]
+
+    # Timestamp (ROS 2 Time -> seconds)
+    lidar.timeStamp = cloud.header.stamp.sec + (cloud.header.stamp.nanosec * 1e-9)
+
+    # Validate point cloud
+    lidar.is_dense = all(
+        all(np.isfinite(coord) for coord in point) for point in lidar.points
+    )
+
+    return lidar
+
+
+class LidarNode(Node):
+    def __init__(self, topic):
+        super().__init__("lidar_node")
+        self._lock = Lock()
+        self.last_cloud_ = None
+        self.sub = self.create_subscription(
+            sensor_msgs.msg.PointCloud2, topic, self.pointcloud_callback, 10
+        )
+
+    def pointcloud_callback(self, cloud):
+        with self._lock:
+            self.last_cloud_ = cloud
+
+    def getLidarData(self):
+        with self._lock:
+            return pointCloud2LidarData(self.last_cloud_)
+
+    def get_point_cloud(self):
+        with self._lock:
+            return self.last_cloud_

common/hal_interfaces/hal_interfaces/general/lidar.py

@@ -138,8 +138,9 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, url) FR
 24	car_junction	Car Junction	Autonomous Navigation through traffic at road junction.	["AUTONOMOUS DRIVING","ROS2"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises/AutonomousCars/car_junction
 25	machine_vision	Machine Vision	Machine Vision exercise	["ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/machine_vision
 26	labyrinth_escape	Labyrinth Escape	Labyrinth Escape exercise	["ROS2","Drones"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/Drones/labyrinth_escape
-27	o3de_warehouse	O3DE Warehouse	Control an O3DE robot to organize a warehouse	["ROS2"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises
+27	o3de_laser_mapping	O3DE LaserMapping	Control an O3DE robot to organize a warehouse	["ROS2"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises
 28	o3de_follow_line	O3DE FollowLine	Control an O3DE robot to follow a line	["ROS2"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises
+29	o3de_follow_person	O3DE FollowPerson	Control an O3DE robot to follow a person	["ROS2"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises
 \.
 
 --
@@ -191,6 +192,7 @@ COPY public.exercises_universes (id, exercise_id, universe_id) FROM stdin;
 41	26	39
 42	27	40
 43	28	41
+44	29	42
 \.
 -- 30  16  3
 
@@ -282,6 +284,9 @@ COPY public.exercises_tools (id, exercise_id, tool_id) FROM stdin;
 81	28	console
 82	28	simulator
 83	28	web_gui
+84	29	console
+85	29	simulator
+86	29	web_gui
 \.
 
 --

database/exercises/db.sql

@@ -39,8 +39,8 @@ def __auto_spin() -> None:
     rclpy.init(args=None)
 
 ### HAL INIT ###
-motor_node = AckerMotorsNode("/chassis_link/cmd_vel", 4, 0.3, 2)
-camera_node = CameraNode("/chassis_link/color/image_color")
+motor_node = AckerMotorsNode("/cmd_vel", 4, 0.3, 2)
+camera_node = CameraNode("/cam_f1_left/image_raw")
 
 executor = rclpy.executors.MultiThreadedExecutor()
 executor.add_node(camera_node)

exercises/static/exercises/assets/img/o3de_follow_line_teaser.png

@@ -47,7 +47,7 @@ def __init__(self, host="ws://127.0.0.1:2303"):
         self.payload = {"image": "", "lap": "", "map": ""}
         self.bridge = CvBridge()
 
-        self.pose3d_object = OdometryNode("/chassis_link/odom")
+        self.pose3d_object = OdometryNode("/odom")
 
         self.executor = rclpy.executors.MultiThreadedExecutor()
         self.executor.add_node(self.webgui_publisher)
@@ -66,6 +66,10 @@ def __init__(self, host="ws://127.0.0.1:2303"):
             self.auto_image_thread.start()
 
         self.start()
+        
+    def get_real_time_factor(self):
+        """Anula la función de la clase padre para que no busque 'world_name' ni use Gazebo."""
+        pass
 
     def _setup_auto_mode(self):
         """Set up automatic image subscription"""

exercises/static/exercises/assets/img/o3de_follow_person_teaser.png

@@ -35,6 +35,10 @@ const WebGUI = () => {
 
   const updateCircuit = (universe) => {
     switch (universe) {
+      case "O3DE FollowLine":
+        circuitName = "o3de default";
+        setCircuitImg(defaultCircuit);
+        break;
       case "Simple Circuit":
         circuitName = "default";
         setCircuitImg(defaultCircuit);