Merge branch 'websocketcamera_new_lat_test' into humble-devel

compose_cfg/user_humble_cpu.yaml

@@ -24,6 +24,7 @@ services:
       - "7164:7164"
       - "7163:7163"
       - "6080:6080"
+      - "8765:8765"      
       - "1108:1108"
     tty: true
     stdin_open: true

database/exercises/db.sql

@@ -98,6 +98,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, templat
 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/
 14	laser_mapping	Laser Mapping	Build a map based on sensor readings	{"tags": "ROS2"}	ACTIVE	RoboticsAcademy/exercises/static/exercises/laser_mapping/python_template/
+15	color_filter_newmanager	Color Filter	Color Filter exercise using React and RAM	{"tags": "ROS2"}	ACTIVE	RoboticsAcademy/exercises/static/exercises/color_filter_newmanager/python_template/
 \.
 
 
@@ -135,6 +136,7 @@ COPY public.exercises_universes (id, exercise_id, universe_id) FROM stdin;
 27	12	27
 28	13	29
 29	14	30
+30	15	3
 \.
 
 

exercises/static/exercises/color_filter_newmanager/entry_point/exercise.py

@@ -0,0 +1,13 @@
+import os.path
+from typing import Callable
+
+from src.manager.libs.applications.compatibility.exercise_wrapper import CompatibilityExerciseWrapper
+
+
+class Exercise(CompatibilityExerciseWrapper):
+    def __init__(self, circuit: str, update_callback: Callable):
+        current_path = os.path.dirname(__file__)
+
+        super(Exercise, self).__init__(exercise_command=f"{current_path}/../python_template/ros1_noetic/exercise.py 0.0.0.0",
+                                       gui_command=f"{current_path}/../python_template/ros1_noetic/gui.py 0.0.0.0 {circuit}",
+                                       update_callback=update_callback)

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

@@ -0,0 +1,13 @@
+import os.path
+from typing import Callable
+
+from src.manager.libs.applications.compatibility.exercise_wrapper_ros2 import CompatibilityExerciseWrapperRos2
+
+
+class Exercise(CompatibilityExerciseWrapperRos2):
+    def __init__(self, circuit: str, update_callback: Callable):
+        current_path = os.path.dirname(__file__)
+
+        super(Exercise, self).__init__(exercise_command=f"{current_path}/../../python_template/ros2_humble/exercise.py 0.0.0.0",
+                                       gui_command=f"{current_path}/../../python_template/ros2_humble/gui.py 0.0.0.0 {circuit}",
+                                       update_callback=update_callback)

exercises/static/exercises/color_filter_newmanager/launch/ros1_noetic/gazebo.launch

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- world -->
+    <arg name="world" default="rescue_people.world"/>
+
+    <!-- gazebo configs -->
+    <arg name="gui" default="false"/>
+    <arg name="debug" default="false"/>
+    <arg name="verbose" default="true"/>
+    <arg name="paused" default="false"/>
+    <arg name="headless" default="true"/>
+    <arg name="use_sim_time" default="true"/>
+
+    <!-- Gazebo Sim -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="world_name" value="$(arg world)"/>
+        <arg name="debug" value="$(arg debug)"/>
+        <arg name="verbose" value="$(arg verbose)"/>
+        <arg name="paused" value="$(arg paused)"/>
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    </include>
+</launch>

exercises/static/exercises/color_filter_newmanager/launch/ros1_noetic/launch.py

