diff --git a/.gitignore b/.gitignore index 05b3344..ff55b60 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,9 @@ # Created by https://www.toptal.com/developers/gitignore/api/ros2,visualstudiocode,vim,python,c++,cmake,opencv # Edit at https://www.toptal.com/developers/gitignore?templates=ros2,visualstudiocode,vim,python,c++,cmake,opencv +### OctoMap ### +*.bt + ### C++ ### # Prerequisites *.d diff --git a/drone_test.sh b/drone_test.sh index a5b4238..12ae905 100644 --- a/drone_test.sh +++ b/drone_test.sh @@ -4,10 +4,20 @@ ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 2, ta ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 10., e: 10., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" -ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: -200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 100., e: 100., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 10., e: 10., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 3}" diff --git a/drone_test2.sh b/drone_test2.sh new file mode 100644 index 0000000..ae18df9 --- /dev/null +++ b/drone_test2.sh @@ -0,0 +1,19 @@ +source drone_ws/install/setup.bash + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 2, takeoff_height: 10.}" + + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 100., e: 100., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 10., e: 10., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 200., e: 200., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 0}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 1, n: 0., e: 0., d: -10., vn: 1., ve: 1., vd: 1., vtol_config: 1}" + +ros2 topic pub -t 1 /enqueue_command custom_msgs/msg/Action "{action_type: 3}" + diff --git a/drone_ws/src/custom_msgs/msg/DroneState.msg b/drone_ws/src/custom_msgs/msg/DroneState.msg index 6c681e0..b69e358 100644 --- a/drone_ws/src/custom_msgs/msg/DroneState.msg +++ b/drone_ws/src/custom_msgs/msg/DroneState.msg @@ -20,6 +20,8 @@ uint32 ACRO=12 uint32 STABILIZED=13 uint32 RATTITUDE=14 +string current_command + uint32 flight_mode bool in_air @@ -56,6 +58,11 @@ float64 y float64 z float64 w +# this is also the attitude, as roll, pitch and yaw +float64 roll_deg +float64 pitch_deg +float64 yaw_deg + # this is the angular velocity (refer to)[https://mariogc.com/post/angular-velocity-quaternions/] float64 roll_rad_s float64 pitch_rad_s diff --git a/drone_ws/src/mapping_and_planning/CMakeLists.txt b/drone_ws/src/mapping_and_planning/CMakeLists.txt deleted file mode 100644 index e705491..0000000 --- a/drone_ws/src/mapping_and_planning/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(mapping_and_planning) - -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) -find_package(rclcpp REQUIRED) -find_package(octomap REQUIRED) -find_package(std_msgs REQUIRED) -find_package (Eigen3 3.3 REQUIRED NO_MODULE) -find_package(rosidl_default_generators REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(mapping_and_planning REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Obstacle.msg" - "msg/ObstacleArray.msg" - ) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -add_executable(map_n_plan src/mapping.cpp) - -# set(ENV{octomap_DIR} /home/unhappysquid/octomap/) -include_directories(${OCTOMAP_INCLUDE_DIRS}) -link_directories(${OCTOMAP_LIBRARY_DIRS}) -link_libraries(${OCTOMAP_LIBRARIES}) - - -target_link_libraries(map_n_plan octomap Eigen3::Eigen) -ament_target_dependencies(map_n_plan rclcpp std_msgs octomap geometry_msgs mapping_and_planning) - -target_include_directories(map_n_plan PUBLIC - $ - $) - -target_compile_features(map_n_plan PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 - -install(TARGETS map_n_plan - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # 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) - # 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() - -ament_package() diff --git a/drone_ws/src/mapping_and_planning/LICENSE b/drone_ws/src/mapping_and_planning/LICENSE deleted file mode 100644 index d645695..0000000 --- a/drone_ws/src/mapping_and_planning/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. diff --git a/drone_ws/src/mapping_and_planning/README.md b/drone_ws/src/mapping_and_planning/README.md deleted file mode 100644 index 206c1b6..0000000 --- a/drone_ws/src/mapping_and_planning/README.md +++ /dev/null @@ -1,13 +0,0 @@ -# Mapping and planning package -Contains nodes relevant for mapping and planning -## Dependency -Refer to the `README.md` under the workspace directory (`../..`) - - - - - - - - - diff --git a/drone_ws/src/mapping_and_planning/include/mapping_and_planning/mapping.hpp b/drone_ws/src/mapping_and_planning/include/mapping_and_planning/mapping.hpp deleted file mode 100644 index e96a620..0000000 --- a/drone_ws/src/mapping_and_planning/include/mapping_and_planning/mapping.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#ifndef MAPPING_HPP -#define MAPPING_HPP - -#include -#include -#include -#include -#include -#include -#include - -#include "Eigen/Dense" -#include "octomap/OcTree.h" -#include "octomap/octomap.h" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" -#include "geometry_msgs/msg/polygon.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "mapping_and_planning/msg/obstacle.hpp" -#include "mapping_and_planning/msg/obstacle_array.hpp" - -using namespace std::chrono_literals; -using std::placeholders::_1; -using namespace std; -using namespace Eigen; - -class Planner : public rclcpp::Node{ -public: - Planner(); - octomap::OcTree build_octomap(std::vector boundary, - std::vector detections); -private: - void boundaryCallback(const geometry_msgs::msg::Polygon::SharedPtr msg); - void obstacleCallback(const mapping_and_planning::msg::ObstacleArray::SharedPtr obs); - vector detections_; - vector boundaries_; - MatrixXd drone_frame_; - double octomap_granularity_; - rclcpp::Subscription::SharedPtr bounds_sub_; - rclcpp::Subscription::SharedPtr obs_sub_; -}; - -#endif // MAPPING_HPP \ No newline at end of file diff --git a/drone_ws/src/mapping_and_planning/msg/Obstacle.msg b/drone_ws/src/mapping_and_planning/msg/Obstacle.msg deleted file mode 100644 index b234162..0000000 --- a/drone_ws/src/mapping_and_planning/msg/Obstacle.msg +++ /dev/null @@ -1,6 +0,0 @@ -#The detected obtsacles will be sent in terms of lon (deg), lat(deg), distance and radius -float64 lon -float64 lat -float64 dist -float64 radius - diff --git a/drone_ws/src/mapping_and_planning/msg/ObstacleArray.msg b/drone_ws/src/mapping_and_planning/msg/ObstacleArray.msg deleted file mode 100644 index 542c379..0000000 --- a/drone_ws/src/mapping_and_planning/msg/ObstacleArray.msg +++ /dev/null @@ -1 +0,0 @@ -mapping_and_planning/Obstacle[] obstacles \ No newline at end of file diff --git a/drone_ws/src/mapping_and_planning/package.xml b/drone_ws/src/mapping_and_planning/package.xml deleted file mode 100644 index 905fe52..0000000 --- a/drone_ws/src/mapping_and_planning/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - mapping_and_planning - 0.0.0 - TODO: Package description - unhappysquid - Apache-2.0 - - ament_cmake - - ament_lint_auto - ament_lint_common - - octomap - eigen - geometry_msgs - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/drone_ws/src/mapping_and_planning/src/mapping.cpp b/drone_ws/src/mapping_and_planning/src/mapping.cpp deleted file mode 100644 index 2880b2d..0000000 --- a/drone_ws/src/mapping_and_planning/src/mapping.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "mapping_and_planning/mapping.hpp" - - - -Planner::Planner() - : Node("planner"), drone_frame_(4, 4), octomap_granularity_(3.) { - this->drone_frame_(3, 3) = 1.; - Quaterniond quat(1, 0, 0, 0); - auto rotation = quat.matrix(); - Vector3d translation(10, 20, 30); - this->drone_frame_.block<3, 3>(0, 0) = rotation; - this->drone_frame_.block<3, 1>(0, 3) = translation; - - bounds_sub_ = this->create_subscription( - "mission_boundary", 10, bind(&Planner::boundaryCallback, this, _1)); - - obs_sub_ = this->create_subscription( - "detection", 10, std::bind(&Planner::obstacleCallback, this, _1)); - cout << this->drone_frame_ << endl; - - // Example boundary and detection, refer to the mapping docs for how the map looks like - vector boundary{{-10, 30}, {20, 30}, {20, 10}, - {30, 10}, {30, 0}, {-30, 0}, - {-30, 10}, {-10, 10}}; - - // First detection should be right above the drone, 5 meters away, with radius 2 - // Second on should be to the right of the drone, 5 meters away, with a radius 10. - vector detections{{0.0, M_PI, 5., 2.}, {M_PI, 2*M_PI, 20., 10.}}; - - - octomap::OcTree tree = build_octomap(boundary, detections); - tree.writeBinary("simple_tree.bt"); - cout << "wrote octomap file simple_tree.bt" << endl << endl; - cout << "now you can use octovis to visualize: octovis simple_tree.bt" - << endl; - } - - void Planner::boundaryCallback(const geometry_msgs::msg::Polygon::SharedPtr msg) - { - RCLCPP_INFO(this->get_logger(), "Received a polygon with %ld vertices.", msg->points.size()); - boundaries_.resize(msg->points.size()); - for (size_t i = 0; i < msg->points.size(); ++i) - { - const auto &point = msg->points[i]; - boundaries_[i] = Vector2d(point.x, point.y); - RCLCPP_INFO(this->get_logger(), "Vertex %ld: [x=%.2f, y=%.2f, z=%.2f]", i, point.x, point.y, point.z); - } - } - - void Planner::obstacleCallback(const mapping_and_planning::msg::ObstacleArray::SharedPtr obs){ - RCLCPP_INFO(this->get_logger(), "Received detections."); - detections_.resize(obs->obstacles.size()); - detections_ = obs->obstacles; - } - - octomap::OcTree Planner::build_octomap(std::vector boundary, - std::vector detections) { - octomap::OcTree tree(this->octomap_granularity_); - - // TODO: Fill your code here - // Build the tree, refer to - // https://github.com/OctoMap/octomap/blob/devel/octomap/src/simple_example.cpp - // for an example of how to use octomap The drone's frame of reference is in - // the this->drone_frame matrix (homogenous coords) std::vector is C++'s - // class for an ArrayList, Eigen::Vector2d and Eigen::Vector4d are eigen's - // implementation of linear algebra vectors - // Avoid the keyword `new` or any form of heap utilization, just use the - // stack! If you do use `new`, always remember to use `delete` when - // appropriate to free the memory. - cout << "Boundary :" << endl; - for (auto vec : boundary) { - cout << vec << endl; - } - cout << "Detections :" << endl; - for (auto vec : detections) { - cout << vec << endl; - } - return tree; - } - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/drone_ws/src/planner/LICENSE b/drone_ws/src/planner/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/drone_ws/src/planner/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/drone_ws/src/planner/package.xml b/drone_ws/src/planner/package.xml new file mode 100644 index 0000000..39f367b --- /dev/null +++ b/drone_ws/src/planner/package.xml @@ -0,0 +1,18 @@ + + + + planner + 0.0.0 + TODO: Package description + axia + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/drone_ws/src/planner/planner/__init__.py b/drone_ws/src/planner/planner/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/drone_ws/src/planner/planner/path_planner.py b/drone_ws/src/planner/planner/path_planner.py new file mode 100644 index 0000000..87fd2af --- /dev/null +++ b/drone_ws/src/planner/planner/path_planner.py @@ -0,0 +1,108 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped +from builtin_interfaces.msg import Time +from typing import List, Tuple +from shapely.geometry import Polygon, Point, LineString +import networkx as nx + +class path_planner(Node){ + def __init__(self): + super().__init__('path_planner') + self.boundary_xy: List[Tuple[float, float]] | None = None + self.waypoints_xy: List[Tuple[float, float]] | None = None + self.publisher_ = self.create_publisher(Path, 'planning', 10)#TODO: CHANGE STRING TO WHATEVER i AM MEANT TO OUTPUT + self.boundary_sub = self.create_subscription( + Path,#TODO: SAME HERE + 'mission_boundaries', + self.boundaries_callback, + 10) + self.waypoints_sub = self.create_subscription( + Path, 'mission_waypoints', self.waypoints_callback, 10) + + def boundaries_callback(self, boundaries: Path)-> None: + """Save boundary vertices""" + self.boundary_xy = [(p.pose.position.x, p.pose.position.y) for p in boundaries.poses] + self.get_logger().info(f'Received mission boundaries') + self.plan() + + def waypoints_callback(self, waypoints: Path)-> None: + """Save way‑points""" + self.waypoints_xy = [(p.pose.position.x, p.pose.position.y) for p in waypoints.poses] + self.get_logger().info(f'Received {len(self.waypoints_xy)} way‑points') + self.plan() + + def plan(self)-> None: + if self.boundary_xy is None or self.waypoints_xy is None: + return # haven’t got both yet + try: + path_xy = self.build_graph( + self.boundary_xy, + self.waypoints_xy, + start=(0.0, 0.0) # drone’s take‑off point in mission_origin frame TODO: potentially change that + ) + except Exception as e: + self.get_logger().error(f'Planner failed: {e}') + return + + self.publish_path(path_xy) + + def build_graph(self, + boundary: List[Tuple[float, float]], + waypoints: List[Tuple[float, float]], + start: Tuple[float, float] + )-> List[Tuple[float, float]]: + + """ Build 2D Visibility Graph """ + #TODO: CREATE BOUNDARY POLYGON (SELF.BOUNDARY) WITH SHAPELY + #TODO: ENSURE EACH WP INSIDE POLYGON + #TODO: ASSEMBLE NODE SET + #TODO: BUILD VISIBILITY EDGES WITH SHAPELY + #TODO: BUILD GRAPH WITH NETWORKX + + def publish_path(self, full_path: List[Tuple[float, float]]) -> None: + """ + Convert a list of (x, y) tuples into nav_msgs/Path and publish it. + • Z is fixed to 0.0 + • Orientation is the identity quaternion (w = 1) + """ + path_msg = Path() + path_msg.header.frame_id = 'map' # <- change to your global frame + path_msg.header.stamp = self.get_clock().now().to_msg() + + for (x, y) in full_path: + pose = PoseStamped() + pose.header = path_msg.header # same frame & stamp + + pose.pose.position.x = float(x) + pose.pose.position.y = float(y) + pose.pose.position.z = 0.0 + + # Identity quaternion (no rotation): (x, y, z, w) = (0, 0, 0, 1) + pose.pose.orientation.w = 1.0 + + path_msg.poses.append(pose) + + self.path_pub.publish(path_msg) + self.get_logger().info(f'Published path with {len(path_msg.poses)} poses') +} + + +def main(args=None): + rclpy.init(args=args) + + node = VisibilityGraphPlanner() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/drone_ws/src/planner/resource/planner b/drone_ws/src/planner/resource/planner new file mode 100644 index 0000000..e69de29 diff --git a/drone_ws/src/planner/setup.cfg b/drone_ws/src/planner/setup.cfg new file mode 100644 index 0000000..2697782 --- /dev/null +++ b/drone_ws/src/planner/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/planner +[install] +install_scripts=$base/lib/planner diff --git a/drone_ws/src/planner/setup.py b/drone_ws/src/planner/setup.py new file mode 100644 index 0000000..9edf8ad --- /dev/null +++ b/drone_ws/src/planner/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'planner' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='axia', + maintainer_email='alexianatenin.ans@gmail.com', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/drone_ws/src/planner/test/test_copyright.py b/drone_ws/src/planner/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/drone_ws/src/planner/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/drone_ws/src/planner/test/test_flake8.py b/drone_ws/src/planner/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/drone_ws/src/planner/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/drone_ws/src/planner/test/test_pep257.py b/drone_ws/src/planner/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/drone_ws/src/planner/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/drone_ws/src/px4_controller/src/drone_node.cpp b/drone_ws/src/px4_controller/src/drone_node.cpp index d1e167e..08a7349 100644 --- a/drone_ws/src/px4_controller/src/drone_node.cpp +++ b/drone_ws/src/px4_controller/src/drone_node.cpp @@ -1,9 +1,12 @@ #include +#include #include #include #include #include #include +#include +#include #include #include #include @@ -25,14 +28,38 @@ using namespace std::chrono_literals; +// UTILITY FUNCTIONS +template +std::string string_format(const std::string &format, Args... args) { + int size_s = std::snprintf(nullptr, 0, format.c_str(), args...) + + 1; // Extra space for '\0' + if (size_s <= 0) { + throw std::runtime_error("Error during formatting."); + } + auto size = static_cast(size_s); + std::unique_ptr buf(new char[size]); + std::snprintf(buf.get(), size, format.c_str(), args...); + return std::string(buf.get(), + buf.get() + size - 1); // We don't want the '\0' inside +} + +/* Returns the smallest angle between 2 angles, in degrees. + * */ +double smallestAngle(double currentAngle, double targetAngle) { + double bigger = std::max(currentAngle, targetAngle); + double smaller = std::min(currentAngle, targetAngle); + return std::min(bigger - smaller, smaller + 360. - bigger); +} + class OffboardNode : public rclcpp::Node { public: - OffboardNode() : Node("offboard_controller") { + // TODO: Add parameters as parameters + OffboardNode() : Node("offboard_controller"), qc_tolerance(1.) { // tf2 stuff tf_world_broadcaster = std::make_shared(this); - this->make_world_frame(); + this->setup_world_frame(); tf_drone_broadcaster = std::make_unique(*this); @@ -40,6 +67,8 @@ class OffboardNode : public rclcpp::Node { // MAVSDK stuff mavsdk = std::make_unique(mavsdk::Mavsdk::Configuration{ mavsdk::Mavsdk::ComponentType::GroundStation}); + + // TODO: Modify the connection url when connected to the PixHawk mavsdk::ConnectionResult con_res = mavsdk->add_any_connection("udp://:14540"); @@ -59,8 +88,9 @@ class OffboardNode : public rclcpp::Node { mavsdk_telem = std::make_unique(sys.value()); mavsdk_action = std::make_unique(sys.value()); mavsdk_offboard = std::make_unique(sys.value()); - const mavsdk::Offboard::VelocityNedYaw stay{}; - mavsdk_offboard->set_velocity_ned(stay); + // const mavsdk::Offboard::VelocityNedYaw stay{}; + mavsdk::Offboard::PositionNedYaw stay_pos{}; + mavsdk_offboard->set_position_ned(stay_pos); this->setup_monitoring(); @@ -73,10 +103,11 @@ class OffboardNode : public rclcpp::Node { std::bind(&OffboardNode::enqueue_topic_callback, this, std::placeholders::_1)); - pop_sub = this->create_subscription( - "pop_command", 10, - std::bind(&OffboardNode::dequeue_topic_callback, this, - std::placeholders::_1)); + // TODO: DECOMMISSIONED FOR NOW, COME BACK TO THIS LATER + // pop_sub = this->create_subscription( + // "cancel_command", 10, + // std::bind(&OffboardNode::cancel_command_topic_callback, this, + // std::placeholders::_1)); this->timer_ = this->create_wall_timer( 100ms, std::bind(&OffboardNode::monitoring_callback, this)); @@ -86,35 +117,70 @@ class OffboardNode : public rclcpp::Node { } private: - // This only makes sense in the context of the action queue + /* Objects that represent a command. This defines a common interface and has + * utility functions + * */ struct Command { - enum FlightMode { - QC = custom_msgs::msg::DroneState::MC, - FW = custom_msgs::msg::DroneState::FW - }; - - struct GoToTuple { - GoToTuple(double target_n, double target_e, double target_d, - double target_vn, double target_ve, double target_vd) { - n = target_n; - e = target_e; - d = target_d; - vn = target_vn; - ve = target_ve; - vd = target_vd; - } - double n, e, d, vn, ve, vd; - }; - // Returns true if its done, false otherwise + // both perform tick and cancel_command return true if they are done, false + // otherwise + + /* Performs one tick of the command. This is for the command to be + * non-blocking + * */ virtual bool perform_tick(OffboardNode &on) = 0; - virtual bool is_goto() { return false; } - virtual std::optional get_goto_tuple() { return std::nullopt; } - virtual std::optional get_desired_fm() { return std::nullopt; } + /* This is also one tick for the same reason as above, but is supposed to + * handel cancelations properly. FOR NOW, DO NOT DO THIS (OUT OF SCOPE) + * */ + virtual bool cancel_command(OffboardNode &on) { + RCLCPP_INFO(on.get_logger(), "This command is not cancelable"); + return perform_tick(on); + } + /* Returns a string that describes the command. For debugging purposes. + * */ + virtual std::string get_description() = 0; + + /* Function that transforms the this command to adapt to the new_command + * This is really only relevant for FW commands as they need to change + * behavour if the new_command is also FW (ex: need to stay in FW and not + * transition after the goal is reached) + * */ + virtual void mutate(Command *new_command) {} virtual ~Command() {} + + protected: + /* Sends a way point command, over 2 ticks if not in offboard mode, over 1 + * tick otherwise. returns true when sending is done (not when reached + * destination) If you don't want control over a parameter (like yaw), set + * it to NAN + * */ + bool send_offboard_goto(OffboardNode &node, double north, double east, + double down, double yaw) { + mavsdk::Offboard::PositionNedYaw pos; + pos.north_m = north; + pos.east_m = east; + pos.down_m = down; + pos.yaw_deg = yaw; + + auto result = node.mavsdk_offboard->set_position_ned(pos); + if (node.state.flight_mode != custom_msgs::msg::DroneState::OFFBOARD) { + node.mavsdk_offboard->start(); + return false; + } + return true; + } + + double distance_from_target(OffboardNode &node, double target_n, + double target_e, double target_d) { + return sqrt(pow((node.state.n - target_n), 2) + + pow((node.state.e - target_e), 2) + + pow((node.state.d - target_d), 2)); + } }; + /* Command responsible for taking off to a certain height. + * */ struct TakeOff : Command { - TakeOff(double height) { this->target_height = height; } + TakeOff(double height) : target_height(height) {} virtual bool perform_tick(OffboardNode &node) override { if (node.state.in_air && @@ -134,12 +200,18 @@ class OffboardNode : public rclcpp::Node { return false; } + virtual std::string get_description() override { + return string_format("Taking off: target height %lf", target_height); + } + ~TakeOff() {} private: double target_height; }; + /* Command responsible for landing. + * */ struct Land : Command { Land() {} @@ -154,144 +226,358 @@ class OffboardNode : public rclcpp::Node { return false; } + virtual std::string get_description() override { + return string_format("Landing"); + } + ~Land() {} }; - struct GoTo : Command { - GoTo(double n, double e, double d, double vn, double ve, double vd, - FlightMode fm) { + /* Command responsible for going to a destination in QC mode. + * */ + struct GoToQC : Command { + GoToQC(double n, double e, double d) + : target_n(n), target_e(e), target_d(d) {} - target_n = n; - target_e = e; - target_d = d; + virtual bool perform_tick(OffboardNode &node) override { + double dist_from_target = + distance_from_target(node, target_n, target_e, target_d); + if (dist_from_target >= node.qc_tolerance && !goto_command_sent) { + goto_command_sent = + send_offboard_goto(node, target_n, target_e, target_d, NAN); + } - target_vn = vn; - target_ve = ve; - target_vd = vd; + return dist_from_target < node.qc_tolerance; + } - desired_mode = fm; + virtual std::string get_description() override { + return string_format("Going to %lf, %lf, %lf in QC", target_n, target_e, + target_d); } - virtual bool is_goto() override { return true; } + virtual bool cancel_command(OffboardNode &node) override { return true; } + + ~GoToQC() {} + double target_n, target_e, target_d; + bool goto_command_sent = false; + }; - virtual std::optional get_goto_tuple() override { - return std::optional(GoToTuple( - target_n, target_e, target_d, target_vn, target_ve, target_vd)); - }; + /* Command responsible for going to a destination in FW mode. This needs to + * adapt it's behabiour w.r.t. the next command. All new commands start as + * SOLE (alone). They then transition to the appropriate type of FW Command if + * need to. + * */ + struct GoToFW : Command { - virtual std::optional get_desired_fm() override { - return std::optional(desired_mode); - } + GoToFW(double n, double e, double d) + : target_n(n), target_e(e), target_d(d) {} - // TODO: Finish implementing velocity and yaw control virtual bool perform_tick(OffboardNode &node) override { - if (!start_pos_recorded) { - start_pos_recorded = true; - start_n = node.state.n; - start_e = node.state.e; - start_d = node.state.d; + bool res; + switch (fw_command_type) { + case TypeFW::SOLE: + res = sole_flight(node); + break; + case TypeFW::STARTING_FW: + res = starting_fw_flight(node); + break; + case TypeFW::MIDDLE_FW: + res = middle_fw_flight(node); + break; + case TypeFW::END_FW: + res = end_fw_flight(node); + break; } + return res; + } - // switch to offboard if not already - if (node.state.flight_mode != custom_msgs::msg::DroneState::OFFBOARD) { - node.mavsdk_offboard->start(); - } + virtual std::string get_description() override { + return string_format( + "Going to %lf, %lf, %lf in FW (FW Type: %s | FW Stage %s)", target_n, + target_e, target_d, get_type_str().c_str(), + get_lifetime_str().c_str()); + } - // send transition command if need to and if not sent already - if (node.state.vtol_state != desired_mode && !transtion_order_sent) { - mavsdk::Action::Result res; - if (desired_mode == FlightMode::QC) { - res = node.mavsdk_action->transition_to_multicopter(); - } else { - res = node.mavsdk_action->transition_to_fixedwing(); + virtual void mutate(Command *new_command) override { + + GoToFW *command_ptr = dynamic_cast(new_command); + if (command_ptr) { + command_ptr->adapt(); + switch (fw_command_type) { + case TypeFW::SOLE: + fw_command_type = TypeFW::STARTING_FW; + break; + case TypeFW::STARTING_FW: + std::cout << "THIS COMMAD IS STARTING_FW AND AT THE END OF THE " + "QUEUE, IMPOSSIBLE!! EXITING..." + << std::endl; + exit(2); + break; + case TypeFW::MIDDLE_FW: + std::cout << "THIS COMMAD IS MIDDLE_FW AND AT THE END OF THE " + "QUEUE, IMPOSSIBLE!! EXITING..." + << std::endl; + exit(3); + break; + case TypeFW::END_FW: + fw_command_type = TypeFW::MIDDLE_FW; + break; } - if (res == mavsdk::Action::Result::Success) - transtion_order_sent = true; - return false; } + } - double dist_from_target = sqrt(pow((node.state.n - target_n), 2) + - pow((node.state.e - target_e), 2) + - pow((node.state.d - target_d), 2)); - - // if above tolerance and command to go somewhere wasnt sent already - if (((desired_mode == FlightMode::QC && dist_from_target >= 1.) || - (desired_mode == FlightMode::FW && dist_from_target >= 10.)) && - !goto_order_sent) { - mavsdk::Offboard::PositionNedYaw pos; - pos.north_m = target_n; - pos.east_m = target_e; - pos.down_m = target_d; - - pos.yaw_deg = NAN; - // If there is a next goto action - if (node.command_deque.size() >= 1 && - node.command_deque[0]->is_goto()) { - - // if next goto action is in FW and we're in QC: setup yaw and - // actually over shoot the point so you dont slow down before trying to transition - if (desired_mode == FlightMode::QC && - node.command_deque[0]->get_desired_fm().value() == - FlightMode::FW) { - - double overshoot_scale = 1.1; - pos.north_m = start_n + (target_n - start_n) * overshoot_scale; - pos.east_m = start_e + (target_e - start_e) * overshoot_scale; - - double next_n, next_e, d_n, d_e; - GoToTuple next_goto = - node.command_deque[0]->get_goto_tuple().value(); - next_n = next_goto.n; - next_e = next_goto.e; - - d_n = next_n - target_n; - d_e = next_e - target_e; - - double angle = atan2(d_e, d_n) * 180. / M_PI; - // atan2 gives -180 to 180 while we want 0 to 360 for yaw control - if (angle < 0) - angle = 360. + angle; - - pos.yaw_deg = angle; - RCLCPP_INFO(node.get_logger(), - "next point is in FW, we're in QC, it is at (%f,%f) " - "from current target, yaw needed is %f", - d_n, d_e, pos.yaw_deg); - } + private: + /* Describes the different types of FW commands in a command. Very much like + * POS Tagging START_FW: Start of a sequence of FW commands, responsible for + * starting the sequence (fix heading and transition) and reaching the + * destination. MIDDLE_FW: Middle of a sequence of FW commands, responsible + * for stopping when destination is reached. END_FW: End of a sequence of FW + * commands, responsible for untransitionning slightly before reaching the + * destination, then stop when destination is reached. SOLE: Unique FW + * command, responsible for both parts of STARTING_FW behaviour and END_FW + * behaviour at the start and end respectively. + * */ + enum class TypeFW { SOLE, STARTING_FW, MIDDLE_FW, END_FW }; + std::string get_type_str() { + switch (fw_command_type) { + case TypeFW::SOLE: + return "SOLE"; + case TypeFW::STARTING_FW: + return "START"; + case TypeFW::MIDDLE_FW: + return "MIDDLE"; + case TypeFW::END_FW: + return "END"; + } + } + /* Make this command the end of a FW sequence, only called if previous + * command is FW and gets mutated + * */ + void adapt() { fw_command_type = TypeFW::END_FW; } + + /* Behaviour needs to once again change w.r.t. what's been done already + * A task should only be complete once its done with its STOP stage. + * */ + enum class LifetimeStage { BEGIN, ONGOING, STOP }; + std::string get_lifetime_str() { + switch (current_stage) { + case LifetimeStage::BEGIN: + return "BEGIN"; + case LifetimeStage::ONGOING: + return "ONGOING"; + case LifetimeStage::STOP: + return "STOP"; + } + } + + /* A function which alligns the heading of the drone with the target + * destination returns true when at the appropriate heading only. + * */ + bool fix_heading(OffboardNode &node) { + double desired_heading = + std::atan2(target_e - node.state.e, target_n - node.state.n) * 180. / + M_PI; + desired_heading = + (desired_heading >= 0) ? desired_heading : 360. + desired_heading; + double current_heading = node.state.yaw_deg; + std::cout << "Desired : " << desired_heading + << ", Current : " << current_heading << std::endl; + if (smallestAngle(desired_heading, current_heading) < 5.0) { + return true; + } + + if (!yaw_fixing_command_sent) { + yaw_fixing_command_sent = send_offboard_goto( + node, node.state.n, node.state.e, node.state.d, desired_heading); + } + + return false; + } + + bool send_waypoint(OffboardNode &node) { + if (!goto_command_sent) { + goto_command_sent = + send_offboard_goto(node, target_n, target_e, target_d, NAN); + } + return goto_command_sent; + } + + /* A function which sends the waypoint and transition commands. Returns true + * when those commands are successfully sent + * */ + bool send_waypoint_transition(OffboardNode &node) { + goto_command_sent = send_waypoint(node); + double current_speed = + sqrt(pow(node.state.vn, 2) + pow(node.state.ve, 2)); + if (current_speed >= 5. && goto_command_sent && + !transition_command_sent) { + auto res = node.mavsdk_action->transition_to_fixedwing(); + transition_command_sent = + (res == mavsdk::Action::Result::Success) && + node.state.vtol_state == custom_msgs::msg::DroneState::FW; + } + return goto_command_sent && transition_command_sent; + } + + /* A function which sends a untransition command. Returns true when the + * command is successful. + * */ + bool send_untransition(OffboardNode &node) { + return node.mavsdk_action->transition_to_multicopter() == + mavsdk::Action::Result::Success; + } + + bool is_within_from_target(OffboardNode &node, double meters) { + return distance_from_target(node, target_n, target_e, target_d) < meters; + } + + bool sole_flight(OffboardNode &node) { + // FIX HEADING + // ONCE FIXED SET WAYPOINT THEN TRANSITION + // THEN PRE-UNTRANSITION BEFORE REACING END + // BOOL -> RETURN TRUE WHEN IN QC AND REACHED + switch (current_stage) { + case LifetimeStage::BEGIN: + if (fix_heading(node)) + current_stage = LifetimeStage::ONGOING; + return false; + break; + case LifetimeStage::ONGOING: + if (send_waypoint_transition(node)) + current_stage = LifetimeStage::STOP; + return false; + break; + case LifetimeStage::STOP: + if (is_within_from_target(node, 50.) && !untransition_command_sent) { + untransition_command_sent = send_untransition(node); + goto_command_sent = false; + goto_command_sent = send_waypoint(node); } + if (is_within_from_target(node, 5.) && untransition_command_sent) + return true; + return false; + break; + } + } - auto res = node.mavsdk_offboard->set_position_ned(pos); - if (res == mavsdk::Offboard::Result::Success) - goto_order_sent = true; + bool starting_fw_flight(OffboardNode &node) { + // FIX HEADING + // ONCE FIXED SET WAYPOINT THEN TRANSITION + // BOOL -> RETURN TRUE WHEN REACHED + switch (current_stage) { + case LifetimeStage::BEGIN: + if (fix_heading(node)) + current_stage = LifetimeStage::ONGOING; + return false; + break; + case LifetimeStage::ONGOING: + if (send_waypoint_transition(node)) + current_stage = LifetimeStage::STOP; + return false; + break; + case LifetimeStage::STOP: + // if () return true; + if (is_within_from_target(node, 5.)) + return true; + return false; + break; } + } - // if within tolerance and in the desired mode, you're done ! - if ((((desired_mode == FlightMode::QC && dist_from_target < 1.) || - (desired_mode == FlightMode::FW && dist_from_target < 10.))) && - node.state.vtol_state == desired_mode) { - // if (node.command_deque.size() == ) { - // node.mavsdk_offboard->stop(); - // } - return true; + bool middle_fw_flight(OffboardNode &node) { + // SET WAYPOINT + // BOOL -> RETURN TRUE WHEN REACHED + switch (current_stage) { + case LifetimeStage::BEGIN: + current_stage = LifetimeStage::ONGOING; + return false; + break; + case LifetimeStage::ONGOING: + if (send_waypoint(node)) + current_stage = LifetimeStage::STOP; + return false; + break; + case LifetimeStage::STOP: + // if () return true; + if (is_within_from_target(node, 5.)) + return true; + return false; + break; } return false; } - ~GoTo() {} + bool end_fw_flight(OffboardNode &node) { + // SET WAYPOINT + // THEN PRE-UNTRANSITION BEFORE REACING END + // BOOL -> RETURN TRUE WHEN IN QC AND REACHED + switch (current_stage) { + case LifetimeStage::BEGIN: + current_stage = LifetimeStage::ONGOING; + return false; + break; + case LifetimeStage::ONGOING: + if (send_waypoint(node)) + current_stage = LifetimeStage::STOP; + return false; + break; + case LifetimeStage::STOP: + if (is_within_from_target(node, 50.) && !untransition_command_sent) { + untransition_command_sent = send_untransition(node); + goto_command_sent = false; + goto_command_sent = send_waypoint(node); + } + if (is_within_from_target(node, 5.) && untransition_command_sent) + return true; + return false; + break; + } + return false; + } - double start_n, start_e, start_d; - double target_n, target_e, target_d, target_vn, target_ve, target_vd; - FlightMode desired_mode; + TypeFW fw_command_type = TypeFW::SOLE; + LifetimeStage current_stage = LifetimeStage::BEGIN; + double target_n, target_e, target_d; - // toggles to not overwhelm the flight controller - bool start_pos_recorded = false; - bool transtion_order_sent = false; - bool goto_order_sent = false; + // FLAGS + bool yaw_fixing_command_sent = false; + bool goto_command_sent = false; + bool transition_command_sent = false; + bool untransition_command_sent = false; }; - // TODO: DO REST OF COMMAND CLASSES + // TODO: IMPLEMENT HOLD + struct Hold : Command {}; + + /* A queue composed object, needed to automatically call mutate on append. + * */ + struct CommandQueue { + + CommandQueue() {} + + int size() { return command_queue.size(); } + + Command *operator[](std::size_t idx) { return command_queue[idx].get(); } + + void push_back(std::unique_ptr c) { + command_queue.push_back(std::move(c)); + } + + void enqueue_command(std::unique_ptr c) { + if (size() > 0) + command_queue[size() - 1]->mutate(c.get()); + push_back(std::move(c)); + } + + void pop_front() { command_queue.pop_front(); } + + private: + std::deque> command_queue = {}; + }; - void make_world_frame() { + /* Sets up the world_frame object (used by RViz). + * */ + void setup_world_frame() { geometry_msgs::msg::TransformStamped t; t.header.stamp = this->get_clock()->now(); t.header.frame_id = "world"; @@ -309,6 +595,9 @@ class OffboardNode : public rclcpp::Node { this->tf_world_broadcaster->sendTransform(t); } + /* Sets up the various mavsdk callbacks necessary to update the state and + * world_frame. + * */ void setup_monitoring() { state.header.frame_id = "world"; @@ -373,6 +662,13 @@ class OffboardNode : public rclcpp::Node { transform.transform.rotation.w = quat.w; }); + mavsdk_telem->subscribe_attitude_euler( + [this](mavsdk::Telemetry::EulerAngle angles) { + state.roll_deg = angles.roll_deg; + state.pitch_deg = angles.pitch_deg; + state.yaw_deg = angles.yaw_deg; + }); + mavsdk_telem->subscribe_attitude_angular_velocity_body( [this](mavsdk::Telemetry::AngularVelocityBody angular_vel) { state.roll_rad_s = angular_vel.roll_rad_s; @@ -432,31 +728,39 @@ class OffboardNode : public rclcpp::Node { }); } + /* Callback to publish the state and world_frame + * */ void monitoring_callback() { state.header.stamp = this->get_clock()->now(); transform.header.stamp = this->get_clock()->now(); state.command_queue_size = command_deque.size(); + if (command_deque.size() == 0) { + state.current_command = "None"; + } else { + state.current_command = command_deque[0]->get_description(); + } this->tf_drone_broadcaster->sendTransform(transform); this->monitoring_pub->publish(state); } + /* Callback to enqueue a new topic in the topic queue. + * */ // TODO: DO THIS void enqueue_topic_callback(const custom_msgs::msg::Action &action) { std::unique_ptr com; switch (action.action_type) { case custom_msgs::msg::Action::HOLD: + // TODO: As in this and break; case custom_msgs::msg::Action::GOTO_ACTION: - OffboardNode::GoTo::FlightMode fm; if (action.vtol_config == custom_msgs::msg::Action::IN_FW) { - fm = OffboardNode::GoTo::FlightMode::FW; - } - if (action.vtol_config == custom_msgs::msg::Action::IN_QC) { - fm = OffboardNode::GoTo::FlightMode::QC; + com = std::make_unique(action.n, action.e, + action.d); + } else if (action.vtol_config == custom_msgs::msg::Action::IN_QC) { + com = std::make_unique(action.n, action.e, + action.d); } - com = std::make_unique( - action.n, action.e, action.d, action.vn, action.ve, action.vd, fm); break; case custom_msgs::msg::Action::LAND: com = std::make_unique(); @@ -464,39 +768,48 @@ class OffboardNode : public rclcpp::Node { case custom_msgs::msg::Action::TAKEOFF_ACTION: com = std::make_unique(action.takeoff_height); break; + default: + std::cout << "ACTION TYPE INVALID!! EXITING..." << std::endl; + exit(1); + break; } - command_deque.push_back(std::move(com)); + command_deque.enqueue_command(std::move(com)); } - void dequeue_topic_callback(const std_msgs::msg::Empty &em) { - if (command_deque.size() > 0) - command_deque.pop_front(); - } + // TODO: DECOMMISIONED, COME BACK TO THIS LATER + // void cancel_command_topic_callback(const std_msgs::msg::Empty &em) { + // if (command_deque.size() > 0) + // want_to_cancel = true; + // } + /* Responsible for running one tick of the top command. + * */ void command_queue_callback() { - // RCLCPP_INFO(this->get_logger(), "Command queue has size %d", - // command_deque.size()); if (command_deque.size() == 0) { - // This is incase we havent reset things because we poped a command before - // it was done or something like that if (state.flight_mode == custom_msgs::msg::DroneState::OFFBOARD) { mavsdk_offboard->stop(); } - if (state.flight_mode != custom_msgs::msg::DroneState::HOLD) { - mavsdk_action->hold(); - } + // This may make it impossible for the Remote controller to take over, + // above should suffice if (state.flight_mode != + // custom_msgs::msg::DroneState::HOLD && state.flight_mode != + // custom_msgs::msg::DroneState::MANUAL) { + // mavsdk_action->hold(); + // } return; } - auto head = std::move(command_deque.front()); - command_deque.pop_front(); + // bool is_done = (want_to_cancel) ? command_deque[0]->cancel_command(*this) + // : command_deque[0]->perform_tick(*this); - bool is_done = head->perform_tick(*this); - - if (!is_done) { - command_deque.push_front(std::move(head)); + bool is_done = command_deque[0]->perform_tick(*this); + if (is_done) { + // want_to_cancel = false; + command_deque.pop_front(); } } + // flight parameters + double qc_tolerance; + // ROS2 related stuff rclcpp::Publisher::SharedPtr monitoring_pub; rclcpp::Subscription::SharedPtr enqueue_sub; @@ -519,7 +832,8 @@ class OffboardNode : public rclcpp::Node { std::unique_ptr mavsdk_offboard; // Command queue - std::deque> command_deque = {}; + CommandQueue command_deque = {}; + // bool want_to_cancel = false; }; int main(int argc, char *argv[]) {