Report is handed in

src/beacon_positioning/src/tracker_position.cpp

@@ -4,10 +4,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "beacon_positioning/msg/tracker_position.hpp"
-
 #include "rtls_driver/rtls_driver.hpp"
-
-// from the example: https://github.com/Terabee/positioning_systems_api/blob/master/examples/rtls_tracker_example.cpp
 #include "serial_communication/serial.hpp"
 
 #define TRACKER_0_PORT "/dev/ttyUSB0"

src/camera/camera/camera_controller.py

@@ -16,14 +16,13 @@ import requests
 RES_4K_H = 3496
 RES_4K_W = 4656
 
-video_url = "http://10.1.1.41:8080/video"
-# Set the headers for the POST request
-headers = {'Content-Type': 'application/octet-stream'}
-
-#TODO change to serve video stream through websockets connection
-
 class CameraController(Node):
     def __init__(self):
+        """Initialize the camera controller node.
+
+        Initializes the service and the video stream for the websocket connection.
+        
+        """
         super().__init__('camera_controller')
 
         self.get_logger().info("Camera controller started. Waiting for service call...")
@@ -37,21 +36,23 @@ class CameraController(Node):
 
         self.timer = self.create_timer(1, self.timer_callback)
 
-        # self.websocket_thread = threading.Thread(target=self.start_listening)
-        # self.websocket_thread.start()
-
         self.video_thread = threading.Thread(target=self.setup_websocket)
         self.video_thread.start()
 
 
 
     def timer_callback(self):
+        """Callback function for shutting down if an error occurred from a different thread."""
         if self.should_exit:
             self.get_logger().info("Shutting down...")
             self.destroy_node()
             sys.exit(-1)
 
     def take_picture_callback(self, request, response):
+        """Callback function for the service call to the /drone/picture service.
+            takes a picture with the given filename and saves it to the drone_img folder.
+            Sends the filename back to the caller.
+        """
         if (request.input_name == "default"):
             self.get_logger().info("Taking picture with default filename")
             now = datetime.now().strftime("droneimage_%Y-%m-%d_%H-%M-%S")
@@ -65,6 +66,7 @@ class CameraController(Node):
         return response
 
     def setup_websocket(self):
+        """Sets up the websocket connection for the video stream on port 9002."""
         loop = asyncio.new_event_loop()
         connected = False
         while not connected:
@@ -83,6 +85,9 @@ class CameraController(Node):
             sys.exit(-1)
     
     async def websocket_video(self,websocket,path):
+        """Function for the websocket connection on port 9002.
+        This continuously captures frames from the video and sends them to the websocket client.
+        A resolution of 1920x1080 is chosen for performance reasons."""
         vid = cv2.VideoCapture(0,cv2.CAP_V4L)
 
         vid.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
@@ -92,12 +97,8 @@ class CameraController(Node):
             try:
                 while(vid.isOpened()):
                     img, frame = vid.read()
-                    # self.get_logger().info("frame before: " + str(frame.shape))
-                    #frame = cv2.resize(frame,(RES_4K_W,RES_4K_H))
-                    #print("frame after: " + str(frame.shape))
                     encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 100]
                     man = cv2.imencode('.jpg', frame)[1]
-                    #sender(man)
                     await websocket.send(man.tobytes())
                 self.get_logger().error("Not opened")
                 error_amount += 1
@@ -109,75 +110,6 @@ class CameraController(Node):
                 self.should_exit = True
                 sys.exit(-1)
 
