diff --git a/.vscode/settings.json b/.vscode/settings.json index 8dc586dd..af3183fa 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -60,7 +60,21 @@ "typeindex": "cpp", "typeinfo": "cpp", "utility": "cpp", - "variant": "cpp" + "variant": "cpp", + "*.srv": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "strstream": "cpp", + "bitset": "cpp", + "complex": "cpp", + "forward_list": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "random": "cpp", + "fstream": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp" }, "python.autoComplete.extraPaths": [ "/home/ubuntu/ros2_ws/install/px4_msgs/lib/python3.8/site-packages", diff --git a/src/beacon_positioning/src/tracker_position.cpp b/src/beacon_positioning/src/tracker_position.cpp index 0b4d9f52..b87c63c3 100644 --- a/src/beacon_positioning/src/tracker_position.cpp +++ b/src/beacon_positioning/src/tracker_position.cpp @@ -19,10 +19,10 @@ using SerialInterface = terabee::serial_communication::Serial; using namespace std::chrono_literals; -class BeaconPositioningPublisher : public rclcpp::Node +class TrackerPositioning : public rclcpp::Node { public: - BeaconPositioningPublisher() : Node("beacon_positioning") + TrackerPositioning() : Node("beacon_positioning") { this->declare_parameter("tracker_serial_port", "/dev/ttyUSB0"); @@ -31,7 +31,7 @@ public: RCLCPP_INFO(this->get_logger(), "serial port is %s\n", serial_port_name.c_str()); - serial_port = std::make_shared("/dev/ttyUSB0"); + serial_port = std::make_shared(serial_port_name); serial_port->setBaudrate(115200); serial_port->setTimeout(800ms); @@ -47,8 +47,6 @@ public: } publisher_ = this->create_publisher("beacon_positioning", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&BeaconPositioningPublisher::timer_callback, this)); } void setup_rtlsdevice(terabee::RtlsDevice *rtls_device, int priority, int label, int update_time, int network_id, bool long_message) @@ -99,16 +97,6 @@ public: } private: - void timer_callback() - - { - // auto message = std_msgs::msg::String(); - // message.data = "Hello beacons!"; - // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - // publisher_->publish(message); - } - - rclcpp::TimerBase::SharedPtr timer_; // timer to trigger the rclcpp::Publisher::SharedPtr publisher_; // pointer to publisher object int tracker_id; @@ -117,7 +105,6 @@ private: terabee::RtlsDevice::config_t device_configuration; terabee::RtlsDevice::OnTrackerDataCallback tracker_data_callback_; terabee::RtlsDevice::tracker_msg_t tracker_msg; - //terabee::RtlsDevice rtls_device; }; int main(int argc, char **argv) @@ -129,7 +116,7 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); rclcpp::executors::MultiThreadedExecutor executor; - std::shared_ptr node = std::make_shared(); + std::shared_ptr node = std::make_shared(); executor.add_node(node); terabee::RtlsDevice rtls_device(node->get_serial_port()); if (node->get_parameter("tracker_serial_port").as_string().compare(TRACKER_0_PORT)) @@ -170,7 +157,6 @@ int main(int argc, char **argv) else { RCLCPP_ERROR(node->get_logger(), "invalid tracker position"); - // RCLCPP_INFO(node->get_logger(), "x = %f, y = %f, z = %f", tracker_msg.tracker_position_xyz[0], tracker_msg.tracker_position_xyz[1], tracker_msg.tracker_position_xyz[2]); } }); rtls_device.startReadingStream(); diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py index e589c9e9..9fe9f6c6 100644 --- a/src/camera/camera/camera_controller.py +++ b/src/camera/camera/camera_controller.py @@ -1,16 +1,13 @@ import rclpy from rclpy.node import Node - from drone_services.srv import TakePicture import os from datetime import datetime -# import cv2 - +#resolution of the camera RES_4K_H = 3496 RES_4K_W = 4656 - class CameraController(Node): def __init__(self): super().__init__('camera_controller') @@ -32,19 +29,14 @@ class CameraController(Node): return response - def main(args=None): rclpy.init(args=args) test_controller = CameraController() rclpy.spin(test_controller) - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) test_controller.destroy_node() rclpy.shutdown() - if __name__ == '__main__': main() diff --git a/src/drone_controls/CMakeLists.txt b/src/drone_controls/CMakeLists.txt index cc182294..4eb03b6f 100644 --- a/src/drone_controls/CMakeLists.txt +++ b/src/drone_controls/CMakeLists.txt @@ -1,32 +1,38 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(drone_controls) -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) +find_package(drone_services REQUIRED) + +add_executable(position_changer src/PositionChanger.cpp) +ament_target_dependencies(position_changer rclcpp px4_msgs drone_services) + +target_include_directories(position_changer PUBLIC + $ + $) + +install(TARGETS position_changer + DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/src/drone_controls/package.xml b/src/drone_controls/package.xml index 5d264637..89d4dbf4 100644 --- a/src/drone_controls/package.xml +++ b/src/drone_controls/package.xml @@ -3,14 +3,17 @@ drone_controls 0.0.0 - TODO: Package description - ubuntu - TODO: License declaration + Package to handle control commands from the api to the PX4Controller + sem + Apache Licence 2.0 + rclcpp + px4_msgs + drone_services + object_detection + height ament_cmake - rclcpp - ament_lint_auto ament_lint_common diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp new file mode 100644 index 00000000..a540b2a7 --- /dev/null +++ b/src/drone_controls/src/PositionChanger.cpp @@ -0,0 +1,292 @@ +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include +#include + +#define _USE_MATH_DEFINES +#include + +#define LIDAR_SENSOR_FR 0 // front right +#define LIDAR_SENSOR_FL 1 // front left +#define LIDAR_SENSOR_RL 2 // rear left +#define LIDAR_SENSOR_RR 3 // rear right + +#define MOVE_DIRECTION_FRONT 0 // front right +#define MOVE_DIRECTION_LEFT 1 // front left +#define MOVE_DIRECTION_BACK 2 // rear left +#define MOVE_DIRECTION_RIGHT 3 // rear right + +#define MIN_DISTANCE 1.0 // in meters + +#define DEFAULT_CONTROL_MODE 16 // default velocity control bitmask + +// converts bitmask control mode to control mode used by PX4Controller +#define PX4_CONTROLLER_CONTROL_MODE(x) ((x) == 4 ? 1 : ((x) == 16 ? 2 : ((x) == 32 ? 3 : -1))) + +using namespace std::chrono_literals; + +struct Quaternion +{ + float w, x, y, z; +}; + +class PositionChanger : public rclcpp::Node +{ +public: + PositionChanger() : Node("position_changer") + { + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); + + this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_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->trajectory_client = this->create_client("/drone/set_trajectory"); + this->vehicle_control_client = this->create_client("/drone/set_vehicle_control"); + + RCLCPP_INFO(this->get_logger(), "waiting for trajectory service..."); + wait_for_service::SharedPtr>(this->trajectory_client); + RCLCPP_INFO(this->get_logger(), "waiting for vehicle control service..."); + wait_for_service::SharedPtr>(this->vehicle_control_client); + + this->trajectory_request = std::make_shared(); + this->vehicle_control_request = std::make_shared(); + + this->vehicle_control_request->control = DEFAULT_CONTROL_MODE; + 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)); + } + + void vehicle_control_service_callback(rclcpp::Client::SharedFuture future) + { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) + { + RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", future.get()->success); + } + else + { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } + + void trajectory_message_callback(rclcpp::Client::SharedFuture future) + { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) + { + RCLCPP_INFO(this->get_logger(), "Seet trajectory set result: %d", future.get()->success); + } + else + { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } + void send_trajectory_message() + { + check_move_direction_allowed(); + this->trajectory_request->values[0] = this->current_speed_x; + this->trajectory_request->values[1] = this->current_speed_y; + this->trajectory_request->values[2] = this->current_speed_z; + this->trajectory_request->yaw = this->current_yaw; + this->trajectory_request->control_mode = PX4_CONTROLLER_CONTROL_MODE(DEFAULT_CONTROL_MODE); + RCLCPP_INFO(this->get_logger(), "Sending trajectory message\nx: %f\ny: %f\nz: %f\nyaw: %f", this->current_speed_x, this->current_speed_y, this->current_speed_z, this->current_yaw); + auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); + } + + /** + * @brief applies the collision prevention weights to the current x and y speed if the drone is too close to an object. + * It moves the drone away from the object until it is far enough away + * + */ + void apply_collision_weights() + { + if (this->current_speed_x > 0) // if moving forward + { + if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) + { + this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_FRONT]; + } + } + else // moving backward + { + if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) + { + this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_BACK]; + } + } + if (this->current_speed_y > 0) // moving right + { + if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) + { + this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT]; + } + + } + else // moving left + { + if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) + { + this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT]; + } + } + } + + /** + * @brief checks for every direction is an object is too close and if we can move in that direction. + * If the object is too close to the drone, calculate the amount we need to move away from it + */ + void check_move_direction_allowed() + { + this->move_direction_allowed[MOVE_DIRECTION_FRONT] = this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE; + this->move_direction_allowed[MOVE_DIRECTION_LEFT] = this->lidar_sensor_values[LIDAR_SENSOR_FL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE; + this->move_direction_allowed[MOVE_DIRECTION_BACK] = this->lidar_sensor_values[LIDAR_SENSOR_RL] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE; + this->move_direction_allowed[MOVE_DIRECTION_RIGHT] = this->lidar_sensor_values[LIDAR_SENSOR_RR] > MIN_DISTANCE && this->lidar_sensor_values[LIDAR_SENSOR_FR] > MIN_DISTANCE; + + // calculate the amount we need to move away from the object to be at the minimum distance + collision_prevention_weights[MOVE_DIRECTION_FRONT] = this->move_direction_allowed[MOVE_DIRECTION_FRONT] ? 0 + : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FR], this->lidar_sensor_values[LIDAR_SENSOR_FL])); + collision_prevention_weights[MOVE_DIRECTION_LEFT] = this->move_direction_allowed[MOVE_DIRECTION_LEFT] ? 0 + : (MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FL], this->lidar_sensor_values[LIDAR_SENSOR_RL])); + collision_prevention_weights[MOVE_DIRECTION_BACK] = this->move_direction_allowed[MOVE_DIRECTION_BACK] ? 0 + : (MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RL], this->lidar_sensor_values[LIDAR_SENSOR_RR])); + collision_prevention_weights[MOVE_DIRECTION_RIGHT] = this->move_direction_allowed[MOVE_DIRECTION_RIGHT] ? 0 + : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR])); + + apply_collision_weights(); + } + + void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message) + { + + if (message->sensor_1 > 0) + { + this->lidar_sensor_values[LIDAR_SENSOR_FR] = message->sensor_1; + } + if (message->sensor_2 > 0) + { + this->lidar_sensor_values[LIDAR_SENSOR_FL] = message->sensor_2; + } + if (message->sensor_3 > 0) + { + this->lidar_sensor_values[LIDAR_SENSOR_RL] = message->sensor_3; + } + if (message->sensor_4 > 0) + { + this->lidar_sensor_values[LIDAR_SENSOR_RR] = message->sensor_4; + } + + for (int i = 0; i < 4; i++) + { + this->lidar_imu_readings[i] = message->imu_data[i]; + } + + check_move_direction_allowed(); + } + + void handle_odometry_message(const px4_msgs::msg::VehicleOdometry::SharedPtr message) + { + Quaternion q = {message->q[0], message->q[1], message->q[2], message->q[3]}; + this->current_yaw = get_yaw_angle(q); + } + + void handle_move_position_request( + const std::shared_ptr request_header, + const std::shared_ptr request, + const std::shared_ptr response) + { + RCLCPP_INFO(this->get_logger(), "Incoming request\nfront_back: %f\nleft_right: %f\nup_down: %f\nangle: %f", request->front_back, request->left_right, request->up_down, request->angle); + // TODO add check_move_direction_allowed results to this calculation + if (request->angle > 360 || request->angle < -360) + { + RCLCPP_ERROR(this->get_logger(), "Angle is not in range [-360, 360]"); + response->success = false; + return; + } + this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians + this->current_speed_z = request->up_down; + get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); + RCLCPP_INFO(this->get_logger(), "Calculated speed x: %f, y: %f", this->current_speed_x, this->current_speed_y); + send_trajectory_message(); + response->success = true; + } + + /** + * @brief Get the yaw angle from a specified normalized quaternion. + * Uses the theory from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + * + * @param q the quaternion that holds the rotation + * @return the yaw angle in radians + */ + static float get_yaw_angle(Quaternion q) + { + float siny_cosp = 2 * (q.w * q.z + q.x * q.y); + float cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); + } + + /** + * @brief Get the new x and y coordinates after a rotation of yaw radians + * + * @param x the original x coordinate + * @param y the original y coordinate + * @param yaw the angle to rotate by in radians + * @param x_out the resulting x coordinate + * @param y_out the resulting y coordinate + */ + static void get_x_y_with_rotation(float x, float y, float yaw, float *x_res, float *y_res) + { + *x_res = x * std::cos(yaw) - y * std::sin(yaw); + *y_res = x * std::sin(yaw) + y * std::cos(yaw); + } + +private: + rclcpp::Subscription::SharedPtr lidar_subscription; + rclcpp::Subscription::SharedPtr odometry_subscription; + + rclcpp::Client::SharedPtr trajectory_client; + rclcpp::Client::SharedPtr vehicle_control_client; + + rclcpp::Service::SharedPtr move_position_service; + + drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; + drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; + + float lidar_sensor_values[4]{MIN_DISTANCE}; // last distance measured by the lidar sensors + float lidar_imu_readings[4]; // last imu readings from the lidar sensors + float current_yaw = 0; // in radians + float current_speed_x = 0; + float current_speed_y = 0; + float current_speed_z = 0; + bool move_direction_allowed[4] = {true}; + float collision_prevention_weights[4] = {0}; + + template + void wait_for_service(T client) + { + while (!client->wait_for_service(1s)) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + } +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 130f0f65..d1ee469d 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -29,7 +29,14 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ArmDrone.srv" "srv/DisarmDrone.srv" "srv/ControlRelais.srv" + "srv/MovePosition.srv" "msg/DroneControlMode.msg" + "msg/DroneArmStatus.msg" + "msg/DroneRouteStatus.msg" + "msg/DroneStatus.msg" + "msg/HeightData.msg" + "msg/LidarReading.msg" + "msg/MultiflexReading.msg" ) if(BUILD_TESTING) diff --git a/src/height/msg/HeightData.msg b/src/drone_services/msg/HeightData.msg similarity index 100% rename from src/height/msg/HeightData.msg rename to src/drone_services/msg/HeightData.msg diff --git a/src/object_detection/msg/LidarReading.msg b/src/drone_services/msg/LidarReading.msg similarity index 100% rename from src/object_detection/msg/LidarReading.msg rename to src/drone_services/msg/LidarReading.msg diff --git a/src/object_detection/msg/MultiflexReading.msg b/src/drone_services/msg/MultiflexReading.msg similarity index 100% rename from src/object_detection/msg/MultiflexReading.msg rename to src/drone_services/msg/MultiflexReading.msg diff --git a/src/drone_services/srv/MovePosition.srv b/src/drone_services/srv/MovePosition.srv new file mode 100644 index 00000000..079e2025 --- /dev/null +++ b/src/drone_services/srv/MovePosition.srv @@ -0,0 +1,6 @@ +float32 front_back +float32 left_right +float32 up_down +float32 angle # difference in degrees, this will be added to the current angle +--- +bool success \ No newline at end of file diff --git a/src/height/CMakeLists.txt b/src/height/CMakeLists.txt index 32394387..ed63a9e4 100644 --- a/src/height/CMakeLists.txt +++ b/src/height/CMakeLists.txt @@ -19,12 +19,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(height REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/HeightData.msg" - ) +find_package(drone_services REQUIRED) add_executable(height_reader src/height_reader.cpp) target_include_directories(height_reader PUBLIC @@ -38,7 +33,7 @@ ament_target_dependencies( height_reader rclcpp TerabeeApi - height + drone_services ) install(TARGETS height_reader diff --git a/src/height/src/height_reader.cpp b/src/height/src/height_reader.cpp index 1df63c2d..55514143 100644 --- a/src/height/src/height_reader.cpp +++ b/src/height/src/height_reader.cpp @@ -7,7 +7,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "height/msg/height_data.hpp" +#include "drone_services/msg/height_data.hpp" #include #include @@ -48,7 +48,7 @@ public: RCLCPP_INFO(this->get_logger(), "Succesfully initialized Evo mini!"); timer_ = this->create_wall_timer(500ms, std::bind(&HeightReader::read_height, this)); - publisher_ = this->create_publisher("drone/height", 10); + publisher_ = this->create_publisher("drone/height", 10); } private: @@ -58,7 +58,7 @@ private: */ void read_height() { - auto msg = height::msg::HeightData(); + auto msg = drone_services::msg::HeightData(); // high initial minimal value float min = 255; @@ -82,7 +82,7 @@ private: // timer for publishing height readings rclcpp::TimerBase::SharedPtr timer_; // publisher for height readings - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; // factory for height sensor std::unique_ptr factory; diff --git a/src/object_detection/CMakeLists.txt b/src/object_detection/CMakeLists.txt index be10d823..26208cff 100644 --- a/src/object_detection/CMakeLists.txt +++ b/src/object_detection/CMakeLists.txt @@ -22,17 +22,11 @@ find_package(ament_cmake REQUIRED) # find_package( REQUIRED) find_package(rclcpp REQUIRED) find_package(TerabeeApi REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(object_detection REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/LidarReading.msg" - "msg/MultiflexReading.msg" - ) +find_package(drone_services REQUIRED) add_executable(lidar_reader src/lidar_reader.cpp) ament_target_dependencies(lidar_reader rclcpp - object_detection + drone_services ) target_link_libraries(lidar_reader ${TerabeeApi_LIBRARIES}) @@ -40,7 +34,7 @@ target_include_directories(lidar_reader PUBLIC ${TerabeeApi_INCLUDE_DIRS}) add_executable(multiflex_reader src/multiflex_reader.cpp) ament_target_dependencies(multiflex_reader rclcpp - object_detection + drone_services ) target_link_libraries(multiflex_reader ${TerabeeApi_LIBRARIES}) diff --git a/src/object_detection/src/lidar_reader.cpp b/src/object_detection/src/lidar_reader.cpp index 85faa854..a27786ac 100644 --- a/src/object_detection/src/lidar_reader.cpp +++ b/src/object_detection/src/lidar_reader.cpp @@ -1,7 +1,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "object_detection/msg/lidar_reading.hpp" +#include "drone_services/msg/lidar_reading.hpp" #include #include @@ -26,7 +26,7 @@ public: this->declare_parameter("lidar_serial_port", "/dev/ttyACM0", descriptor); // create publisher and bind timer to publish function - publisher_ = this->create_publisher("drone/object_detection", 10); + publisher_ = this->create_publisher("drone/object_detection", 10); timer_ = this->create_wall_timer(500ms, std::bind(&LidarReader::read_lidar_data, this)); factory = terabee::ITerarangerFactory::getFactory(); @@ -50,7 +50,7 @@ public: private: // publisher for lidar data - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; // timer for publishing readings rclcpp::TimerBase::SharedPtr timer_; @@ -64,7 +64,7 @@ private: */ void read_lidar_data() { - auto msg = object_detection::msg::LidarReading(); + auto msg = drone_services::msg::LidarReading(); // read distance data from all sensors msg.sensor_1 = tower->getDistance().distance.at(0); diff --git a/src/object_detection/src/multiflex_reader.cpp b/src/object_detection/src/multiflex_reader.cpp index a134f2a5..594afe02 100644 --- a/src/object_detection/src/multiflex_reader.cpp +++ b/src/object_detection/src/multiflex_reader.cpp @@ -1,7 +1,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "object_detection/msg/multiflex_reading.hpp" +#include "drone_services/msg/multiflex_reading.hpp" #include #include @@ -48,7 +48,7 @@ public: RCLCPP_INFO(this->get_logger(), "Multiflex initialized"); - publisher_ = this->create_publisher("drone/object_detection", 10); + publisher_ = this->create_publisher("drone/object_detection", 10); timer_ = this->create_wall_timer(500ms, std::bind(&MultiflexReader::read_multiflex_data, this)); } @@ -59,7 +59,7 @@ private: // timer and publisher for publishing message onto topic rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Publisher::SharedPtr publisher_; /** * @brief Reads distance data from the sensor and publishes it onto the drone/object_detection topic @@ -71,7 +71,7 @@ private: terabee::DistanceData data = multiflex->getDistance(); // populate message with readings - auto msg = object_detection::msg::MultiflexReading(); + auto msg = drone_services::msg::MultiflexReading(); for (size_t i = 0; i < data.size(); i++) { RCLCPP_INFO(this->get_logger(), "distance %f", data.distance[i]); diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fbad0885..7d975056 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -38,27 +38,27 @@ class PX4Controller : public rclcpp::Node public: PX4Controller() : Node("px4_controller") { - rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); - // create a publisher on the vehicle attitude setpoint topic + // publishers for controlling the drone vehicle_setpoint_publisher_ = this->create_publisher("/fmu/in/vehicle_attitude_setpoint", 10); vehicle_command_publisher_ = this->create_publisher("/fmu/in/vehicle_command", 10); trajectory_setpoint_publisher = this->create_publisher("/fmu/in/trajectory_setpoint", 10); + // publisher for publishing if the drone is armed arm_status_publisher_ = this->create_publisher("/drone/arm_status", 10); - // offboard_control_mode_publisher_ = this->create_publisher("/fmu/in/offboard_control_mode", 10); + // subscriptions from the Pixhawk vehicle_attitude_subscription_ = this->create_subscription("/fmu/out/vehicle_attitude", qos, std::bind(&PX4Controller::on_attitude_receive, 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)); + // subscription on receiving a new control mode control_mode_subscription_ = this->create_subscription("/drone/control_mode", qos, std::bind(&PX4Controller::on_control_mode_receive, this, std::placeholders::_1)); - - + // services for controlling the drone set_attitude_service_ = this->create_service("/drone/set_attitude", std::bind(&PX4Controller::handle_attitude_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); set_trajectory_service_ = this->create_service("/drone/set_trajectory", std::bind(&PX4Controller::handle_trajectory_setpoint, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); disarm_service_ = this->create_service("/drone/disarm", std::bind(&PX4Controller::handle_disarm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); arm_service_ = this->create_service("/drone/arm", std::bind(&PX4Controller::handle_arm_request, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - // create timer to send vehicle attitude setpoints every second + // create timer to send setpoints every 100 milliseconds timer_ = this->create_wall_timer(100ms, std::bind(&PX4Controller::send_setpoint, this)); start_time_ = this->get_clock()->now().seconds(); @@ -80,56 +80,47 @@ private: rclcpp::Service::SharedPtr disarm_service_; rclcpp::Service::SharedPtr arm_service_; - // rclcpp::Publisher::SharedPtr offboard_control_mode_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - double start_time_; - bool has_sent_status = false; + double start_time_; // time when the node was started bool user_in_control = false; // if user has taken over control - bool armed = false; - bool has_swithed = false; - int setpoint_count = 0; - float thrust = 0.0; - bool ready_to_fly = false; - float cur_yaw = 0; - bool new_setpoint = false; // for printing new q_d when a new setpoint has been received + bool armed = false; // if the drone is armed + bool ready_to_fly = false; // if the drone is ready to fly + bool new_setpoint = false; // for printing new q_d when a new setpoint has been received - float last_setpoint[3] = {0, 0, 0}; - float last_angle = 0; // angle in radians (-PI .. PI) - float last_thrust = 0; // default 10% thrust for when the drone gets armed + float last_setpoint[3] = {0, 0, 0}; // yaw, pitch, roll + float last_angle = 0; // angle in radians (-PI .. PI) + float last_thrust = 0; // default 10% thrust for when the drone gets armed - float velocity[3] = {0, 0, 0}; - float position[3] = {0, 0, 0}; + float velocity[3] = {0, 0, 0}; // velocity setpoint [x,y,z] + float position[3] = {0, 0, 0}; // position setpoint [x,y,z] - float base_q[4] = {0, 0, 0, 0}; - int base_q_amount = 0; + float base_q[4] = {0, 0, 0, 0}; // base local position quaternion + int base_q_amount = 0; // amount of base quaternions received char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control - bool start_trajectory = false; - const float omega = 0.3; // angular speed of the POLAR trajectory - const float K = 0; // [m] gain that regulates the spiral pitch + const float omega = 0.3; // angular speed of the POLAR trajectory + const float K = 0; // [m] gain that regulates the spiral pitch - float rho_0 = 0; - float theta_0 = 0; - const float p0_z = -0.0; - float p0_x = rho_0 * cos(theta_0); - float p0_y = rho_0 * sin(theta_0); - float des_x = p0_x, des_y = p0_y, des_z = p0_z; - float dot_des_x = 0.0, dot_des_y = 0.0; - float gamma = M_PI_4; + float rho_0 = 0; // initial rho of polar coordinate + float theta_0 = 0; // initial theta of polar coordinate + float p0_z = -0.0; // initial height + float des_x = p0_x, des_y = p0_y, des_z = p0_z; // desired position + float dot_des_x = 0.0, dot_des_y = 0.0; // desired velocity + float gamma = M_PI_4; // desired heading direction - float X; - float Y; + float local_x = 0; // local position x + float local_y = 0; // local position y - float local_x = 0; - float local_y = 0; - - uint32_t discrete_time_index = 0; - - // result quaternion - std::array q = {0, 0, 0, 0}; + std::array q = {0, 0, 0, 0}; // result quaternion + /** + * @brief handles reception of attitude setpoints through service + * + * @param request_header the header of the request + * @param request the request containing the new attitude setpoint + * @param response the response indicating if the setpoint was changed successfully + */ void handle_attitude_setpoint( const std::shared_ptr request_header, const std::shared_ptr request, @@ -172,6 +163,13 @@ private: } } + /** + * @brief handles reception of trajectory setpoints through service + * + * @param request_header the header of the request + * @param request the request containing the new trajectory setpoint (velocity or position) + * @param response the response indicating if the setpoint was changed successfully + */ void handle_trajectory_setpoint( const std::shared_ptr request_header, const std::shared_ptr request, @@ -189,7 +187,7 @@ private: { for (int i = 0; i < 3; i++) { - velocity[i] += request->values[i]; + velocity[i] = request->values[i]; } RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint. %f %f %f", velocity[0], velocity[1], velocity[2]); } @@ -202,7 +200,7 @@ private: RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]); } - last_angle += request->yaw; + last_angle = request->yaw; new_setpoint = true; RCLCPP_INFO(this->get_logger(), "Yaw: %f", last_angle); response->success = true; @@ -232,7 +230,7 @@ private: auto msg = drone_services::msg::DroneArmStatus(); msg.armed = false; arm_status_publisher_->publish(msg); - + response->success = true; } else @@ -280,13 +278,17 @@ private: } } + /** + * @brief sends the latest received attitude setpoint to the drone + * + */ void send_attitude_setpoint() { // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); - // move up? + // move up msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east msg.thrust_body[2] = -last_thrust; @@ -306,6 +308,10 @@ private: vehicle_setpoint_publisher_->publish(msg); } + /** + * @brief sends the latest received velocity setpoint to the drone + * + */ void send_velocity_setpoint() { auto msg = px4_msgs::msg::TrajectorySetpoint(); @@ -313,20 +319,24 @@ private: msg.velocity[0] = velocity[0]; msg.velocity[1] = velocity[1]; msg.velocity[2] = -velocity[2]; + // set no position control for (int i = 0; i < 3; i++) { msg.position[i] = NAN; } - publish_trajectory_setpoint(msg); } + /** + * @brief sends the latest received position setpoint to the drone + * + */ void send_position_setpoint() { auto msg = px4_msgs::msg::TrajectorySetpoint(); RCLCPP_INFO(this->get_logger(), "Sending position setpoint: %f %f %f", des_x, des_y, des_z); - RCLCPP_INFO(this->get_logger(),"local position: %f %f", local_x, local_y); + RCLCPP_INFO(this->get_logger(), "local position: %f %f", local_x, local_y); msg.position = {local_x, local_y, des_z}; msg.velocity = {dot_des_x, dot_des_y, 0.0}; msg.yaw = gamma; //-3.14; // [-PI:PI] @@ -335,6 +345,11 @@ private: trajectory_setpoint_publisher->publish(msg); } + /** + * @brief publishes a trajectory setpoint + * + * @param msg the message containing the trajectory setpoint + */ void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg) { msg.yaw = last_angle; @@ -344,7 +359,7 @@ private: } /** - * @brief Send setpoints to PX4. First, 20 setpoints will be sent before offboard mode will be engaged and the drone will be armed. + * @brief Send setpoints to PX4. * */ void send_setpoint() @@ -352,7 +367,7 @@ private: // 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 * discrete_time_index; + float theta = theta_0 + omega * 0.1; float rho = rho_0 + K * theta; // from polar to cartesian coordinates @@ -376,7 +391,6 @@ private: { if (current_control_mode == CONTROL_MODE_ATTITUDE) { - // RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); send_attitude_setpoint(); } else @@ -387,32 +401,26 @@ private: } if (current_control_mode == CONTROL_MODE_VELOCITY) { - // RCLCPP_INFO(this->get_logger(), "Sending velocity setpoint"); send_velocity_setpoint(); } else if (current_control_mode == CONTROL_MODE_POSITION) { - // RCLCPP_INFO(this->get_logger(), "Sending position setpoint"); send_position_setpoint(); } new_setpoint = false; } } - if (start_trajectory) - { - discrete_time_index++; - } } + /** + * @brief Callback function for the attitude topic. + * This function is called every time a new message is received on the topic. + * It only stores the first two measurements while the drone is not armed. + * + * @param msg the message containing the attitude + */ void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) { - /* - average q: - - 0.7313786745071411 - - 0.005042835604399443 - - 0.0002370707516092807 - - 0.6819528937339783 - */ if (!armed) { if (base_q_amount > 2) @@ -428,32 +436,28 @@ private: base_q[3] = msg->q[3]; base_q_amount++; } - - // RCLCPP_INFO(this->get_logger(), "base_q: %f %f %f %f", base_q[0], base_q[1], base_q[2], base_q[3]); } } + /** + * @brief Callback function for receiving the local position from the Pixhawk + * The local position is only received while the drone is not yet flying (user has not yet taken over control) + * + * @param msg the message conataining the local position + */ void on_local_position_receive(const px4_msgs::msg::VehicleLocalPosition::SharedPtr msg) { // set start values to current position if (!user_in_control) { // https://www.math.usm.edu/lambers/mat169/fall09/lecture33.pdf - rho_0 = pow(msg->x,2) + pow(msg->y,2); + rho_0 = pow(msg->x, 2) + pow(msg->y, 2); theta_0 = atan2(msg->y, msg->x); last_angle = msg->heading; local_x = msg->x; local_y = msg->y; } - X = msg->x; - Y = msg->y; - float Z = msg->z; - if (!start_trajectory && (p0_x + 1.0 > X && p0_x - 1.0 < X) && (p0_y + 1.0 > Y && p0_y - 1.0 < Y) && (p0_z + 1.0 > Z && p0_z - 1.0 < Z)) - { - start_trajectory = true; - RCLCPP_INFO(this->get_logger(), "start trajectory"); - } } void on_control_mode_receive(const drone_services::msg::DroneControlMode::SharedPtr msg) diff --git a/src/px4_msgs b/src/px4_msgs index ffc3a4cd..b64ef047 160000 --- a/src/px4_msgs +++ b/src/px4_msgs @@ -1 +1 @@ -Subproject commit ffc3a4cd578776213a444abe17d7eabf9621b266 +Subproject commit b64ef0475c1d44605688f4770899fe453d532be4