Comments + Indoor navigation

Author: reinzler

Indoor_navigation.pdf

@@ -1,37 +1,14 @@
-# solitude_test
-Solitude Aerial test case
+# Solitude Aerial test case
 
+Steps to reproduce:
+colcon build --packages-select px4_ros_com ros2_aruco px4_msgs && source install/local_setup.bash
+~/src/PX4-Autopilot make px4_sitl gazebo-classic
+MicroXRCEAgent udp4 -p 8888
 
-git clone https://github.com/PX4/PX4-Autopilot
-git clone https://github.com/PX4/px4_msgs
-git clone --recursive https://github.com/Auterion/px4-ros2-interface-lib
-
-pip3 install kconfiglib
-pip3 install symforce
-
-ros2 run example_mode_manual_cpp example_mode_manual
-make px4_sitl gz_x500
-~/addons/src/PX4-Autopilot make px4_sitl gazebo-classic
-
-micro-xrce-dds-agent udp4 -p 8888 / MicroXRCEAgent udp4 -p 8888
-ros2 run px4_ros_com offboard_control.py
+To test the drone's flight in a circle
 ros2 run px4_ros_com circle_flight.py
 
-./generate_markers_model.py -i /home/vadim/Downloads/gazebo_models/marker.png -s 2000 -w 1000
-PX4_SIM_WORLD=iris_irlock.world make px4_sitl gazebo-classic
-
-
-
-export PX4_HOME_LAT=59.92545296605414
-export PX4_HOME_LON=30.358049691155855
-export PX4_HOME_ALT=28.5
-
-
-ros2 run rqt_image_view rqt_image_view
+To test flight of a drone in a straight line and AAruco marker detection:
+ros2 run px4_ros_com straight_flight.py
 ros2 launch ros2_aruco aruco_recognition.launch.py
-colcon build --packages-select px4_ros_com ros2_aruco 
-source /home/vadim/solitude_test/install/local_setup.bash
 
-QGroundControl
-place QGroundControl AppImage to /usr/local/bin
-QGroundControl.AppImage

README.md

@@ -4,10 +4,10 @@
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
 from px4_msgs.msg import OffboardControlMode, TrajectorySetpoint, VehicleCommand, VehicleLocalPosition, VehicleStatus
-from colorama import Fore
 import math
 
-class OffboardControl(Node):
+
+class CircleFlight(Node):
     """Node for controlling a vehicle in offboard mode."""
 
     def __init__(self) -> None:
@@ -36,10 +36,10 @@ def __init__(self) -> None:
             VehicleStatus, '/fmu/out/vehicle_status', self.vehicle_status_callback, qos_profile)
 
         # Initialize variables
-        self.offboard_setpoint_counter = 0
+        self.offboard_setpoint_counter = 0  # Counter for offboard setpoint publishing
         self.vehicle_local_position = VehicleLocalPosition()
         self.vehicle_status = VehicleStatus()
-        self.takeoff_height = -15.0
+        self.takeoff_height = -15.0  # Desired takeoff height
         self.radius = 10.0  # Radius of the circle
         self.num_points = 36  # Number of points on the circle
         self.circle_points = self.generate_circle_points(self.radius, self.num_points)
@@ -132,25 +132,28 @@ def timer_callback(self) -> None:
         """Callback function for the timer."""
         self.publish_offboard_control_heartbeat_signal()
 
+        # After 10 iterations, engage offboard mode and arm the vehicle
         if self.offboard_setpoint_counter == 10:
             self.engage_offboard_mode()
             self.arm()
 
+        # If the vehicle has reached the takeoff height and is in offboard mode, start flying in a circle
         if self.vehicle_local_position.z > self.takeoff_height and self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD:
             x, y = self.circle_points[self.point_index]
             self.publish_position_setpoint(x, y, self.takeoff_height)
             self.point_index = (self.point_index + 1) % len(self.circle_points)
 
+        # Increment the offboard setpoint counter until it reaches 11
         if self.offboard_setpoint_counter < 11:
             self.offboard_setpoint_counter += 1
 
 
 def main(args=None) -> None:
-    print('Starting offboard control node...')
+    print("Starting offboard control node, drone is flying in a circle")
     rclpy.init(args=args)
-    offboard_control = OffboardControl()
-    rclpy.spin(offboard_control)
-    offboard_control.destroy_node()
+    circle_flight = CircleFlight()
+    rclpy.spin(circle_flight)
+    circle_flight.destroy_node()
     rclpy.shutdown()
 
 

src/px4_ros_com/src/examples/offboard_py/circle_flight.py

@@ -9,17 +9,15 @@
 import numpy as np
 from std_msgs.msg import String
 
-
 def generate_linear_trajectory():
-    """Generate points in a linear."""
+    """Generate points in a linear trajectory from 0.01 to 300.1 with a step of 0.01."""
     x = np.arange(0.01, 300.1, 0.01).tolist()
-    y = list(map(lambda y: y, x))
+    y = list(map(lambda y: y, x))  # This line is redundant since y will be the same as x
     points = list(zip(x, y))
-
     return points
 
 
-class OffboardControl(Node):
+class StraightFlightArucoDetection(Node):
     """Node for controlling a vehicle in offboard mode."""
 
     def __init__(self) -> None:
@@ -55,6 +53,7 @@ def __init__(self) -> None:
         self.takeoff_height = -2.6
         self.point_index = 0  # Index of the current point in the circle
         self.aruco_marker_detected = False
+
         # Create a timer to publish control commands
         self.timer = self.create_timer(0.1, self.timer_callback)
 
@@ -67,6 +66,7 @@ def vehicle_status_callback(self, vehicle_status):
         self.vehicle_status = vehicle_status
 
     def aruco_callback(self, msg):
+        """Callback function for aruco_detected topic subscriber."""
         if msg is not None:
             self.aruco_marker_detected = True
         else:
@@ -138,11 +138,14 @@ def timer_callback(self) -> None:
         """Callback function for the timer."""
         self.publish_offboard_control_heartbeat_signal()
 
+        # After 10 iterations, engage offboard mode and arm the vehicle
         if self.offboard_setpoint_counter == 10:
             self.engage_offboard_mode()
             self.arm()
 
+        # If the vehicle has reached the takeoff height and is in offboard mode, start flying in a circle
         if self.vehicle_local_position.z > self.takeoff_height and self.vehicle_status.nav_state == VehicleStatus.NAVIGATION_STATE_OFFBOARD:
+            # If an Aruco marker is detected, hover at the previous point and change altitude to 1m
             if self.aruco_marker_detected:
                 self.get_logger().info("Aruco marker detected, hovering...")
                 points = generate_linear_trajectory()
@@ -154,21 +157,22 @@ def timer_callback(self) -> None:
                 self.publish_position_setpoint(x, y, self.takeoff_height)
                 self.point_index += 1
 
+        # Increment the offboard setpoint counter until it reaches 11
         if self.offboard_setpoint_counter < 11:
             self.offboard_setpoint_counter += 1
 
 
 def main(args=None) -> None:
-    print('Starting offboard control node...')
+    print('Starting offboard control node, straight flight and aruco marker detection')
     rclpy.init(args=args)
-    offboard_control = OffboardControl()
-    rclpy.spin(offboard_control)
-    offboard_control.destroy_node()
+    straight_flight = StraightFlightArucoDetection()
+    rclpy.spin(straight_flight)
+    straight_flight.destroy_node()
     rclpy.shutdown()
 
 
 if __name__ == '__main__':
     try:
         main()
     except Exception as e:
-        print(e)
+        print(e)