Merge pull request #2087 from JdeRobot/issue-2072

Issue 2072- Add Follow_person exercise

exercises/static/exercises/follow_person_newmanager/entry_point/ros2_humble/exercise.py

@@ -1,10 +1,10 @@
 import os.path
 from typing import Callable
 
-from src.manager.libs.applications.compatibility.exercise_wrapper import CompatibilityExerciseWrapper
+from src.manager.libs.applications.compatibility.exercise_wrapper_ros2 import CompatibilityExerciseWrapperRos2
 
 
-class Exercise(CompatibilityExerciseWrapper):
+class Exercise(CompatibilityExerciseWrapperRos2):
     def __init__(self, circuit: str, update_callback: Callable):
         current_path = os.path.dirname(__file__)
 

exercises/static/exercises/follow_person_newmanager/launch/ros2_humble/follow_person.launch.py

@@ -13,7 +13,7 @@ def generate_launch_description():
   pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
 
   # Set the path to the Turtlebot2 ROS package
-  pkg_turtlebot2 = FindPackageShare(package='custom_robots').find('custom_robots')
+  pkg_share_dir = FindPackageShare(package='custom_robots').find('custom_robots')
 
   # Set Turtlebot2 Arguments
   x_turtlebot2_position = '0'
@@ -33,10 +33,12 @@ def generate_launch_description():
     description="Position on the axis z of Turtlebot2"
   )
 
-  world_path = "/RoboticsAcademy/exercises/static/exercises/follow_person_newmanager/launch/ros2_humble/hospital_follow_person.world"
+  world_name = "hospital_follow_person_followingcam.world"
+  current_dir = "/RoboticsAcademy/exercises/static/exercises/follow_person_newmanager/launch/ros2_humble"
+  world_path = os.path.join(current_dir, world_name)
 
   # Set the path to the SDF model files
-  gazebo_models_path = os.path.join(pkg_turtlebot2, 'models')
+  gazebo_models_path = os.path.join(pkg_share_dir, 'models')
   os.environ["GAZEBO_MODEL_PATH"] = f"{os.environ.get('GAZEBO_MODEL_PATH', '')}:{':'.join(gazebo_models_path)}"
 
   ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ##############  
@@ -75,9 +77,8 @@ def generate_launch_description():
     launch_arguments={'world': world}.items())
 
   start_turtlebot2_cmd = IncludeLaunchDescription(
-    PythonLaunchDescriptionSource(os.path.join(pkg_turtlebot2, 'launch', 'spawn_model.launch.py')),
-    launch_arguments = {'-x': x_turtlebot2_position, '-y': y_turtlebot2_position, '-z': z_turtlebot2_position}.items()
-  )
+    PythonLaunchDescriptionSource(os.path.join(pkg_share_dir, 'launch', 'spawn_model.launch.py')),
+    launch_arguments = {'-x': x_turtlebot2_position, '-y': y_turtlebot2_position, '-z': z_turtlebot2_position}.items())
 
   # Create the launch description and populate
   ld = LaunchDescription()

exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/brain.py

@@ -28,17 +28,15 @@ def __init__(self, code, exit_signal, stop_signal):
         self.iteration_counter = 0
 
         # Get the sequential and iterative code
-        # Something wrong over here! The code is reversing
-        # Found a solution but could not find the reason for this
-        self.sequential_code = code[1]
-        self.iterative_code = code[0]
+        self.sequential_code = code[0]
+        self.iterative_code = code[1]
 
     # Function to run to start the process
     def run(self):
         # Two threads for running and measuring
         self.measure_thread = threading.Thread(target=self.measure_frequency)
         self.thread = threading.Thread(target=self.process_code)
-
+        
         self.measure_thread.start()
         self.thread.start()
 

exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/exercise.py

@@ -85,7 +85,7 @@ def parse_code(self, source_code):
 
         else:
             sequential_code, iterative_code = self.seperate_seq_iter(source_code[6:])
-            return iterative_code, sequential_code
+            return sequential_code, iterative_code
 
     # Function to seperate the iterative and sequential code
     def seperate_seq_iter(self, source_code):
@@ -127,13 +127,11 @@ def execute_thread(self, source_code):
                 pass
 
         # Turn the flag down, the iteration has successfully stopped!
-
         self.reload.clear()
         # New thread execution
         code = self.parse_code(source_code)
         if code[0] == "" and code[1] == "":
             return
-
         self.brain_process = BrainProcess(code, self.reload, self.stop_brain)
         self.brain_process.start()
         self.send_code_message()
