[drones] multiprocessing prototypes

exercises/static/exercises/drone_cat_mouse/web-template/drone_cat_mouse.world

@@ -1,81 +1,53 @@
 <?xml version="1.0" ?>
-<?xml-model href="http://sdformat.org/schemas/root.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <sdf version="1.5">
-  <world name="default">
-
-    <gui fullscreen="1"></gui>
-
-    <include>
-  	  <uri>model://sun</uri>
-	  </include>
-
-    <!-- Ground -->
-	  <include>
-  		<uri>model://grass_plane</uri>
-	  </include>
-
-    <!-- JdeRobot Logo -->
-    <include>
-		  <name>logo</name>
+	<world name="default">
+		<gui fullscreen="1"></gui>
+		<include>
+			<uri>model://sun</uri>
+		</include>
+
+		<include>
+			<uri>model://grass_plane</uri>
+		</include>
+
+		<include>
+			<name>logo</name>
 			<uri>model://logoJdeRobot</uri>
 			<pose>0 -4 0 0 0 0</pose>
-	  </include>
-
-		<!-- Sky -->
-	  <scene>
-        <shadows>0</shadows>
-        <ambient>0.4 0.4 0.4 1.0</ambient>
-        <background>0.7 0.7 0.7 1</background>
-	    <sky>
-	        <clouds>
-	            <speed>12</speed>
-	        </clouds>
-	    </sky>
-	  </scene>
-
-
-    <!-- Only one ROS interface plugin is required per world, as any other plugin can connect a Gazebo
-         topic to a ROS topic (or vise versa). -->
-    <plugin name="ros_interface_plugin" filename="librotors_gazebo_ros_interface_plugin.so"/>
-
-
-    <!-- coordinate system -->
-    <spherical_coordinates>
-      <surface_model>EARTH_WGS84</surface_model>
-      <latitude_deg>47.3667</latitude_deg>
-      <longitude_deg>8.5500</longitude_deg>
-      <elevation>500.0</elevation>
-      <heading_deg>0</heading_deg>
-    </spherical_coordinates>
-
-    <!-- physics engine -->
-    <physics name="default_physics" default="0"  type='ode'>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <iters>1000</iters>
-          <sor>1.3</sor>
-        </solver>
-        <constraints>
-          <cfm>0</cfm>
-          <erp>0.2</erp>
-          <contact_max_correcting_vel>100</contact_max_correcting_vel>
-          <contact_surface_layer>0.001</contact_surface_layer>
-        </constraints>
-      </ode>
-      <max_step_size>0.004</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <real_time_update_rate>250</real_time_update_rate>
-      <magnetic_field>6.0e-06 2.3e-05 -4.2e-05</magnetic_field>
-      <gravity>0 0 -9.8</gravity>
-    </physics>
-
-    <!-- turn off shadows -->
-    <scene>
-      <shadows>false</shadows>
-      <sky>
-        <clouds/>
-      </sky>
-    </scene>
-  </world>
-</sdf>
+		</include>
+
+		<scene>
+            <shadows>0</shadows>
+            <ambient>0.4 0.4 0.4 1.0</ambient>
+            <background>0.7 0.7 0.7 1</background>
+			<sky>
+				<clouds>
+					<speed>12</speed>
+				</clouds>
+			</sky>
+		</scene>
+
+		<!-- Gazebo required specifications -->
+		<physics name='default_physics' default='0' type='ode'>
+			<gravity>0 0 -9.8066</gravity>
+			<ode>
+				<solver>
+					<type>quick</type>
+					<iters>10</iters>
+					<sor>1.3</sor>
+					<use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
+				</solver>
+				<constraints>
+					<cfm>0</cfm>
+					<erp>0.2</erp>
+					<contact_max_correcting_vel>100</contact_max_correcting_vel>
+					<contact_surface_layer>0.001</contact_surface_layer>
+				</constraints>
+			</ode>
+			<max_step_size>0.004</max_step_size>
+			<real_time_factor>1</real_time_factor>
+			<real_time_update_rate>250</real_time_update_rate>
+			<magnetic_field>6.0e-6 2.3e-5 -4.2e-5</magnetic_field>
+		</physics>
+	</world>
+</sdf>

exercises/static/exercises/drone_cat_mouse/web-template/gui.py

@@ -274,4 +274,4 @@ def run(self):
             dt = finish_time - start_time
             ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0
             if ms < self.ideal_cycle:
-                time.sleep((self.ideal_cycle-ms) / 1000.0)
+                time.sleep((self.ideal_cycle-ms) / 1000.0)

exercises/static/exercises/drone_cat_mouse/web-template/gui_guest.py

@@ -274,4 +274,4 @@ def run(self):
             dt = finish_time - start_time
             ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0
             if ms < self.ideal_cycle:
