From 303a3f0dbd795067fcb1f5e1ff106bb453121e85 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Sat, 17 Jun 2023 00:04:53 +0200 Subject: [PATCH] Report is handed in --- .../src/tracker_position.cpp | 3 - src/camera/camera/camera_controller.py | 96 +++---------------- src/drone_controls/src/PositionChanger.cpp | 54 ++++++++--- .../test/test_positionchanger.py | 4 - .../test/test_positionchanger_lidar.py | 2 - src/px4_connection/src/px4_controller.cpp | 4 - .../test/test_failsafe_enabled.py | 1 - 7 files changed, 53 insertions(+), 111 deletions(-) diff --git a/src/beacon_positioning/src/tracker_position.cpp b/src/beacon_positioning/src/tracker_position.cpp index b87c63c3..8437717c 100644 --- a/src/beacon_positioning/src/tracker_position.cpp +++ b/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" diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py index 7f8a5d9f..5fdbc739 100644 --- a/src/camera/camera/camera_controller.py +++ b/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) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 93a1612d..ceca658a 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -53,7 +53,6 @@ public: this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this, std::placeholders::_1)); this->height_subscription = this->create_subscription("/drone/height", qos, std::bind(&PositionChanger::handle_height_message, this, std::placeholders::_1)); - // vehicle_local_position_subscription_ = this->create_subscription("/fmu/out/vehicle_local_position", qos, std::bind(&PX4Controller::on_local_position_receive, this, std::placeholders::_1)); this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); this->move_position_service = this->create_service("/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/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::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::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::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::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 request_header, const std::shared_ptr 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 request_header, const std::shared_ptr 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; } diff --git a/src/drone_controls/test/test_positionchanger.py b/src/drone_controls/test/test_positionchanger.py index 0bbab7d4..1e3bb4de 100644 --- a/src/drone_controls/test/test_positionchanger.py +++ b/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( diff --git a/src/drone_controls/test/test_positionchanger_lidar.py b/src/drone_controls/test/test_positionchanger_lidar.py index 5d2f70c8..8bd5b574 100644 --- a/src/drone_controls/test/test_positionchanger_lidar.py +++ b/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 diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 7c0d8930..f9ecf48c 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/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 diff --git a/src/px4_connection/test/test_failsafe_enabled.py b/src/px4_connection/test/test_failsafe_enabled.py index 11aeca3a..4f6a8b33 100644 --- a/src/px4_connection/test/test_failsafe_enabled.py +++ b/src/px4_connection/test/test_failsafe_enabled.py @@ -1,5 +1,4 @@ import os -import sys import unittest import launch