-
-    def handle_video_connection(self):
-        self.get_logger().info('Starting sending video')
-        vid = cv2.VideoCapture(0, cv2.CAP_DSHOW)
-
-        vid.set(cv2.CAP_PROP_FRAME_WIDTH, RES_4K_W)
-        vid.set(cv2.CAP_PROP_FRAME_HEIGHT, RES_4K_H)
-        while True:
-            try:
-                while vid.isOpened():
-                    pass
-                    ret, frame = vid.read()
-
-                    if not ret:
-                        # If reading the frame failed, break the loop
-                        break
-
-                    # Convert the frame to bytes
-                    _, img_encoded = cv2.imencode('.jpg', frame)
-                    frame_data = img_encoded.tobytes()
-
-                    # Send the frame data as the request body
-                    response = requests.post(video_url, data=frame_data, headers=headers)
-
-                    # Check the response status
-                    if response.status_code == 200:
-                        print('Frame sent successfully.')
-                    else:
-                        print('Failed to send frame.')
-                    # if self.websocket is not None:
-                    #     img,frame = vid.read()
-                    #     frame = cv2.resize(frame,(640,480))
-                    #     encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 65]
-                    #     man = cv2.imencode('.jpg', frame, encode_param)[1]
-                    #     self.get_logger().info('Sending video')
-                    #     asyncio.ensure_future(self.websocket.send(man.tobytes()),loop=self.event_loop)
-                    #     await asyncio.sleep(1)
-                    # else:
-                    #     self.get_logger().info('No websocket connection')
-
-            except Exception as e:
-                self.get_logger().error('Something went wrong while reading and sending video: ' + str(e))
-
-    def start_listening(self):
-        self.get_logger().info('Starting listening for websocket connections')
-        asyncio.run(self.start_websocket_server())
-
-    async def start_websocket_server(self):
-        self.get_logger().info('Starting websocket server for video')
-        self.event_loop = asyncio.get_event_loop()
-        self.server = await websockets.server.serve(self.websocket_handler, '0.0.0.0', 9002)
-        await self.server.wait_closed()
-    
-    async def websocket_handler(self,websocket):
-        self.get_logger().info('New connection')
-
-        self.websocket = websocket
-        try:
-            async for message in websocket:
-                self.get_logger().info(f"Received message: {message}")
-
-        except websockets.exceptions.ConnectionClosed:
-            self.get_logger().info('Connection closed')
-            self.websocket = None
-        except Exception as e:
-            self.get_logger().error('Something went wrong!')
-            self.get_logger().error(str(e))
-            self.websocket = None
-
        
 def main(args=None):
     rclpy.init(args=args)

src/drone_controls/src/PositionChanger.cpp

@@ -53,7 +53,6 @@ public:
 
         this->lidar_subscription = this->create_subscription<drone_services::msg::LidarReading>("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1));
         this->height_subscription = this->create_subscription<drone_services::msg::HeightData>("/drone/height", qos, std::bind(&PositionChanger::handle_height_message, this, std::placeholders::_1));
-        // vehicle_local_position_subscription_ = this->create_subscription<px4_msgs::msg::VehicleLocalPosition>("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1));
         this->odometry_subscription = this->create_subscription<px4_msgs::msg::VehicleOdometry>("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1));
         this->move_position_service = this->create_service<drone_services::srv::MovePosition>("/drone/move_position", std::bind(&PositionChanger::handle_move_position_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
         this->ready_drone_service = this->create_service<drone_services::srv::ReadyDrone>("/drone/ready", std::bind(&PositionChanger::handle_ready_drone_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
@@ -113,6 +112,11 @@ public:
         }
     }
 
+/**
+ * @brief callback function for the /drone/arm service
+ * 
+ * @param future the future object that holds the result of the service call
+ */
     void arm_drone_service_callback(rclcpp::Client<drone_services::srv::ArmDrone>::SharedFuture future)
     {
         auto status = future.wait_for(1s);
@@ -137,6 +141,11 @@ public:
         }
     }
 
+/**
+ * @brief callback function for the /drone/set_attitude service
+ * 
+ * @param future the future object that holds the result of the service call
+ */
     void attitude_message_callback(rclcpp::Client<drone_services::srv::SetAttitude>::SharedFuture future)
     {
         auto status = future.wait_for(1s);
@@ -220,39 +229,26 @@ public:
      */
     void apply_collision_weights()
     {
-
-        // if (this->current_speed_x > 0) // if moving forward
-        // {
         if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT])
         {
             RCLCPP_INFO(this->get_logger(), "Collision prevention front: %f", collision_prevention_weights[MOVE_DIRECTION_FRONT]);
             get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_FRONT], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
         }
-        // }
-        // else // moving backward
-        // {
         if (!this->move_direction_allowed[MOVE_DIRECTION_BACK])
         {
             RCLCPP_INFO(this->get_logger(), "Collision prevention back: %f", collision_prevention_weights[MOVE_DIRECTION_BACK]);
             get_x_y_with_rotation(collision_prevention_weights[MOVE_DIRECTION_BACK], 0, this->current_yaw, &this->current_speed_x, &this->current_speed_y);
         }
-        // }
-        // if (this->current_speed_y > 0) // moving right
-        // {
         if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT])
         {
             RCLCPP_INFO(this->get_logger(), "Collision prevention right: %f", collision_prevention_weights[MOVE_DIRECTION_RIGHT]);
             get_x_y_with_rotation(0, collision_prevention_weights[MOVE_DIRECTION_RIGHT], this->current_yaw, &this->current_speed_x, &this->current_speed_y);
         }