-                time.sleep((self.ideal_cycle-ms) / 1000.0)
+                time.sleep((self.ideal_cycle-ms) / 1000.0)

exercises/static/exercises/drone_cat_mouse/web-template/hal.py

@@ -78,4 +78,4 @@ def takeoff(self, h=3):
         self.cat.takeoff(h)
 
     def land(self):
-        self.cat.land()
+        self.cat.land()

exercises/static/exercises/drone_cat_mouse/web-template/hal_guest.py

@@ -78,4 +78,4 @@ def takeoff(self, h=3):
         self.mouse.takeoff(h)
 
     def land(self):
-        self.mouse.land()
+        self.mouse.land()

exercises/static/exercises/drone_cat_mouse/web-template/launch/gazebo.launch

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<launch>
+    <!-- world -->
+    <arg name="world" default="/RoboticsAcademy/exercises/drone_cat_mouse/web-template/drone_cat_mouse.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>
+
+    <rosparam command="load" file="/RoboticsAcademy/exercises/drone_cat_mouse/web-template/drone_cat_mouse.yaml" />
+</launch>

exercises/static/exercises/drone_cat_mouse/web-template/launch/launch.py

@@ -0,0 +1,164 @@
+#!/usr/bin/env python3
+
+import stat
+import rospy
+from os import lstat
+from subprocess import Popen, PIPE
+
+
+DRI_PATH = "/dev/dri/card0"
+EXERCISE = "drone_cat_mouse"
+TIMEOUT = 30
+MAX_ATTEMPT = 2
+
+
+# Check if acceleration can be enabled
+def check_device(device_path):
+    try:
+        return stat.S_ISCHR(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, instance):
+        rospy.logwarn("[PX4-SITL] Launching")
+        start_time = rospy.get_time()
+        args = ["./PX4-Autopilot/build/px4_sitl_default/bin/px4-commander","--instance", str(instance), "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/" + EXERCISE + "/web-template/launch/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_CAT (INSTANCE 0) ######## 
+        args = ["/opt/ros/noetic/bin/roslaunch", 
+                "/RoboticsAcademy/exercises/" + EXERCISE + "/web-template/launch/px4_cat.launch", 
+                "--log"
+                ]
+
+        attempt = 1
+        while True:
+            spawn_process(args, insert_vglrun=self.acceleration_enabled)
+            if self.test.px4("0") == True:
+                break
+            if attempt == MAX_ATTEMPT:
+                rospy.logerr("[PX4_CAT] Launch Failed")
+                return
+            attempt = attempt + 1
+
+
+        ######## LAUNCH MAVROS_CAT ########
+        args = ["/opt/ros/noetic/bin/roslaunch", 
+                "/RoboticsAcademy/exercises/" + EXERCISE + "/web-template/launch/mavros_cat.launch", 
+                "--log"
+                ]
+
+        attempt = 1
+        while True:
+            spawn_process(args, insert_vglrun=self.acceleration_enabled)
+            if self.test.mavros("/cat") == True:
+                break
+            if attempt == MAX_ATTEMPT:
+                rospy.logerr("[MAVROS_CAT] Launch Failed")
+                return
+            attempt = attempt + 1
+
+        ######## LAUNCH PX4_MOUSE  (INSTANCE 1) ########
+        args = ["/opt/ros/noetic/bin/roslaunch", 
+                "/RoboticsAcademy/exercises/" + EXERCISE + "/web-template/launch/px4_mouse.launch", 
+                "--log"
+                ]
+
+        attempt = 1
+        while True:
+            spawn_process(args, insert_vglrun=self.acceleration_enabled)
+            if self.test.px4("1") == True:
+                break
+            if attempt == MAX_ATTEMPT:
+                rospy.logerr("[PX4_MOUSE] Launch Failed")
+                return
+            attempt = attempt + 1
+
+
+        ######## LAUNCH MAVROS_MOUSE ########
+        args = ["/opt/ros/noetic/bin/roslaunch", 
+                "/RoboticsAcademy/exercises/" + EXERCISE + "/web-template/launch/mavros_mouse.launch", 
+                "--log"
+                ]
+
+        attempt = 1
+        while True:
+            spawn_process(args, insert_vglrun=self.acceleration_enabled)
+            if self.test.mavros("/mouse") == True:
+                break
+            if attempt == MAX_ATTEMPT:
+                rospy.logerr("[MAVROS_MOUSE] 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/drone_cat_mouse/web-template/launch/mavros_cat.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+    <group ns="cat">
+        <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>
+    </group>
+</launch>

exercises/static/exercises/drone_cat_mouse/web-template/launch/mavros_mouse.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+    <group ns="mouse">
+        <arg name="ID" value="1"/>
+        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
+        <!-- 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>
+    </group>
+</launch>