@@ -0,0 +1,134 @@
+#!/usr/bin/env python3
+
+import stat
+import rospy
+import os
+from subprocess import Popen, PIPE
+
+
+# If DRI_NAME is not set by user, use card0
+DRI_PATH = os.path.join("/dev/dri", os.environ.get("DRI_NAME", "card0"))
+EXERCISE = "rescue_people_newmanager"
+TIMEOUT = 30
+MAX_ATTEMPT = 2
+
+
+# Check if acceleration can be enabled
+def check_device(device_path):
+    try:
+        return stat.S_ISCHR(os.lstat(device_path)[stat.ST_MODE])
+    except:
+        return False
+
+
+# Spawn new process
+def spawn_process(args, insert_vglrun=False):
+    if insert_vglrun:
+        args.insert(0, "vglrun")
+    process = Popen(args, stdout=PIPE, bufsize=1, universal_newlines=True)
+    return process
+
+
+class Test():
+    def gazebo(self):
+        rospy.logwarn("[GAZEBO] Launching")
+        try:
+            rospy.wait_for_service("/gazebo/get_model_properties", TIMEOUT)
+            return True
+        except rospy.ROSException:
+            return False
+
+    def px4(self):
+        rospy.logwarn("[PX4-SITL] Launching")
+        start_time = rospy.get_time()
+        args = ["./PX4-Autopilot/build/px4_sitl_default/bin/px4-commander",
+                "--instance", "0", "check"]
+        while rospy.get_time() - start_time < TIMEOUT:
+            process = spawn_process(args, insert_vglrun=False)
+            with process.stdout:
+                for line in iter(process.stdout.readline, ''):
+                    if ("Prearm check: OK" in line):
+                        return True
+            rospy.sleep(2)
+        return False
+
+    def mavros(self, ns=""):
+        rospy.logwarn("[MAVROS] Launching")
+        try:
+            rospy.wait_for_service(ns + "/mavros/cmd/arming", TIMEOUT)
+            return True
+        except rospy.ROSException:
+            return False
+
+
+class Launch():
+    def __init__(self):
+        self.test = Test()
+        self.acceleration_enabled = check_device(DRI_PATH)
+
+        # Start roscore
+        args = ["/opt/ros/noetic/bin/roscore"]
+        spawn_process(args, insert_vglrun=False)
+
+        rospy.init_node("launch", anonymous=True)
+
+    def start(self):
+        ######## LAUNCH GAZEBO ########
+        args = ["/opt/ros/noetic/bin/roslaunch",
+                "/RoboticsAcademy/exercises/static/exercises/" +
+                EXERCISE + "/launch/ros1_noetic/gazebo.launch",
+                "--wait",
+                "--log"
+                ]
+
+        attempt = 1
+        while True:
+            spawn_process(args, insert_vglrun=self.acceleration_enabled)
+            if self.test.gazebo() == True:
+                break
+            if attempt == MAX_ATTEMPT:
+                rospy.logerr("[GAZEBO] Launch Failed")
+                return
+            attempt = attempt + 1
+
+        ######## LAUNCH PX4 ########
+        args = ["/opt/ros/noetic/bin/roslaunch",
+                "/RoboticsAcademy/exercises/static/exercises/" +
+                EXERCISE + "/launch/ros1_noetic/px4.launch",
+                "--log"
+                ]
+
+        attempt = 1
+        while True:
+            spawn_process(args, insert_vglrun=self.acceleration_enabled)
+            if self.test.px4() == True:
+                break
+            if attempt == MAX_ATTEMPT:
+                rospy.logerr("[PX4] Launch Failed")
+                return
+            attempt = attempt + 1
+
+        ######## LAUNCH MAVROS ########
+        args = ["/opt/ros/noetic/bin/roslaunch",
+                "/RoboticsAcademy/exercises/static/exercises/" +
+                EXERCISE + "/launch/ros1_noetic/mavros.launch",
+                "--log"
+                ]
+
+        attempt = 1
+        while True:
+            spawn_process(args, insert_vglrun=self.acceleration_enabled)
+            if self.test.mavros() == True:
+                break
+            if attempt == MAX_ATTEMPT:
+                rospy.logerr("[MAVROS] Launch Failed")
+                return
+            attempt = attempt + 1
+
+
+if __name__ == "__main__":
+    launch = Launch()
+    launch.start()
+
+    with open("/drones_launch.log", "w") as f:
+        f.write("success")

exercises/static/exercises/color_filter_newmanager/launch/ros1_noetic/mavros.launch

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="ID" value="0"/>
+    <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
+
+    <!-- MAVROS -->
+    <include file="$(find mavros)/launch/px4.launch">
+        <arg name="fcu_url" value="$(arg fcu_url)"/>
+        <arg name="gcs_url" value=""/>
+        <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
+        <arg name="tgt_component" value="1"/>
+    </include>
+</launch>

exercises/static/exercises/color_filter_newmanager/launch/ros1_noetic/px4.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- vehicle model -->
+    <arg name="est" default="ekf2"/>
+    <arg name="vehicle" default="iris"/>
+
+    <!-- vehicle configs -->
+    <arg name="ID" value="0"/>
+    <!-- PX4 SITL and vehicle spawn -->
+    <include file="$(find drone_wrapper)/launch/single_vehicle_spawn_sdf.launch">
+        <arg name="z" value="1.45"/>
+        <arg name="Y" value="1.57"/>
+        <arg name="vehicle" value="$(arg vehicle)"/>
+        <arg name="model" value="iris_dual_cam"/>
+        <arg name="model_name" value="iris"/>
+        <arg name="mavlink_udp_port" value="14560"/>
+        <arg name="mavlink_tcp_port" value="4560"/>
+        <arg name="ID" value="$(arg ID)"/>
+    </include>
+</launch>

exercises/static/exercises/color_filter_newmanager/launch/ros1_noetic/rescue_people.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+    <arg name="world" default="$(eval env('PWD') + '/rescue_people.world')"/>
+    <arg name="solution_file_name" default="$(eval env('PWD') + '/my_solution.py')"/>
+    <arg name="perspective" default="$(find rqt_drone_teleop)/perspectives/drone_teleop_vel_cam.perspective"/>
+
+    <include file="$(find drone_wrapper)/launch/mavros_px4_sitl.launch">
+        <arg name="world" value="$(arg world)"/>
+        <arg name="z" value="1.45"/>
+        <arg name="Y" value="1.57"/>
+    </include>
+
+    <node name="rqt_gui" pkg="rqt_gui" type="rqt_gui" respawn="false" output="screen" args="--perspective-file $(arg perspective)"/>
+    <node name="my_solution" pkg="drone_wrapper" type="play_python_code" output="screen" args="$(arg solution_file_name)"/>
+</launch>