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