sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Report is handed in

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-06-17 00:04:53 +02:00
parent 67df381251
commit 303a3f0dbd
7 changed files with 53 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/beacon_positioning/src/tracker_position.cpp
View File

@@ -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"

96
src/camera/camera/camera_controller.py
View File

@@ -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)

54
src/drone_controls/src/PositionChanger.cpp
View File

@@ -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;
}

4
src/drone_controls/test/test_positionchanger.py
View File

@@ -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(

2
src/drone_controls/test/test_positionchanger_lidar.py
View File

@@ -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

4
src/px4_connection/src/px4_controller.cpp
View File

@@ -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

1
src/px4_connection/test/test_failsafe_enabled.py
View File

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