@@ -205,17 +203,19 @@ def read_teleop_message(self, teleop_message):
         w = float(teleop_message["w"])
 
         return v, w
-
+    
     # The websocket function
     # Gets called when there is an incoming message from the client
     def handle(self, client, server, message):
         if(message[:5] == "#freq"):
             frequency_message = message[5:]
             self.read_frequency_message(frequency_message)
+            time.sleep(1)
             self.send_frequency_message()
             return
 
         elif(message[:5] == "#ping"):
+            time.sleep(1)
             self.send_ping_message()
             return
 
@@ -245,15 +245,17 @@ def handle(self, client, server, message):
 
         elif (message[:5] == "#code"):
             try:
+                print("\n\n LOAD CODE\n\n",self.brain_process)
                 # First pause the teleoperator thread if exists
-                if self.teleop.is_alive():
-                    self.exit_signal_teleop.set()
+                #if self.teleop.is_alive():
+                #    self.exit_signal_teleop.set()
 
                 # Once received turn the reload flag up and send it to execute_thread function
                 self.user_code = message
                 self.reload.set()
                 self.stop_brain.clear()
                 self.execute_thread(self.user_code)
+                print("\n\n LOAD CODE5\n\n")
             except:
                 pass
 

exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/hal.py

@@ -16,6 +16,8 @@
 from shared.laserdata import SharedLaserData
 from shared.pose3d import SharedPose3D
 
+from user_functions import HALFunctions
+
 # Hardware Abstraction Layer
 
 class HAL:
@@ -99,6 +101,9 @@ def setW(self):
         angular = self.shared_w.get()
         self.motors.sendW(angular)
 
+    def getBoundingBoxes(self, img):
+        return HALFunctions.getBoundingBoxes(img)
+
     def update_hal(self):
         self.getLaserData()
         self.getImage()

exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/interfaces/ssd_detection.py

@@ -1,7 +1,7 @@
 import cv2
 
-FROZEN_GRAPH = "/RoboticsAcademy/exercises/static/exercises/follow_person_newmanager/web-template/interfaces/pretrained_model/ssd_inception_v2_coco.pb"
-PB_TXT = "/RoboticsAcademy/exercises/static/exercises/follow_person_newmanager/web-template/interfaces/pretrained_model/ssd_inception_v2_coco.pbtxt"
+FROZEN_GRAPH = "/RoboticsAcademy/exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/interfaces/pretrained_model/ssd_inception_v2_coco.pb"
+PB_TXT = "/RoboticsAcademy/exercises/static/exercises/follow_person_newmanager/python_template/ros2_humble/interfaces/pretrained_model/ssd_inception_v2_coco.pbtxt"
 SIZE = 300
 
 class BoundingBox:

exercises/templates/exercises/follow_person_newmanager/exercise.html

@@ -0,0 +1,6 @@
+{% extends "react_frontend/exercise_base.html" %}
+{% load react_component %}
+
+{% block react-content %}
+{% react_component components/templates/FollowPersonRAM %} {% end_react_component %}
+{% endblock %}

react_frontend/src/components/templates/FollowPersonRAM.js

@@ -0,0 +1,35 @@
+import * as React from "react";
+import { Box } from "@mui/material";
+import { ViewProvider } from "../../contexts/ViewContext";
+import { ExerciseProvider } from "../../contexts/NewManagerExerciseContext";
+import NewManagerExerciseContext from "../../contexts/NewManagerExerciseContext";
+import MainAppBar from "../common/MainAppBar";
+import View from "../common/View";
+import { THEORY_URL } from "../../helpers/TheoryUrlGetter";
+import RAMFollowLineExerciseView from "../views/RAM/RAMFollowLineExerciseView";
+import CircuitSelector from "../buttons/RAM/RAMExerciseConfiguration";
+import { Footer } from "../common/RAM/Footer";
+
+function FollowPersonRAM() {
+  return (
+    <Box>
+      <ViewProvider>
+        <ExerciseProvider>
+          <MainAppBar
+            exerciseName={"Follow Person"}
+            specificConfiguration={<CircuitSelector></CircuitSelector>}
+          />
+          <View
+            url={THEORY_URL.FollowLine}
+            exerciseId={
+              <RAMFollowLineExerciseView context={NewManagerExerciseContext} />
+            }
+          />
+          <Footer></Footer>
+        </ExerciseProvider>
+      </ViewProvider>
+    </Box>
+  );
+}
+
+export default FollowPersonRAM;