-        // }
-        // else // moving left
-        // {
         if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT])
         {
             RCLCPP_INFO(this->get_logger(), "Collision prevention left: %f", collision_prevention_weights[MOVE_DIRECTION_LEFT]);
             get_x_y_with_rotation(0, collision_prevention_weights[MOVE_DIRECTION_LEFT], this->current_yaw, &this->current_speed_x, &this->current_speed_y);
         }
-        // }
     }
 
     /**
@@ -278,6 +274,11 @@ public:
         apply_collision_weights();
     }
 
+    /**
+     * @brief Callback for when a set_attitude request is made while landing
+     *
+     * @param future the future object containing the result of the request.
+     */
     void attitude_land_callback(rclcpp::Client<drone_services::srv::SetAttitude>::SharedFuture future)
     {
         auto status = future.wait_for(1s);
@@ -295,6 +296,11 @@ public:
         }
     }
 
+    /**
+     * @brief Callback for when a set_vehicle_control request is made while landing
+     *
+     * @param future the future object containing the result of the request.
+     */
     void vehicle_control_land_request_callback(rclcpp::Client<drone_services::srv::SetVehicleControl>::SharedFuture future)
     {
         auto status = future.wait_for(1s);
@@ -316,6 +322,11 @@ public:
         }
     }
 
+    /**
+     * @brief Callback method for receiving new height data
+     *
+     * @param message the message containing the height data
+     */
     void handle_height_message(const drone_services::msg::HeightData::SharedPtr message)
     {
         this->current_height = message->min_height;
@@ -445,6 +456,13 @@ public:
         }
     }
 
+    /**
+     * @brief Callback function for when the /drone/land service is called
+     *
+     * @param request_header the header of the request
+     * @param request the request to land
+     * @param response the response to send back.
+     */
     void handle_land_request(
         const std::shared_ptr<rmw_request_id_t> request_header,
         const std::shared_ptr<drone_services::srv::Land::Request> request,
@@ -454,6 +472,13 @@ public:
         response->is_landing = this->is_landing;
     }
 
+    /**
+     * @brief Callback function for when the /drone/ready_drone service is called
+     *
+     * @param request_header the header of the request
+     * @param request the request to ready the drone
+     * @param response the response to send back. Contains true if the request was successful, false otherwise
+     */
     void handle_ready_drone_request(
         const std::shared_ptr<rmw_request_id_t> request_header,
         const std::shared_ptr<drone_services::srv::ReadyDrone::Request> request,
@@ -474,7 +499,6 @@ public:
         this->vehicle_control_request->control = CONTROL_MODE_ATTITUDE;
         auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request,
                                                                                       std::bind(&PositionChanger::vehicle_control_service_callback, this, std::placeholders::_1));
-        // TODO set motors to spin at 30%
         response->success = true;
     }
 

src/drone_controls/test/test_positionchanger.py

@@ -1,5 +1,3 @@
-import os
-import sys
 import unittest
 
 import launch
@@ -17,8 +15,6 @@ from drone_services.msg import LidarReading
 
 @pytest.mark.rostest
 def generate_test_description():
-    file_path = os.path.dirname(__file__)
-    # device under test
     positionchanger_node = launch_ros.actions.Node(
         package='drone_controls', executable='position_changer')
     failsafe_node = launch_ros.actions.Node(

src/drone_controls/test/test_positionchanger_lidar.py

@@ -1,5 +1,4 @@
 import os
-import sys
 import unittest
 
 import launch
@@ -11,7 +10,6 @@ import rclpy
 import time
 
 from drone_services.srv import MovePosition
-from drone_services.msg import FailsafeMsg
 from drone_services.msg import LidarReading
 
 @pytest.mark.rostest

src/px4_connection/src/px4_controller.cpp

@@ -396,9 +396,6 @@ private:
             send_velocity_setpoint();
             return;
         }
-        // the spiral, in polar coordinates (theta, rho), is given by
-        // theta = theta_0 + omega*t
-        // rho = rho_0 + K*theta
         float theta = theta_0 + omega * 0.1;
         float rho = rho_0 + K * theta;
 
@@ -409,7 +406,6 @@ private:
 
         if (!user_in_control)
         {
-            // RCLCPP_INFO(this->get_logger(), "Sending idle attitude setpoint");
             send_attitude_setpoint();
         }
         else

src/px4_connection/test/test_failsafe_enabled.py

@@ -1,5 +1,4 @@
 import os
-import sys
 import unittest
 
 import launch