From 6493a4f85dd049726e594170df1d8096710668b6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 24 May 2023 08:48:07 +0200 Subject: [PATCH 01/43] add PositionController file --- src/drone_controls/CMakeLists.txt | 27 ++++++++++------------ src/drone_controls/package.xml | 12 ++++++---- src/drone_controls/src/PositionChanger.cpp | 20 ++++++++++++++++ src/px4_msgs | 2 +- 4 files changed, 40 insertions(+), 21 deletions(-) create mode 100644 src/drone_controls/src/PositionChanger.cpp diff --git a/src/drone_controls/CMakeLists.txt b/src/drone_controls/CMakeLists.txt index cc182294..c2fb836b 100644 --- a/src/drone_controls/CMakeLists.txt +++ b/src/drone_controls/CMakeLists.txt @@ -1,32 +1,29 @@ -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(drone_services REQUIRED) +find_package(object_detection REQUIRED) +find_package(height REQUIRED) 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..9523b719 100644 --- a/src/drone_controls/package.xml +++ b/src/drone_controls/package.xml @@ -3,14 +3,16 @@ 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 + 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..8a7b3670 --- /dev/null +++ b/src/drone_controls/src/PositionChanger.cpp @@ -0,0 +1,20 @@ +#include "rclcpp/rclcpp.hpp" + +#include "object_detection/msg/LidarReading.msg" +#include "height/msg/HeightData.msg" +#include "drone_services/srv/SetVehicleControl.srv" +class PositionChanger : public rclcpp::Node +{ +public: + PositionChanger() : Node("position_changer") + { + + } +}; +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} 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 -- 2.47.2 From ea7ae0f6d1f1ebf4d44b156c3d3ea6b8ed5ef8cb Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 24 May 2023 09:00:18 +0200 Subject: [PATCH 02/43] add subscribtion to lidar topic --- src/drone_controls/src/PositionChanger.cpp | 29 ++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 8a7b3670..1a9e82a8 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -3,13 +3,42 @@ #include "object_detection/msg/LidarReading.msg" #include "height/msg/HeightData.msg" #include "drone_services/srv/SetVehicleControl.srv" +#include "drone_services/srv/SetTrajectory/srv" + +#define LIDAR_SENSOR_FR 0 // front right +#define LIDAR_SENSOR_FL 1 // front left +#define LIDAR_SENSOR_RR 2 // rear right +#define LIDAR_SENSOR_RL 3 // rear left + 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)) } + + void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) + { + this->lidar_sensor_values[0] = message->sensor_1; + this->lidar_sensor_values[1] = message->sensor_2; + this->lidar_sensor_values[2] = message->sensor_3; + this->lidar_sensor_values[3] = message->sensor_4; + + for (int i = 0; i < 4; i++) + { + this->lidar_imu_readings[i] = message->imu_data[i]; + } + } + +private: + rclcpp::Subscription lidar_subscription; + float lidar_sensor_values[4]; + float lidar_imu_readings[4]; }; int main(int argc, char *argv[]) { -- 2.47.2 From c7f764726f89c68394d99145ef5842a15800d47c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 24 May 2023 17:01:45 +0200 Subject: [PATCH 03/43] add calculation of rotation x and y values --- .vscode/settings.json | 16 ++- src/drone_controls/CMakeLists.txt | 1 + src/drone_controls/package.xml | 1 + src/drone_controls/src/PositionChanger.cpp | 113 +++++++++++++++++++-- src/px4_connection/src/px4_controller.cpp | 2 +- 5 files changed, 124 insertions(+), 9 deletions(-) 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/drone_controls/CMakeLists.txt b/src/drone_controls/CMakeLists.txt index c2fb836b..3a068f39 100644 --- a/src/drone_controls/CMakeLists.txt +++ b/src/drone_controls/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED) # further dependencies manually. # find_package( REQUIRED) find_package(rclcpp REQUIRED) +find_package(px4_msgs REQUIRED) find_package(drone_services REQUIRED) find_package(object_detection REQUIRED) find_package(height REQUIRED) diff --git a/src/drone_controls/package.xml b/src/drone_controls/package.xml index 9523b719..89d4dbf4 100644 --- a/src/drone_controls/package.xml +++ b/src/drone_controls/package.xml @@ -7,6 +7,7 @@ sem Apache Licence 2.0 rclcpp + px4_msgs drone_services object_detection height diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 1a9e82a8..3bdd06e4 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -1,14 +1,30 @@ #include "rclcpp/rclcpp.hpp" +#include "px4_msgs/msg/VehicleOdometry.msg" #include "object_detection/msg/LidarReading.msg" #include "height/msg/HeightData.msg" #include "drone_services/srv/SetVehicleControl.srv" #include "drone_services/srv/SetTrajectory/srv" +#define _USE_MATH_DEFINES +#include + #define LIDAR_SENSOR_FR 0 // front right #define LIDAR_SENSOR_FL 1 // front left -#define LIDAR_SENSOR_RR 2 // rear right -#define LIDAR_SENSOR_RL 3 // rear 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 + +struct Quaternion +{ + float w, x, y, z; +}; class PositionChanger : public rclcpp::Node { @@ -18,27 +34,110 @@ public: 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)) + this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, 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)); + } + /** + * @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; + + if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) + { + collision_prevention_weights[MOVE_DIRECTION_FRONT] = -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FR], this->lidar_sensor_values[LIDAR_SENSOR_FL])); + } + if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) + { + collision_prevention_weights[MOVE_DIRECTION_LEFT] = -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FL], this->lidar_sensor_values[LIDAR_SENSOR_RL])); + } + if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) + { + collision_prevention_weights[MOVE_DIRECTION_BACK] = -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RL], this->lidar_sensor_values[LIDAR_SENSOR_RR])); + } + if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) + { + collision_prevention_weights[MOVE_DIRECTION_RIGHT] = -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR])); + } } void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) { - this->lidar_sensor_values[0] = message->sensor_1; - this->lidar_sensor_values[1] = message->sensor_2; - this->lidar_sensor_values[2] = message->sensor_3; - this->lidar_sensor_values[3] = message->sensor_4; + + 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]}; + odometry_yaw = get_yaw_angle(q); + } + + /** + * @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_out, float &y_out) + { + x_out = x * std::cos(yaw) - y * std::sin(yaw); + y_out = x * std::sin(yaw) + y * std::cos(yaw); } private: rclcpp::Subscription lidar_subscription; + rclcpp::Subscription odometry_subscription; float lidar_sensor_values[4]; float lidar_imu_readings[4]; + float currrent_yaw = 0; + bool move_direction_allowed[4] = {true, true, true, true}; + float collision_prevention_weights[4] = {0,0,0,0}; }; int main(int argc, char *argv[]) { diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fbad0885..cc5d4587 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -174,7 +174,7 @@ private: void handle_trajectory_setpoint( const std::shared_ptr request_header, - const std::shared_ptr request, + const std::shared_ptr request,handle_trajehandle_traje const std::shared_ptr response) { if (!(request->control_mode == CONTROL_MODE_VELOCITY || request->control_mode == CONTROL_MODE_POSITION)) -- 2.47.2 From cfa2a39612bfbd965c75f06704cc1b264aa160d4 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 24 May 2023 17:09:57 +0200 Subject: [PATCH 04/43] clean up checking move direction --- src/drone_controls/src/PositionChanger.cpp | 51 ++++++++++------------ 1 file changed, 23 insertions(+), 28 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 3bdd06e4..f7271018 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -34,12 +34,11 @@ public: 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)) - this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); - } + this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, 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)); + } /** - * @brief checks for every direction is an object is too close and if we can move in that direction. + * @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() @@ -49,22 +48,15 @@ public: 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; - if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) - { - collision_prevention_weights[MOVE_DIRECTION_FRONT] = -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FR], this->lidar_sensor_values[LIDAR_SENSOR_FL])); - } - if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) - { - collision_prevention_weights[MOVE_DIRECTION_LEFT] = -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_FL], this->lidar_sensor_values[LIDAR_SENSOR_RL])); - } - if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) - { - collision_prevention_weights[MOVE_DIRECTION_BACK] = -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RL], this->lidar_sensor_values[LIDAR_SENSOR_RR])); - } - if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) - { - collision_prevention_weights[MOVE_DIRECTION_RIGHT] = -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR])); - } + // 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])); } void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) @@ -98,13 +90,13 @@ public: 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]}; - odometry_yaw = get_yaw_angle(q); + odometry_yaw = get_yaw_angle(q); } /** * @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 */ @@ -117,7 +109,7 @@ public: /** * @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 @@ -133,11 +125,14 @@ public: private: rclcpp::Subscription lidar_subscription; rclcpp::Subscription odometry_subscription; - float lidar_sensor_values[4]; - float lidar_imu_readings[4]; - float currrent_yaw = 0; - bool move_direction_allowed[4] = {true, true, true, true}; - float collision_prevention_weights[4] = {0,0,0,0}; + float lidar_sensor_values[4]; // last distance measured by the lidar sensors + float lidar_imu_readings[4]; // last imu readings from the lidar sensors + float currrent_yaw = 0; // in radians + float current_x_speed = 0; + float current_y_speed = 0; + float current_z_speed = 0; + bool move_direction_allowed[4] = {true}; + float collision_prevention_weights[4] = {0}; }; int main(int argc, char *argv[]) { -- 2.47.2 From 59f0ee7deb68e1f737e63eb6d1366ea607b129d3 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 24 May 2023 19:14:19 +0200 Subject: [PATCH 05/43] add clients for heartbeat --- src/drone_controls/src/PositionChanger.cpp | 42 +++++++++++++++++++--- 1 file changed, 37 insertions(+), 5 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index f7271018..4d70aec9 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -21,6 +21,8 @@ #define MIN_DISTANCE 1.0 // in meters +using namespace std::chrono_literals; + struct Quaternion { float w, x, y, z; @@ -35,6 +37,16 @@ public: 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)) this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, 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->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); + } } /** @@ -50,13 +62,13 @@ public: // 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])); + : -(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])); + : -(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])); + : -(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])); + : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR])); } void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) @@ -90,7 +102,7 @@ public: 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]}; - odometry_yaw = get_yaw_angle(q); + current_yaw = get_yaw_angle(q); } /** @@ -125,6 +137,11 @@ public: private: rclcpp::Subscription lidar_subscription; rclcpp::Subscription odometry_subscription; + rclcpp::Client::SharedPtr trajectory_client; + rclcpp::Client::SharedPtr vehicle_control_client; + drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; + drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; + float lidar_sensor_values[4]; // last distance measured by the lidar sensors float lidar_imu_readings[4]; // last imu readings from the lidar sensors float currrent_yaw = 0; // in radians @@ -133,6 +150,21 @@ private: float current_z_speed = 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 0; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + } + + } }; int main(int argc, char *argv[]) { -- 2.47.2 From 9f9b0e33927b1c5d0d967984b6efd4ad21e11b43 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Wed, 24 May 2023 20:02:37 +0200 Subject: [PATCH 06/43] add service --- src/drone_controls/src/PositionChanger.cpp | 239 ++++++++++++--------- src/drone_services/CMakeLists.txt | 4 + src/drone_services/srv/MovePosition.srv | 6 + 3 files changed, 147 insertions(+), 102 deletions(-) create mode 100644 src/drone_services/srv/MovePosition.srv diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 4d70aec9..aebf429d 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -1,10 +1,11 @@ #include "rclcpp/rclcpp.hpp" -#include "px4_msgs/msg/VehicleOdometry.msg" -#include "object_detection/msg/LidarReading.msg" -#include "height/msg/HeightData.msg" -#include "drone_services/srv/SetVehicleControl.srv" -#include "drone_services/srv/SetTrajectory/srv" +#include +#include +#include +#include +#include +#include #define _USE_MATH_DEFINES #include @@ -21,6 +22,8 @@ #define MIN_DISTANCE 1.0 // in meters +#define DEFAULT_CONTROL_MODE 4 // default velocity control bitmask + using namespace std::chrono_literals; struct Quaternion @@ -38,6 +41,7 @@ public: this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, 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->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"); @@ -46,126 +50,157 @@ public: 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); - } - } - /** - * @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; + this->trajectory_request = std::make_shared(); + this->vehicle_control_request = std::make_shared(); - // 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])); - } + this->vehicle_control_request->control_mode = DEFAULT_CONTROL_MODE; + auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request); - void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) - { - - if (message->sensor_1 > 0) + if (rclcpp::spin_until_future_complete(this, control_mode_response) == + rclcpp::FutureReturnCode::SUCCESS) { - this->lidar_sensor_values[LIDAR_SENSOR_FR] = message->sensor_1; + RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", control_mode_response.get()->success); } - if (message->sensor_2 > 0) + else { - this->lidar_sensor_values[LIDAR_SENSOR_FL] = message->sensor_2; + RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control"); } - 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) +/** + * @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])); +} + +void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) +{ + + if (message->sensor_1 > 0) { - Quaternion q = {message->q[0], message->q[1], message->q[2], message->q[3]}; - current_yaw = get_yaw_angle(q); + 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; } - /** - * @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) + for (int i = 0; i < 4; i++) { - 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); + this->lidar_imu_readings[i] = message->imu_data[i]; } - /** - * @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_out, float &y_out) - { - x_out = x * std::cos(yaw) - y * std::sin(yaw); - y_out = x * std::sin(yaw) + y * std::cos(yaw); - } + 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]}; + 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) +{ + //TODO add check_move_direction_allowed results to this calculation + this->current_yaw = request->angle * (M_PI / 180.0); // get the angle in radians + get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); +} + +/** + * @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 lidar_subscription; - rclcpp::Subscription odometry_subscription; - rclcpp::Client::SharedPtr trajectory_client; - rclcpp::Client::SharedPtr vehicle_control_client; - drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; - drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; +rclcpp::Subscription lidar_subscription; +rclcpp::Subscription odometry_subscription; - float lidar_sensor_values[4]; // last distance measured by the lidar sensors - float lidar_imu_readings[4]; // last imu readings from the lidar sensors - float currrent_yaw = 0; // in radians - float current_x_speed = 0; - float current_y_speed = 0; - float current_z_speed = 0; - bool move_direction_allowed[4] = {true}; - float collision_prevention_weights[4] = {0}; +rclcpp::Client::SharedPtr trajectory_client; +rclcpp::Client::SharedPtr vehicle_control_client; - template - void wait_for_service(T 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]; // last distance measured by the lidar sensors +float lidar_imu_readings[4]; // last imu readings from the lidar sensors +float currrent_yaw = 0; // in radians +float current_x_speed = 0; +float current_y_speed = 0; +float current_z_speed = 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)) { - while (!client->wait_for_service(1s)) + if (!rclcpp::ok()) { - if (!rclcpp::ok()) - { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); - return 0; - } - RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return 0; } - + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); } -}; +} +} +; int main(int argc, char *argv[]) { rclcpp::init(argc, argv); diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 130f0f65..344655d3 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -29,7 +29,11 @@ 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" ) if(BUILD_TESTING) diff --git a/src/drone_services/srv/MovePosition.srv b/src/drone_services/srv/MovePosition.srv new file mode 100644 index 00000000..25d82e00 --- /dev/null +++ b/src/drone_services/srv/MovePosition.srv @@ -0,0 +1,6 @@ +float front_back +float left_right +float up_down +float angle # in degrees +--- +bool success \ No newline at end of file -- 2.47.2 From ced23113d7c83359582fbd122794b574bcbb5323 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 10:40:57 +0200 Subject: [PATCH 07/43] typo --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index cc5d4587..fbad0885 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -174,7 +174,7 @@ private: void handle_trajectory_setpoint( const std::shared_ptr request_header, - const std::shared_ptr request,handle_trajehandle_traje + const std::shared_ptr request, const std::shared_ptr response) { if (!(request->control_mode == CONTROL_MODE_VELOCITY || request->control_mode == CONTROL_MODE_POSITION)) -- 2.47.2 From f372d8d32ddfe3eb940a4757aa2e6a85387ae300 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 11:28:41 +0200 Subject: [PATCH 08/43] fix moveposition service floats --- src/drone_services/srv/MovePosition.srv | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drone_services/srv/MovePosition.srv b/src/drone_services/srv/MovePosition.srv index 25d82e00..705ef95c 100644 --- a/src/drone_services/srv/MovePosition.srv +++ b/src/drone_services/srv/MovePosition.srv @@ -1,6 +1,6 @@ -float front_back -float left_right -float up_down -float angle # in degrees +float32 front_back +float32 left_right +float32 up_down +float32 angle # in degrees --- bool success \ No newline at end of file -- 2.47.2 From bb9fcd97ab05729a2d25ca21038fd4b957a0ac5e Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 11:45:30 +0200 Subject: [PATCH 09/43] add publish client and log incoming message --- src/drone_controls/src/PositionChanger.cpp | 23 ++++++++++++++++++++-- src/drone_services/srv/MovePosition.srv | 2 +- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index aebf429d..50f96241 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -68,6 +68,24 @@ public: } } } +void send_trajectory_message() +{ + this->trajectory_request->x = this->current_speed_x; + this->trajectory_request->y = this->current_speed_y; + this->trajectory_request->z = this->current_z_speed; + this->trajectory_request->yaw = this->current_yaw; + auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request); + + if (rclcpp::spin_until_future_complete(this, trajectory_response) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Trajectory set result: %d", trajectory_response.get()->success); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to call service to set trajectory"); + } +} /** * @brief checks for every direction is an object is too close and if we can move in that direction. @@ -123,7 +141,7 @@ void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr m 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]}; - current_yaw = get_yaw_angle(q); + this->current_yaw = get_yaw_angle(q); } void handle_move_position_request( @@ -131,8 +149,9 @@ void handle_move_position_request( 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 - this->current_yaw = request->angle * (M_PI / 180.0); // get the angle in radians + this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); } diff --git a/src/drone_services/srv/MovePosition.srv b/src/drone_services/srv/MovePosition.srv index 705ef95c..079e2025 100644 --- a/src/drone_services/srv/MovePosition.srv +++ b/src/drone_services/srv/MovePosition.srv @@ -1,6 +1,6 @@ float32 front_back float32 left_right float32 up_down -float32 angle # in degrees +float32 angle # difference in degrees, this will be added to the current angle --- bool success \ No newline at end of file -- 2.47.2 From fe3917050e457dde586918b199001b01e95db9ce Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 11:53:55 +0200 Subject: [PATCH 10/43] move height and lidar message definitions to drone_services package --- src/drone_services/CMakeLists.txt | 3 +++ src/{height => drone_services}/msg/HeightData.msg | 0 .../msg/LidarReading.msg | 0 .../msg/MultiflexReading.msg | 0 src/height/CMakeLists.txt | 9 ++------- src/height/src/height_reader.cpp | 8 ++++---- src/object_detection/CMakeLists.txt | 12 +++--------- src/object_detection/src/lidar_reader.cpp | 8 ++++---- src/object_detection/src/multiflex_reader.cpp | 8 ++++---- 9 files changed, 20 insertions(+), 28 deletions(-) rename src/{height => drone_services}/msg/HeightData.msg (100%) rename src/{object_detection => drone_services}/msg/LidarReading.msg (100%) rename src/{object_detection => drone_services}/msg/MultiflexReading.msg (100%) diff --git a/src/drone_services/CMakeLists.txt b/src/drone_services/CMakeLists.txt index 344655d3..d1ee469d 100644 --- a/src/drone_services/CMakeLists.txt +++ b/src/drone_services/CMakeLists.txt @@ -34,6 +34,9 @@ rosidl_generate_interfaces(${PROJECT_NAME} "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/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]); -- 2.47.2 From dae0d06d61828cf94229cbc49ad59f8930726be2 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 11:55:36 +0200 Subject: [PATCH 11/43] update drone services msg definitions in positionchanger --- src/drone_controls/src/PositionChanger.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 50f96241..f8e0d803 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -1,8 +1,8 @@ #include "rclcpp/rclcpp.hpp" #include -#include -#include +#include +#include #include #include #include @@ -39,7 +39,7 @@ public: 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)) this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); + this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, 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->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)); @@ -110,7 +110,7 @@ check_move_direction_allowed() : -(MIN_DISTANCE - std::min(this->lidar_sensor_values[LIDAR_SENSOR_RR], this->lidar_sensor_values[LIDAR_SENSOR_FR])); } -void handle_lidar_message(const object_detection::msg::LidarReading::SharedPtr message) +void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message) { if (message->sensor_1 > 0) @@ -185,7 +185,7 @@ static void get_x_y_with_rotation(float x, float y, float yaw, float *x_res, flo } private: -rclcpp::Subscription lidar_subscription; +rclcpp::Subscription lidar_subscription; rclcpp::Subscription odometry_subscription; rclcpp::Client::SharedPtr trajectory_client; -- 2.47.2 From c0c67d65a0fe808717aea0181aaef8087ac7fa08 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 11:56:27 +0200 Subject: [PATCH 12/43] remove object detection and height packages from position changer cmakelists --- src/drone_controls/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drone_controls/CMakeLists.txt b/src/drone_controls/CMakeLists.txt index 3a068f39..f46355cc 100644 --- a/src/drone_controls/CMakeLists.txt +++ b/src/drone_controls/CMakeLists.txt @@ -13,8 +13,6 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(px4_msgs REQUIRED) find_package(drone_services REQUIRED) -find_package(object_detection REQUIRED) -find_package(height REQUIRED) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) -- 2.47.2 From f94186c916a3a8b24179861c87753e1bda677a69 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 11:58:27 +0200 Subject: [PATCH 13/43] add position changer executable --- src/drone_controls/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drone_controls/CMakeLists.txt b/src/drone_controls/CMakeLists.txt index f46355cc..b240af43 100644 --- a/src/drone_controls/CMakeLists.txt +++ b/src/drone_controls/CMakeLists.txt @@ -14,6 +14,10 @@ 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) + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights -- 2.47.2 From 54a7636417b33a98541cbff6e2156b6b340ee0e5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:00:27 +0200 Subject: [PATCH 14/43] typo --- src/drone_controls/src/PositionChanger.cpp | 260 ++++++++++----------- 1 file changed, 129 insertions(+), 131 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index f8e0d803..b826cac1 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -67,158 +67,156 @@ public: RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control"); } } -} -void send_trajectory_message() -{ - this->trajectory_request->x = this->current_speed_x; - this->trajectory_request->y = this->current_speed_y; - this->trajectory_request->z = this->current_z_speed; - this->trajectory_request->yaw = this->current_yaw; - auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request); + void send_trajectory_message() + { + this->trajectory_request->x = this->current_speed_x; + this->trajectory_request->y = this->current_speed_y; + this->trajectory_request->z = this->current_z_speed; + this->trajectory_request->yaw = this->current_yaw; + auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request); - if (rclcpp::spin_until_future_complete(this, trajectory_response) == - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Trajectory set result: %d", trajectory_response.get()->success); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to call service to set trajectory"); - } -} - -/** - * @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])); -} - -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; + if (rclcpp::spin_until_future_complete(this, trajectory_response) == + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO(this->get_logger(), "Trajectory set result: %d", trajectory_response.get()->success); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to call service to set trajectory"); + } } - for (int i = 0; i < 4; i++) + /** + * @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->lidar_imu_readings[i] = message->imu_data[i]; + 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])); } - check_move_direction_allowed(); -} + void handle_lidar_message(const drone_services::msg::LidarReading::SharedPtr message) + { -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); -} + 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; + } -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 - this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians - get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); -} + for (int i = 0; i < 4; i++) + { + this->lidar_imu_readings[i] = message->imu_data[i]; + } -/** - * @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); -} + check_move_direction_allowed(); + } -/** - * @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); -} + 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 + this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians + get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); + } + + /** + * @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 lidar_subscription; -rclcpp::Subscription odometry_subscription; + rclcpp::Subscription lidar_subscription; + rclcpp::Subscription odometry_subscription; -rclcpp::Client::SharedPtr trajectory_client; -rclcpp::Client::SharedPtr vehicle_control_client; + rclcpp::Client::SharedPtr trajectory_client; + rclcpp::Client::SharedPtr vehicle_control_client; -rclcpp::Service::SharedPtr move_position_service; + rclcpp::Service::SharedPtr move_position_service; -drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; -drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; + drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; + drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; -float lidar_sensor_values[4]; // last distance measured by the lidar sensors -float lidar_imu_readings[4]; // last imu readings from the lidar sensors -float currrent_yaw = 0; // in radians -float current_x_speed = 0; -float current_y_speed = 0; -float current_z_speed = 0; -bool move_direction_allowed[4] = {true}; -float collision_prevention_weights[4] = {0}; + float lidar_sensor_values[4]; // last distance measured by the lidar sensors + float lidar_imu_readings[4]; // last imu readings from the lidar sensors + float currrent_yaw = 0; // in radians + float current_x_speed = 0; + float current_y_speed = 0; + float current_z_speed = 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)) + template + void wait_for_service(T client) { - if (!rclcpp::ok()) + while (!client->wait_for_service(1s)) { - RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); - return 0; + if (!rclcpp::ok()) + { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); } - RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); } -} -} +}; ; int main(int argc, char *argv[]) { -- 2.47.2 From e4d5228cd5f00a2f9f1f9eaa89711a6926b546aa Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:03:17 +0200 Subject: [PATCH 15/43] ; --- src/drone_controls/src/PositionChanger.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index b826cac1..b3069aa1 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -39,7 +39,8 @@ public: 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)) this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); + this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, 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->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)); -- 2.47.2 From 73ca2a5e2df5a666c1c273e2d746048485a03faa Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:06:06 +0200 Subject: [PATCH 16/43] stuff --- src/drone_controls/src/PositionChanger.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index b3069aa1..e0a1d0c6 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -40,8 +40,9 @@ public: 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)); - this->odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, 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)); + + // 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"); -- 2.47.2 From a48408ee0c37e89e994ef8f879b6af7782ec0ad6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:07:03 +0200 Subject: [PATCH 17/43] remove lidar subscription placeholder --- src/drone_controls/src/PositionChanger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index e0a1d0c6..fa503b54 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -39,7 +39,7 @@ public: 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)); + this->lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this)); // 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)); -- 2.47.2 From 3d8074e8230419efd5bb277f8a78cb5ab5de7dc2 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:10:24 +0200 Subject: [PATCH 18/43] remove this --- src/drone_controls/src/PositionChanger.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index fa503b54..8da28b2c 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -39,15 +39,15 @@ public: 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)); + 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"); + odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); + 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)); + trajectory_client = this->create_client("/drone/set_trajectory"); + 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..."); -- 2.47.2 From 7b304866d3bf384c23a6af1506cd1d3e2e1a0e1a Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:12:14 +0200 Subject: [PATCH 19/43] fix pointer --- src/drone_controls/src/PositionChanger.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 8da28b2c..5bfa5838 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -39,15 +39,15 @@ public: rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 5), qos_profile); - lidar_subscription = this->create_subscription("/drone/object_detection", qos, std::bind(&PositionChanger::handle_lidar_message, this,std::placeholders::_1)); + 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)); - odometry_subscription = this->create_subscription("/fmu/out/vehicle_odometry", qos, std::bind(&PositionChanger::handle_odometry_message, this, std::placeholders::_1)); - 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->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"); - trajectory_client = this->create_client("/drone/set_trajectory"); - 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..."); @@ -185,8 +185,8 @@ public: } private: - rclcpp::Subscription lidar_subscription; - rclcpp::Subscription odometry_subscription; + rclcpp::Subscription::SharedPtr lidar_subscription; + rclcpp::Subscription::SharedPtr odometry_subscription; rclcpp::Client::SharedPtr trajectory_client; rclcpp::Client::SharedPtr vehicle_control_client; -- 2.47.2 From 6ca7597703a1ed40d29e42ca052f4ec7a8cd0542 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:14:14 +0200 Subject: [PATCH 20/43] fix control request --- src/drone_controls/src/PositionChanger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 5bfa5838..22b0cfff 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -56,7 +56,7 @@ public: this->trajectory_request = std::make_shared(); this->vehicle_control_request = std::make_shared(); - this->vehicle_control_request->control_mode = DEFAULT_CONTROL_MODE; + this->vehicle_control_request->control = DEFAULT_CONTROL_MODE; auto control_mode_response = this->vehicle_control_client->async_send_request(this->vehicle_control_request); if (rclcpp::spin_until_future_complete(this, control_mode_response) == -- 2.47.2 From 680b7a2792f51490263cb54f5468e976268592b3 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:20:40 +0200 Subject: [PATCH 21/43] add service callback --- src/drone_controls/src/PositionChanger.cpp | 25 ++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 22b0cfff..d2bb66f6 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -39,10 +39,10 @@ public: 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)); - + 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->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"); @@ -57,7 +57,8 @@ public: 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); + 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)); if (rclcpp::spin_until_future_complete(this, control_mode_response) == rclcpp::FutureReturnCode::SUCCESS) @@ -69,6 +70,22 @@ public: RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control"); } } + + void vehicle_control_service_callback(rclcpp::Client::SharedFuture future) + { + auto status = future.wait_for(1s); + if (status == std::future_status::ready) + { + // uncomment below line if using Empty() message + // RCLCPP_INFO(this->get_logger(), "Result: success"); + // comment below line if using Empty() message + RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", future.get()->success); + } + else + { + RCLCPP_INFO(this->get_logger(), "Service In-Progress..."); + } + } void send_trajectory_message() { this->trajectory_request->x = this->current_speed_x; -- 2.47.2 From 06f833bda0bb825f539b1bdafef3097710bc7340 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:21:20 +0200 Subject: [PATCH 22/43] remove non callback --- src/drone_controls/src/PositionChanger.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index d2bb66f6..1858271e 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -59,16 +59,6 @@ public: 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)); - - if (rclcpp::spin_until_future_complete(this, control_mode_response) == - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", control_mode_response.get()->success); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to call service to set vehicle control"); - } } void vehicle_control_service_callback(rclcpp::Client::SharedFuture future) -- 2.47.2 From 8700014e11dbc30b38dc9d5620665fbaf3ea0331 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:25:06 +0200 Subject: [PATCH 23/43] add callback to trajectory message service call --- src/drone_controls/src/PositionChanger.cpp | 24 +++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 1858271e..4f9ab0d1 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -66,9 +66,6 @@ public: auto status = future.wait_for(1s); if (status == std::future_status::ready) { - // uncomment below line if using Empty() message - // RCLCPP_INFO(this->get_logger(), "Result: success"); - // comment below line if using Empty() message RCLCPP_INFO(this->get_logger(), "Vehicle control mode set result: %d", future.get()->success); } else @@ -76,13 +73,26 @@ public: 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() { - this->trajectory_request->x = this->current_speed_x; - this->trajectory_request->y = this->current_speed_y; - this->trajectory_request->z = this->current_z_speed; + 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_z_speed; this->trajectory_request->yaw = this->current_yaw; - auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request); + auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_requeststd::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); if (rclcpp::spin_until_future_complete(this, trajectory_response) == rclcpp::FutureReturnCode::SUCCESS) -- 2.47.2 From 952620c0a8eff25df4bc182add7d37dad75c49ca Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:28:08 +0200 Subject: [PATCH 24/43] fixes --- src/drone_controls/src/PositionChanger.cpp | 30 +++++++++++----------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 4f9ab0d1..23ac7d5c 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -94,15 +94,15 @@ public: this->trajectory_request->yaw = this->current_yaw; auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_requeststd::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); - if (rclcpp::spin_until_future_complete(this, trajectory_response) == - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_INFO(this->get_logger(), "Trajectory set result: %d", trajectory_response.get()->success); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to call service to set trajectory"); - } + // if (rclcpp::spin_until_future_complete(this, trajectory_response) == + // rclcpp::FutureReturnCode::SUCCESS) + // { + // RCLCPP_INFO(this->get_logger(), "Trajectory set result: %d", trajectory_response.get()->success); + // } + // else + // { + // RCLCPP_ERROR(this->get_logger(), "Failed to call service to set trajectory"); + // } } /** @@ -215,10 +215,10 @@ private: float lidar_sensor_values[4]; // last distance measured by the lidar sensors float lidar_imu_readings[4]; // last imu readings from the lidar sensors - float currrent_yaw = 0; // in radians - float current_x_speed = 0; - float current_y_speed = 0; - float current_z_speed = 0; + 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}; @@ -230,13 +230,13 @@ private: if (!rclcpp::ok()) { RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); - return 0; + return; } RCLCPP_INFO(this->get_logger(), "service not available, waiting again..."); } } }; -; + int main(int argc, char *argv[]) { rclcpp::init(argc, argv); -- 2.47.2 From 40d3d42323f3f412611d89dcfc1a16d0fca2e074 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:29:22 +0200 Subject: [PATCH 25/43] typo --- src/drone_controls/src/PositionChanger.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 23ac7d5c..ad7e5a38 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -90,9 +90,9 @@ public: { 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_z_speed; + this->trajectory_request->values[2] = this->current_speed_z; this->trajectory_request->yaw = this->current_yaw; - auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_requeststd::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); + auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request,std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); // if (rclcpp::spin_until_future_complete(this, trajectory_response) == // rclcpp::FutureReturnCode::SUCCESS) -- 2.47.2 From 66c0591beda5e2b1630ac3f9f24cb244c8ae007d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:32:07 +0200 Subject: [PATCH 26/43] install target --- src/drone_controls/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drone_controls/CMakeLists.txt b/src/drone_controls/CMakeLists.txt index b240af43..4eb03b6f 100644 --- a/src/drone_controls/CMakeLists.txt +++ b/src/drone_controls/CMakeLists.txt @@ -17,6 +17,12 @@ 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) -- 2.47.2 From e7dc035a8370a5cd1c672819259d7e7f61b59cb9 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:33:45 +0200 Subject: [PATCH 27/43] change default control mode to velocity --- src/drone_controls/src/PositionChanger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index ad7e5a38..4fb5b7cd 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -22,7 +22,7 @@ #define MIN_DISTANCE 1.0 // in meters -#define DEFAULT_CONTROL_MODE 4 // default velocity control bitmask +#define DEFAULT_CONTROL_MODE 16 // default velocity control bitmask using namespace std::chrono_literals; -- 2.47.2 From 4532e33c7f28915acc948206578c5acf468ad5b0 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:37:27 +0200 Subject: [PATCH 28/43] add response --- src/drone_controls/src/PositionChanger.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 4fb5b7cd..ac1c4ebd 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -92,7 +92,7 @@ public: 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; - auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request,std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); + auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); // if (rclcpp::spin_until_future_complete(this, trajectory_response) == // rclcpp::FutureReturnCode::SUCCESS) @@ -168,8 +168,9 @@ public: { 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 - this->current_yaw += request->angle * (M_PI / 180.0); // get the angle in radians + this->current_yaw += (request->angle % 360) * (M_PI / 180.0); // get the angle in radians get_x_y_with_rotation(request->front_back, request->left_right, this->current_yaw, &this->current_speed_x, &this->current_speed_y); + response->success = true; } /** @@ -215,7 +216,7 @@ private: float lidar_sensor_values[4]; // 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_yaw = 0; // in radians float current_speed_x = 0; float current_speed_y = 0; float current_speed_z = 0; -- 2.47.2 From 1e0acb8e7a7dac2544fbb4ef24b21802c2b00a37 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:38:39 +0200 Subject: [PATCH 29/43] add log on handle request --- src/drone_controls/src/PositionChanger.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index ac1c4ebd..cb961b29 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -170,6 +170,8 @@ public: // TODO add check_move_direction_allowed results to this calculation this->current_yaw += (request->angle % 360) * (M_PI / 180.0); // get the angle in radians 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; } -- 2.47.2 From 3542fd3adeaf57a06cdc76e9e92efa0ff6c235e6 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:39:26 +0200 Subject: [PATCH 30/43] modulo float --- src/drone_controls/src/PositionChanger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index cb961b29..e6a2e13b 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -168,7 +168,7 @@ public: { 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 - this->current_yaw += (request->angle % 360) * (M_PI / 180.0); // get the angle in radians + this->current_yaw += (request->angle % 360.0) * (M_PI / 180.0); // get the angle in radians 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(); -- 2.47.2 From 89a36829f74c1e20d08a4c4e40a9cfa59d9bc627 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:40:07 +0200 Subject: [PATCH 31/43] cast modulo to float --- src/drone_controls/src/PositionChanger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index e6a2e13b..9873a94d 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -168,7 +168,7 @@ public: { 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 - this->current_yaw += (request->angle % 360.0) * (M_PI / 180.0); // get the angle in radians + this->current_yaw += (request->angle % (float)360.0) * (M_PI / 180.0); // get the angle in radians 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(); -- 2.47.2 From f5c50f366ffbec9c75e3bb069c7debe31bf02ff8 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:41:35 +0200 Subject: [PATCH 32/43] cast-iron stuff --- src/drone_controls/src/PositionChanger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 9873a94d..fbbaf52b 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -168,7 +168,7 @@ public: { 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 - this->current_yaw += (request->angle % (float)360.0) * (M_PI / 180.0); // get the angle in radians + this->current_yaw += (float)(((double)request->angle % 360.0) * (M_PI / 180.0)); // get the angle in radians 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(); -- 2.47.2 From d59da4f5ac33ed1304aa47ada2bd001f08f23922 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:42:59 +0200 Subject: [PATCH 33/43] add checking of request angle --- src/drone_controls/src/PositionChanger.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index fbbaf52b..4ad2f307 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -168,7 +168,13 @@ public: { 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 - this->current_yaw += (float)(((double)request->angle % 360.0) * (M_PI / 180.0)); // get the angle in radians + 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 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(); -- 2.47.2 From 47ff1354225a05fb9563ded90e8a01d3788981cd Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:50:32 +0200 Subject: [PATCH 34/43] add converting control mode --- src/drone_controls/src/PositionChanger.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 4ad2f307..36ffc25a 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -24,6 +24,9 @@ #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 @@ -92,6 +95,7 @@ public: 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); auto trajectory_response = this->trajectory_client->async_send_request(this->trajectory_request, std::bind(&PositionChanger::trajectory_message_callback, this, std::placeholders::_1)); // if (rclcpp::spin_until_future_complete(this, trajectory_response) == -- 2.47.2 From f356f45c26765d00cbec69f0c70025ea0fbedbef Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:53:03 +0200 Subject: [PATCH 35/43] add up down handling --- src/drone_controls/src/PositionChanger.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 36ffc25a..30051b50 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -179,6 +179,7 @@ public: 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(); -- 2.47.2 From a491269ace111fe536ff6972d952764cfd6ea6a3 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:55:35 +0200 Subject: [PATCH 36/43] log of sending message --- src/drone_controls/src/PositionChanger.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 30051b50..55191e6c 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -96,6 +96,7 @@ public: 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)); // if (rclcpp::spin_until_future_complete(this, trajectory_response) == -- 2.47.2 From b5b2da909d65370293c8b82b651fa1de5adf0ccf Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 12:57:50 +0200 Subject: [PATCH 37/43] change adding of angle to setting of angle in px4controller, the angle is already added in the positionchanger --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index fbad0885..f1cca593 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -202,7 +202,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; -- 2.47.2 From dc5138cb8f2c0284059af7fbcaed3d5e63e0845d Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 14:19:51 +0200 Subject: [PATCH 38/43] change px4controller add velocity to set velocity --- src/px4_connection/src/px4_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index f1cca593..e7a428d1 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -189,7 +189,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]); } -- 2.47.2 From b94007eabccd3d6a1e14439eb556983e5a1be614 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 15:49:29 +0200 Subject: [PATCH 39/43] add handling collision weights --- src/drone_controls/src/PositionChanger.cpp | 75 +++++++++++++++++----- 1 file changed, 60 insertions(+), 15 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index 55191e6c..ff8207ff 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -25,7 +25,7 @@ #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))) +#define PX4_CONTROLLER_CONTROL_MODE(x) ((x) == 4 ? 1 : ((x) == 16 ? 2 : ((x) == 32 ? 3 : -1))) using namespace std::chrono_literals; @@ -91,6 +91,7 @@ public: } 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; @@ -98,16 +99,58 @@ public: 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)); + } - // if (rclcpp::spin_until_future_complete(this, trajectory_response) == - // rclcpp::FutureReturnCode::SUCCESS) - // { - // RCLCPP_INFO(this->get_logger(), "Trajectory set result: %d", trajectory_response.get()->success); - // } - // else - // { - // RCLCPP_ERROR(this->get_logger(), "Failed to call service to set trajectory"); - // } + /** + * @brief applies the collision prevention weights to the current x and y speed + * + */ + 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 + { + this->current_speed_x = 0; + } + } + else // moving backward + { + if (this->move_direction_allowed[MOVE_DIRECTION_BACK]) + { + this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_BACK]; + } + else + { + this->current_speed_x = 0; + } + } + 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 + { + this->current_speed_y = 0; + } + } + else // moving left + { + if (this->move_direction_allowed[MOVE_DIRECTION_LEFT]) + { + this->current_speed_y += collision_prevention_weights[MOVE_DIRECTION_LEFT]; + } + else + { + this->current_speed_y = 0; + } + } } /** @@ -125,11 +168,13 @@ public: 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])); + : (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])); + : (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) @@ -228,9 +273,9 @@ private: drone_services::srv::SetTrajectory::Request::SharedPtr trajectory_request; drone_services::srv::SetVehicleControl::Request::SharedPtr vehicle_control_request; - float lidar_sensor_values[4]; // 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 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; -- 2.47.2 From 28f8a79a9a7f0aad6f9c094639a4fd20ce9c0e8b Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 15:52:26 +0200 Subject: [PATCH 40/43] make drone move away from object if it is too close --- src/drone_controls/src/PositionChanger.cpp | 32 ++++++---------------- 1 file changed, 9 insertions(+), 23 deletions(-) diff --git a/src/drone_controls/src/PositionChanger.cpp b/src/drone_controls/src/PositionChanger.cpp index ff8207ff..a540b2a7 100644 --- a/src/drone_controls/src/PositionChanger.cpp +++ b/src/drone_controls/src/PositionChanger.cpp @@ -102,53 +102,39 @@ public: } /** - * @brief applies the collision prevention weights to the current x and y speed + * @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]) + if (!this->move_direction_allowed[MOVE_DIRECTION_FRONT]) { this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_FRONT]; } - else - { - this->current_speed_x = 0; - } } else // moving backward { - if (this->move_direction_allowed[MOVE_DIRECTION_BACK]) + if (!this->move_direction_allowed[MOVE_DIRECTION_BACK]) { this->current_speed_x += collision_prevention_weights[MOVE_DIRECTION_BACK]; } - else - { - this->current_speed_x = 0; - } } if (this->current_speed_y > 0) // moving right { - if (this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) + if (!this->move_direction_allowed[MOVE_DIRECTION_RIGHT]) { - this->current_speed_y += collision_prevention_weights[MOVE_DIRECTION_RIGHT]; - } - else - { - this->current_speed_y = 0; + this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_RIGHT]; } + } else // moving left { - if (this->move_direction_allowed[MOVE_DIRECTION_LEFT]) + if (!this->move_direction_allowed[MOVE_DIRECTION_LEFT]) { - this->current_speed_y += collision_prevention_weights[MOVE_DIRECTION_LEFT]; - } - else - { - this->current_speed_y = 0; + this->current_speed_y = collision_prevention_weights[MOVE_DIRECTION_LEFT]; } } } -- 2.47.2 From f44f1ff9b02f435edd7f20ee5dd97f58d21551de Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 21:27:54 +0200 Subject: [PATCH 41/43] comments --- src/px4_connection/src/px4_controller.cpp | 132 +++++++++++++--------- 1 file changed, 78 insertions(+), 54 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index e7a428d1..1bf94796 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,44 +80,37 @@ 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 + bool start_trajectory = false; // start trajectory when drone is at start position + 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 + const float p0_z = -0.0; // initial height + float p0_x = rho_0 * cos(theta_0); // initial x position + float p0_y = rho_0 * sin(theta_0); // initial y position + 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; @@ -127,9 +120,15 @@ private: 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 +171,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, @@ -232,7 +238,7 @@ private: auto msg = drone_services::msg::DroneArmStatus(); msg.armed = false; arm_status_publisher_->publish(msg); - + response->success = true; } else @@ -280,6 +286,10 @@ private: } } + /** + * @brief sends the latest received attitude setpoint to the drone + * + */ void send_attitude_setpoint() { @@ -306,6 +316,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 +327,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 +353,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 +367,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() @@ -376,7 +399,6 @@ private: { if (current_control_mode == CONTROL_MODE_ATTITUDE) { - // RCLCPP_INFO(this->get_logger(), "Sending attitude setpoint"); send_attitude_setpoint(); } else @@ -387,12 +409,10 @@ 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; @@ -404,15 +424,15 @@ private: } } + /** + * @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,18 +448,22 @@ 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; -- 2.47.2 From bab1c0c3bcf3e1d446bc0495ccd792050f555841 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Thu, 25 May 2023 21:31:31 +0200 Subject: [PATCH 42/43] remove start_trajectory and stuff --- src/px4_connection/src/px4_controller.cpp | 28 ++++------------------- 1 file changed, 4 insertions(+), 24 deletions(-) diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index 1bf94796..ed90e095 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -99,26 +99,18 @@ private: char current_control_mode = CONTROL_MODE_ATTITUDE; // start with attitude control - bool start_trajectory = false; // start trajectory when drone is at start position 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; // initial rho of polar coordinate float theta_0 = 0; // initial theta of polar coordinate - const float p0_z = -0.0; // initial height - float p0_x = rho_0 * cos(theta_0); // initial x position - float p0_y = rho_0 * sin(theta_0); // initial y position + 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; - float local_y = 0; - - uint32_t discrete_time_index = 0; + float local_x = 0; // local position x + float local_y = 0; // local position y std::array q = {0, 0, 0, 0}; // result quaternion @@ -375,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 @@ -418,10 +410,6 @@ private: new_setpoint = false; } } - if (start_trajectory) - { - discrete_time_index++; - } } /** @@ -470,14 +458,6 @@ private: 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) -- 2.47.2 From 8621d17c8e5a008642f6a18b8f739d876569986c Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 26 May 2023 14:48:44 +0200 Subject: [PATCH 43/43] verslag stuff --- .../src/tracker_position.cpp | 22 ++++--------------- src/camera/camera/camera_controller.py | 10 +-------- src/px4_connection/src/px4_controller.cpp | 2 +- 3 files changed, 6 insertions(+), 28 deletions(-) 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/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index ed90e095..7d975056 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -288,7 +288,7 @@ private: // 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; -- 2.47.2