diff --git a/.gitignore b/.gitignore index c29ef59..0d3fc42 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,6 @@ config/simulation.rviz .gitignore .vscode -build \ No newline at end of file +build +install +log \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..c91629c --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(mavswarm2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(Armadillo REQUIRED) +find_package(GSL REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(simulator_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(glog REQUIRED) + +add_executable(mavswarm2 src/quadrotor.cpp) + +ament_target_dependencies(mavswarm2 + rclcpp + geometry_msgs + tf2 + tf2_ros + visualization_msgs + simulator_interfaces +) + +target_link_libraries(mavswarm2 + GSL::gsl + GSL::gslcblas + glog::glog +) + +install( + TARGETS mavswarm2 + DESTINATION lib/${PROJECT_NAME} +) +install( + DIRECTORY launch config cf_description + DESTINATION share/${PROJECT_NAME}/ +) + +include_directories( + include/${PROJECT_NAME} + ${EIGEN3_INCLUDE_DIRS} +) + +ament_package() diff --git a/multi_uav_simulator/cf_description/crazyflie.urdf.xacro b/cf_description/crazyflie.urdf.xacro old mode 100755 new mode 100644 similarity index 59% rename from multi_uav_simulator/cf_description/crazyflie.urdf.xacro rename to cf_description/crazyflie.urdf.xacro index a6aa3f5..9b751c0 --- a/multi_uav_simulator/cf_description/crazyflie.urdf.xacro +++ b/cf_description/crazyflie.urdf.xacro @@ -1,12 +1,12 @@ - + - + - + diff --git a/multi_uav_simulator/cf_description/crazyflie2.dae b/cf_description/crazyflie2.dae old mode 100755 new mode 100644 similarity index 99% rename from multi_uav_simulator/cf_description/crazyflie2.dae rename to cf_description/crazyflie2.dae index c9ad350..81ae2d8 --- a/multi_uav_simulator/cf_description/crazyflie2.dae +++ b/cf_description/crazyflie2.dae @@ -7,7 +7,7 @@ 2016-04-29T11:06:01Z 2016-04-29T11:06:01Z - + Z_UP diff --git a/multi_uav_simulator/config/bigquad_params.yaml b/config/bigquad_params.yaml similarity index 100% rename from multi_uav_simulator/config/bigquad_params.yaml rename to config/bigquad_params.yaml diff --git a/config/crazyflie_params.yaml b/config/crazyflie_params.yaml new file mode 100644 index 0000000..74c5a7e --- /dev/null +++ b/config/crazyflie_params.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + controller_gains: [1.0, 0.5, 0.0015, 0.00075] + # kx: 1 + # kv: 0.5 + # kr: 0.0015 + # komega: 0.00075 + + model: + J: [2.3951e-5, 2.3951e-5, 3.2347e-5] + m: 0.032 + d: 0.08 + ctf: 0.0037 + gravity: 9.81 + diff --git a/multi_uav_simulator/config/custom_rosconsole.conf b/config/custom_rosconsole.conf similarity index 100% rename from multi_uav_simulator/config/custom_rosconsole.conf rename to config/custom_rosconsole.conf diff --git a/config/initial_conditions.yaml b/config/initial_conditions.yaml new file mode 100644 index 0000000..5e56764 --- /dev/null +++ b/config/initial_conditions.yaml @@ -0,0 +1,32 @@ +# define the initial positions of each robot in NWU frame +# change the initial rotation matrix to [1, 0, 0, 0, 1, 0, 0, 0, 1] for initialing the +# drones right side up. +robot_1: + position: [0, 0, -10] + velocity: [0, 0, 0] + rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1] + omega: [0, 0, 0] + +# robot_2: +# position: [-2, -2, -2] +# velocity: [0, 0, 0] +# rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] +# omega: [0, 0, 0] + +# robot_3: +# position: [-1, 1, -2] +# velocity: [0, 0, 0] +# rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] +# omega: [0, 0, 0] + +# robot_4: +# position: [1, -1, -2] +# velocity: [0, 0, 0] +# rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] +# omega: [0, 0, 0] + +# robot_5: +# position: [0, 0, -2] +# velocity: [0, 0, 0] +# rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] +# omega: [0, 0, 0] \ No newline at end of file diff --git a/config/initial_conditions_2.yaml b/config/initial_conditions_2.yaml new file mode 100644 index 0000000..125e283 --- /dev/null +++ b/config/initial_conditions_2.yaml @@ -0,0 +1,77 @@ +# define the initial positions of each robot in NWU frame +# change the initial rotation matrix to [1, 0, 0, 0, 1, 0, 0, 0, 1] for initialing the +# drones right side up. +# "/**/robot_0" matches the node whether it is namespaced (e.g. /mavswarm2/robot_0 +# when started from the launch file) or run bare as /robot_0. A plain "robot_0:" key +# does NOT match a namespaced node, which silently drops these params. +# +# Line formation along +y, spaced 1.0 m, centred symmetrically about y = 0. +/**/robot_0: + ros__parameters: + position: [2.0, -4.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_1: + ros__parameters: + position: [2.0, -3.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_2: + ros__parameters: + position: [2.0, -2.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_3: + ros__parameters: + position: [2.0, -1.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_4: + ros__parameters: + position: [2.0, -0.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_5: + ros__parameters: + position: [2.0, 0.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_6: + ros__parameters: + position: [2.0, 1.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_7: + ros__parameters: + position: [2.0, 2.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_8: + ros__parameters: + position: [2.0, 3.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] + +/**/robot_9: + ros__parameters: + position: [2.0, 4.5, 2.0] + velocity: [0.0, 0.0, 0.0] + rotation: [1.0, 0.0, 0.0, 0.0, -0.9995, -0.0314, 0.0, 0.0314, -0.9995] + omega: [0.0, 0.0, 0.0] diff --git a/cover.gif b/cover.gif deleted file mode 100644 index f34fd00..0000000 Binary files a/cover.gif and /dev/null differ diff --git a/cover.png b/cover.png new file mode 100644 index 0000000..7301e7b Binary files /dev/null and b/cover.png differ diff --git a/cover2.png b/cover2.png new file mode 100644 index 0000000..5ac1323 Binary files /dev/null and b/cover2.png differ diff --git a/geo_controller/CMakeLists.txt b/geo_controller/CMakeLists.txt deleted file mode 100755 index ae04b4b..0000000 --- a/geo_controller/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(geo_controller) - -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - simulator_utils - ) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES geo_controller - CATKIN_DEPENDS message_runtime roscpp -) - -include_directories( - include/${PROJECT_NAME} - ${catkin_INCLUDE_DIRS} -) - -#find_package(simulator_utils) - -#add_executable(geo_controller -# include/geo_controller/controllerImpl.h -# src/controllerImpl.cpp include/geo_controller/definitions.h) - -add_library(${PROJECT_NAME} - src/controllerImpl.cpp - src/definitions.cpp - ) \ No newline at end of file diff --git a/geo_controller/LICENSE b/geo_controller/LICENSE deleted file mode 100755 index 261eeb9..0000000 --- a/geo_controller/LICENSE +++ /dev/null @@ -1,201 +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/geo_controller/include/geo_controller/controllerImpl.h b/geo_controller/include/geo_controller/controllerImpl.h deleted file mode 100755 index 1e38946..0000000 --- a/geo_controller/include/geo_controller/controllerImpl.h +++ /dev/null @@ -1,48 +0,0 @@ -// -// Created by malintha on 11/27/17. -// - -#ifndef PROJECT_TRACKING_CONTROL_H -#define PROJECT_TRACKING_CONTROL_H - -#pragma once - -#include -#include "simulator_utils/simulator_utils.h" -#include "definitions.h" - -using namespace Eigen; - -class ControllerImpl -{ - -public: - ControllerImpl(params_t params_, init_vals_t init_vals_, gains_t gains_, double dt_); - control_out_t get_control(state_space_t ss_, const desired_state_t& desired_s); - -private: - - params_t params; - init_vals_t init_vals; - - double dt; - gains_t gains; - - // derivative operators - dxdt dxdt_; - dxdt d2xdt; - dxdt d3xdt; - dxdt d4xdt; - dxdt db1dt; - dxdt d2b1dt; - dxdt dvdt; - dxdt d2vdt; - - derivatives_t get_derivatives(const desired_state_t& desired_state, const state_space_t& ss); - - - - -}; - -#endif diff --git a/geo_controller/include/geo_controller/definitions.h b/geo_controller/include/geo_controller/definitions.h deleted file mode 100644 index 713a598..0000000 --- a/geo_controller/include/geo_controller/definitions.h +++ /dev/null @@ -1,44 +0,0 @@ -#include - -using namespace Eigen; - -// compute the derivative of a signal -struct dxdt { - double tau; - double ts; - double a1; - double a2; - int order; - Vector3d dot; - Vector3d x_d1; - int it = 1; - - dxdt(); - dxdt(double tau_, double ts_, int order_); - Vector3d calculate(const Vector3d &x); -}; - -typedef struct -{ - Vector3d ex; - Vector3d ev; - Vector3d ea; - Vector3d ej; - Vector3d er; - Vector3d eomega; -} err_t; - -typedef struct { - // derivatives of desired states - Vector3d d_xd; - Vector3d d2_xd; - Vector3d d3_xd; - Vector3d d4_xd; - Vector3d db1; - Vector3d d2b1; - - // derivatives of current velocity - Vector3d dv; - Vector3d d2v; - -} derivatives_t; \ No newline at end of file diff --git a/geo_controller/package.xml b/geo_controller/package.xml deleted file mode 100755 index b67c086..0000000 --- a/geo_controller/package.xml +++ /dev/null @@ -1,69 +0,0 @@ - - - geo_controller - 0.0.0 - The geo_controller package - - - - - malintha - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - message_generation - - - - - - message_runtime - - - - - catkin - roscpp - - std_msgs - roscpp - - std_msgs - roscpp - - std_msgs - simulator_utils - - - - - - - - diff --git a/geo_controller/src/definitions.cpp b/geo_controller/src/definitions.cpp deleted file mode 100644 index d2fe4f9..0000000 --- a/geo_controller/src/definitions.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include - -dxdt::dxdt(double tau_, double ts_, int order_) : tau(tau_), ts(ts_), order(order_) -{ - a1 = (2 * tau - ts) / (2 * tau + ts); - a2 = 2 / (2 * tau + ts); - dot = Vector3d(0, 0, 0); - x_d1 = Vector3d(0, 0, 0); -} - -dxdt::dxdt() {} - -Vector3d dxdt::calculate(const Vector3d &x) -{ - if (it > order) - { - dot = a1 * dot + a2 * (x - x_d1); - } - it++; - x_d1 = x; - return dot; -} diff --git a/geo_controller/src/controllerImpl.cpp b/include/mavswarm2/controller/geometric_controller.hpp similarity index 71% rename from geo_controller/src/controllerImpl.cpp rename to include/mavswarm2/controller/geometric_controller.hpp index 7837a50..ccae68f 100644 --- a/geo_controller/src/controllerImpl.cpp +++ b/include/mavswarm2/controller/geometric_controller.hpp @@ -1,11 +1,44 @@ -#include "controllerImpl.h" +/* + +Copyright (c) 2024 Malintha Fernando (malintha@onmail.com) + +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. + +*/ + +#ifndef GEOMETRIC_CONTROLLER +#define GEOMETRIC_CONTROLLER + +#pragma once + +#include +#include "types.hpp" +#include "definitions.hpp" #include +using namespace Eigen; + namespace su = simulator_utils; -ControllerImpl::ControllerImpl(params_t params_, init_vals_t init_vals_, gains_t gains_, double dt_) - :params(std::move(params_)), init_vals(std::move(init_vals_)), gains(gains_), dt(dt_) +class Geometric_Controller +{ + +public: + +Geometric_Controller(params_t params_, gains_t gains_, double dt_) + :params(std::move(params_)), gains(gains_), dt(dt_) { + // initialize derivative operators dxdt_ = dxdt(0.05, dt, 1); d2xdt = dxdt(0.05, dt, 2); @@ -17,9 +50,10 @@ ControllerImpl::ControllerImpl(params_t params_, init_vals_t init_vals_, gains_t dvdt = dxdt(0.05, dt, 1); d2vdt = dxdt(0.05, dt, 2); + } -control_out_t ControllerImpl::get_control(state_space_t ss, const desired_state_t& desired_s) +control_out_t get_control(state_space_t ss, const desired_state_t& desired_s) { // compute the derivatives of desired state derivatives_t derivatives = get_derivatives(desired_s, ss); @@ -82,7 +116,9 @@ control_out_t ControllerImpl::get_control(state_space_t ss, const desired_state_ return control; } -derivatives_t ControllerImpl::get_derivatives(const desired_state_t& desired_s, const state_space_t& ss) +private: + +derivatives_t get_derivatives(const desired_state_t& desired_s, const state_space_t& ss) { derivatives_t dds; // numerical derivative of desired x @@ -98,4 +134,26 @@ derivatives_t ControllerImpl::get_derivatives(const desired_state_t& desired_s, dds.d2v = d2vdt.calculate(dds.dv); return dds; -} \ No newline at end of file +} + + +params_t params; +init_vals_t init_vals; + +double dt; +gains_t gains; + +// derivative operators +dxdt dxdt_; +dxdt d2xdt; +dxdt d3xdt; +dxdt d4xdt; +dxdt db1dt; +dxdt d2b1dt; +dxdt dvdt; +dxdt d2vdt; + + +}; + +#endif \ No newline at end of file diff --git a/include/mavswarm2/definitions.hpp b/include/mavswarm2/definitions.hpp new file mode 100644 index 0000000..8b415db --- /dev/null +++ b/include/mavswarm2/definitions.hpp @@ -0,0 +1,83 @@ +/* + +Copyright (c) 2024 Malintha Fernando (malintha@onmail.com) + +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. + +*/ + +#include + +using namespace Eigen; + +// compute the derivative of a signal +struct dxdt { + double tau; + double ts; + double a1; + double a2; + int order; + Vector3d dot; + Vector3d x_d1; + int it = 1; + +dxdt(double tau_, double ts_, int order_) : tau(tau_), ts(ts_), order(order_) +{ + a1 = (2 * tau - ts) / (2 * tau + ts); + a2 = 2 / (2 * tau + ts); + dot = Vector3d(0, 0, 0); + x_d1 = Vector3d(0, 0, 0); +} + +dxdt() {} + +Vector3d calculate(const Vector3d &x) +{ + if (it > order) + { + dot = a1 * dot + a2 * (x - x_d1); + } + it++; + x_d1 = x; + return dot; +} + +}; + +typedef struct +{ + Vector3d ex; + Vector3d ev; + Vector3d ea; + Vector3d ej; + Vector3d er; + Vector3d eomega; +} err_t; + +typedef struct { + // derivatives of desired states + Vector3d d_xd; + Vector3d d2_xd; + Vector3d d3_xd; + Vector3d d4_xd; + Vector3d db1; + Vector3d d2b1; + + // derivatives of current velocity + Vector3d dv; + Vector3d d2v; + +} derivatives_t; + + + diff --git a/multi_uav_simulator/src/DynamicsProvider.cpp b/include/mavswarm2/dynamics_provider.hpp similarity index 68% rename from multi_uav_simulator/src/DynamicsProvider.cpp rename to include/mavswarm2/dynamics_provider.hpp index 5958df6..dfe9aa0 100644 --- a/multi_uav_simulator/src/DynamicsProvider.cpp +++ b/include/mavswarm2/dynamics_provider.hpp @@ -1,12 +1,35 @@ -// -// Created by malintha on 7/31/18. -// +/* -#include "DynamicsProvider.h" +Copyright (c) 2024 Malintha Fernando (malintha@onmail.com) + +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. + +*/ + +#ifndef CF_SIMULATOR_DYNAMICSPROVIDER_H +#define CF_SIMULATOR_DYNAMICSPROVIDER_H + +#include +// #include "tf2/transform_datatypes.h" +#include "types.hpp" +#include + +// #include "dynamics_provider.h" #include #include -#include - +// #include "rcutils/logging.h" +#include +using namespace Eigen; /** * The equations and state_space use NED inertial frame. Therefore, the initial values are needed to be * converted from map frame (NWU) to NED frame. And convert them back to map frame before broadcasting. @@ -14,7 +37,11 @@ * Note: the drone is already started upside down in the xacro, matching to the NED. * So no need to convert the rotation matrix and omega. */ -DynamicsProvider::DynamicsProvider(params_t params, init_vals_t init_vals) +class DynamicsProvider { + +public: + +DynamicsProvider(params_t params, init_vals_t init_vals) { this->params = params; this->init_vals = init_vals; @@ -33,57 +60,8 @@ DynamicsProvider::DynamicsProvider(params_t params, init_vals_t init_vals) } -/** - * ODE step for updating the quad state Dynamic equations are obtained from - * Geometric Tracking Control of a Quadrotor UAV on SE(3), Lee et al, (2010) -*/ -int DynamicsProvider::step(double t, const double *y, double *f, void *params) -{ - auto *param = reinterpret_cast(params); - (void) (t); - // copy R values from y - double R_temp[9]; - for (int i = 6, k = 0; i < 15; i++, k++) - { - R_temp[k] = y[i]; - } - Matrix3d R(R_temp); - Vector3d omega(y[15], y[16], y[17]); - - // velocity for the next timestep eq(3) - Vector3d e3(0, 0, 1); - Vector3d v = param->gravity * e3 - (1 / param->mass) * (param->F * R * e3); - - // Rdot for next timestep eq(4) - Matrix3d Rdot = R * simulator_utils::hat(omega); - - // Omegadot for the next timestep eq(5) - Vector3d Omegadot = param->J_inv * (param->M - omega.cross(param->J * omega)); - - // writing values to f - // position of next timestep eq(2) - int k = 0; - for (int i = 3; i < 6; i++, k++) - { - f[k] = y[i]; - } - for (int i = 0; i < 3; i++, k++) - { - f[k] = v[i]; - } - for (int i = 0; i < 9; i++, k++) - { - f[k] = Rdot.data()[i]; - } - for (int i = 0; i < 3; i++, k++) - { - f[k] = Omegadot[i]; - } - return GSL_SUCCESS; -} - -void DynamicsProvider::update(control_out_t control, double sim_time_) +void update(control_out_t control, double sim_time_) { t_start = sim_time; double t_stop = sim_time_; @@ -115,7 +93,7 @@ void DynamicsProvider::update(control_out_t control, double sim_time_) y[k] = state_space.omega[i]; } - gsl_odeiv2_system sys = {DynamicsProvider::step, nullptr, DIMENSIONS, ¶ms}; + gsl_odeiv2_system sys = {step, nullptr, DIMENSIONS, ¶ms}; double t = t_start; double h = 1e-5; while (t < t_stop) @@ -154,16 +132,85 @@ void DynamicsProvider::update(control_out_t control, double sim_time_) { state_space.omega[i] = y[k]; } - // ROS_DEBUG_STREAM(t_stop<<" R: \n" << state_space.R << " \n v: \n" << state_space.velocity); sim_time = t_stop; } -state_space_t DynamicsProvider::get_state() +state_space_t get_state() { return this->state_space; } -void DynamicsProvider::reset_dynamics() { +void reset_dynamics() { gsl_odeiv2_evolve_reset(this->e); } + +private: + const size_t DIMENSIONS = 18; + const gsl_odeiv2_step_type *T = gsl_odeiv2_step_rkf45; + // Check if GSL is compatible with ROS 2, otherwise use alternatives + bool state_set = false; + state_space_t state_space; + params_t params; + init_vals_t init_vals; + double sim_time, t_start; + // static int step(double t, const double* y, double* f, void *params); + gsl_odeiv2_step* s; + gsl_odeiv2_control *c; + gsl_odeiv2_evolve *e; + +/** + * ODE step for updating the quad state Dynamic equations are obtained from + * Geometric Tracking Control of a Quadrotor UAV on SE(3), Lee et al, (2010) +*/ +static int step(double t, const double *y, double *f, void *params) +{ + auto *param = reinterpret_cast(params); + (void) (t); + // copy R values from y + double R_temp[9]; + for (int i = 6, k = 0; i < 15; i++, k++) + { + R_temp[k] = y[i]; + } + Matrix3d R(R_temp); + + Vector3d omega(y[15], y[16], y[17]); + + // velocity for the next timestep eq(3) + Vector3d e3(0, 0, 1); + Vector3d v = param->gravity * e3 - (1 / param->mass) * (param->F * R * e3); + + // Rdot for next timestep eq(4) + Matrix3d Rdot = R * simulator_utils::hat(omega); + + // Omegadot for the next timestep eq(5) + Vector3d Omegadot = param->J_inv * (param->M - omega.cross(param->J * omega)); + + // writing values to f + // position of next timestep eq(2) + int k = 0; + for (int i = 3; i < 6; i++, k++) + { + f[k] = y[i]; + } + for (int i = 0; i < 3; i++, k++) + { + f[k] = v[i]; + } + for (int i = 0; i < 9; i++, k++) + { + f[k] = Rdot.data()[i]; + } + for (int i = 0; i < 3; i++, k++) + { + f[k] = Omegadot[i]; + } + return GSL_SUCCESS; +} + + +}; + + +#endif diff --git a/include/mavswarm2/quadrotor.hpp b/include/mavswarm2/quadrotor.hpp new file mode 100644 index 0000000..d6bed92 --- /dev/null +++ b/include/mavswarm2/quadrotor.hpp @@ -0,0 +1,350 @@ +/* +Copyright (c) 2024 Malintha Fernando (malintha@onmail.com) + +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. + +*/ + +#ifndef PROJECT_QUADROTOR_H +#define PROJECT_QUADROTOR_H + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/LinearMath/Quaternion.h" + +#include "scp/scp_planner.hpp" + +#include "trajectory_t.hpp" +#include "dynamics_provider.hpp" +#include "controller/geometric_controller.hpp" +#include "simulator_interfaces/msg/waypoint.hpp" + +using namespace std::chrono_literals; + +#ifndef STATE_H +#define STATE_H +enum State { + Idle, + TakingOff, + Landing, + Hover, + Autonomous +}; +#endif + +using namespace Eigen; +using namespace std; + +class Quadrotor : public rclcpp::Node { +public: + Quadrotor(int robot_id, double frequency) + : Node("robot_" + to_string(robot_id)), frequency(frequency), robot_id(robot_id) + { + sim_time = 0; + tau = 0; + dt = 1.0 / frequency; + m_state = State::Idle; + set_init_target = false; + + RCLCPP_INFO(this->get_logger(), "dt: %4f ", dt); + + timer_ = this->create_wall_timer( + std::chrono::duration(dt), + std::bind(&Quadrotor::iteration, this)); + + RCLCPP_INFO(this->get_logger(), "Loading parameters"); + if (!load_params()) { + RCLCPP_ERROR(this->get_logger(), "Could not load the drone parameters"); + rclcpp::shutdown(); + return; + } + + controller = std::make_shared(params_, gains, dt); + dynamics = std::make_shared(params_, init_vals); + set_state_space(); + + tf_broadcaster_ = std::make_unique(*this); + worldframe = "map"; + // per-robot TF child frame so multiple drones don't all broadcast map->base_link + robot_link_name = "robot_" + std::to_string(robot_id) + "/base_link"; + + // initialise hover target to the drone's starting position (NWU) + target_pos = simulator_utils::ned_nwu_rotation(init_vals.position); + + // per-robot desired_state topic, e.g. /mavswarm2/robot_3/desired_state + std::string desired_topic = "robot_" + std::to_string(robot_id) + "/desired_state"; + desired_state_sub_ = this->create_subscription( + desired_topic, 10, + std::bind(&Quadrotor::desired_pos_cb, this, std::placeholders::_1)); + RCLCPP_INFO(this->get_logger(), "Subscribing to goal topic: %s", desired_topic.c_str()); + + // SCP planner owns neighbour-trajectory communication; quad just feeds + // it goals and reads back the reference each tick. + scp_planner_ = std::make_shared(this, robot_id, dt); + + // trajectory trail: a LINE_STRIP marker (last N points) on this robot's + // own topic, e.g. /mavswarm2/robot_3/trajectory. + path_marker_pub_ = this->create_publisher( + "robot_" + std::to_string(robot_id) + "/trajectory", 10); + // distinct colour per robot from the id, via the golden-ratio hue trick: + // well-spread, unique colours for ANY number of robots (no fixed palette). + hsv_to_rgb(std::fmod(robot_id * 0.6180339887f, 1.0f), 0.85f, 1.0f, + base_r_, base_g_, base_b_); + path_marker_.header.frame_id = worldframe; // "map" + path_marker_.ns = "trajectory"; + path_marker_.id = robot_id; + path_marker_.type = visualization_msgs::msg::Marker::LINE_STRIP; + path_marker_.action = visualization_msgs::msg::Marker::ADD; + path_marker_.scale.x = 0.03; // line width + path_marker_.pose.orientation.w = 1.0; + path_marker_.color.r = base_r_; + path_marker_.color.g = base_g_; + path_marker_.color.b = base_b_; + path_marker_.color.a = 1.0; + + this->setState(State::Autonomous); + } + + void setState(State m_state_) { this->m_state = m_state_; } + +private: + // ----------------------------------------------------------------------- + // State + // ----------------------------------------------------------------------- + State m_state; + double sim_time, tau, dt, frequency; + int robot_id; + std::string worldframe, robot_link_name; + + gains_t gains; + Vector3d u, target_pos; + state_space_t state_space; + bool set_init_target; + + std::shared_ptr scp_planner_; + + std::shared_ptr controller; + std::shared_ptr dynamics; + std::unique_ptr tf_broadcaster_; + rclcpp::Subscription::SharedPtr desired_state_sub_; + rclcpp::Publisher::SharedPtr path_marker_pub_; + visualization_msgs::msg::Marker path_marker_; + int path_tick_ = 0; + float base_r_ = 1.0f, base_g_ = 1.0f, base_b_ = 1.0f; + rclcpp::TimerBase::SharedPtr timer_; + + params_t params_; + init_vals_t init_vals; + + // ----------------------------------------------------------------------- + // Parameter loading + // ----------------------------------------------------------------------- + bool load_params() { + declare_parameter("controller_gains", std::vector(4, 0.0)); + vector gains_ = get_parameter("controller_gains").as_double_array(); + + declare_parameter("model.m", 0.00); + declare_parameter("model.gravity", 9.81); + declare_parameter("model.d", 0.08); + declare_parameter("model.ctf", 0.0037); + declare_parameter("model.J", std::vector(3, 0.0)); + + params_.mass = get_parameter("model.m").as_double(); + params_.gravity = get_parameter("model.gravity").as_double(); + vector J_ = get_parameter("model.J").as_double_array(); + params_.J << J_[0], 0, 0, + 0, J_[1], 0, + 0, 0, J_[2]; + params_.J_inv = params_.J.inverse(); + params_.F = 0; + params_.M = Vector3d(0, 0, 0); + + declare_parameter("position", std::vector(3, 0.0)); + declare_parameter("velocity", std::vector(3, 0.0)); + declare_parameter("rotation", std::vector(9, 0.0)); + declare_parameter("omega", std::vector(3, 0.0)); + + vector pos_ = get_parameter("position").as_double_array(); + vector vel_ = get_parameter("velocity").as_double_array(); + vector rot_ = get_parameter("rotation").as_double_array(); + vector omega_ = get_parameter("omega").as_double_array(); + + // store raw values (NWU position/velocity, the NED-matched "upside-down" R). + // ned_nwu_rotation is applied downstream (dynamics start state, iteration xd, + // and broadcast) — converting here would double-convert and break frames. + init_vals.position = Vector3d(pos_.data()); + init_vals.velocity = Vector3d(vel_.data()); + init_vals.R = Matrix3d(rot_.data()); + init_vals.omega = Vector3d(omega_.data()); + + gains = {.kx = gains_[0], .kv = gains_[1], .kr = gains_[2], .komega = gains_[3]}; + + RCLCPP_INFO(this->get_logger(), "Init Params: : %4f %4f %4f ", + init_vals.position[0], init_vals.position[1], init_vals.position[2]); + RCLCPP_INFO(this->get_logger(), "Param: Gravity: %s ", to_string(params_.gravity).c_str()); + RCLCPP_INFO(this->get_logger(), "Param: Mass: %s ", to_string(params_.mass).c_str()); + RCLCPP_INFO(this->get_logger(), "Param: MOI: %6f %6f %6f", J_[0], J_[1], J_[2]); + RCLCPP_INFO(this->get_logger(), "Param: Controller Gains: %4f %4f %4f %4f", + gains.kx, gains.kv, gains.kr, gains.komega); + RCLCPP_INFO(this->get_logger(), "Loaded control parameters"); + return true; + } + + // ----------------------------------------------------------------------- + // Goal subscription -> hand the goal to the SCP planner (NED frame) + // ----------------------------------------------------------------------- + void desired_pos_cb(const geometry_msgs::msg::Point::SharedPtr pt) + { + Vector3d p1(pt->x, pt->y, pt->z); + if ((target_pos - p1).norm() < 0.2) return; + target_pos = p1; + set_init_target = true; + state_space_t ss = dynamics->get_state(); + scp_planner_->set_goal(p1, ss.position, ss.velocity); + RCLCPP_INFO(this->get_logger(), "New goal [%.2f %.2f %.2f]", p1[0], p1[1], p1[2]); + } + + // ----------------------------------------------------------------------- + // TF broadcasting + // ----------------------------------------------------------------------- + void send_transform() + { + Vector3d position = state_space.position; + Vector3d rpy = simulator_utils::R2RPY(state_space.R); + + tf2::Quaternion q; + q.setRPY(rpy[0], rpy[1], rpy[2]); + if (std::isnan(q.x())) return; + + geometry_msgs::msg::TransformStamped t; + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = worldframe; + t.child_frame_id = robot_link_name; + t.transform.translation.x = position[0]; + t.transform.translation.y = position[1]; + t.transform.translation.z = position[2]; + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + // RCLCPP_INFO(this->get_logger(), " %4f %4f %4f %4f %4f %4f %4f", position[0], position[1],position[2], q.x(), q.y(), q.z(), q.w()); + + tf_broadcaster_->sendTransform(t); + } + + // ----------------------------------------------------------------------- + // Control loop + // ----------------------------------------------------------------------- + void move(const desired_state_t &d_state) + { + state_space_t s = dynamics->get_state(); + RCLCPP_DEBUG(this->get_logger(), "%2f position %4f %4f %4f", + sim_time, s.position[0], s.position[1], s.position[2]); + + control_out_t control = controller->get_control(s, d_state); + Vector3d rpy = simulator_utils::R2RPY(state_space.R); + + // RCLCPP_INFO(this->get_logger(), "%2f current vs desired %4f %4f %4f | %4f %4f %4f", + // sim_time, rpy[0], rpy[1], rpy[2], d_state.b1[0],d_state.b1[1], d_state.b1[2]); + dynamics->update(control, sim_time); + set_state_space(); + send_transform(); + publish_path(); + dynamics->reset_dynamics(); + sim_time += dt; + } + + // Trajectory trail: append the current (map-frame) position to a LINE_STRIP + // marker, keep only the last N points, and publish. Mirrors the ROS 1 + // Quadrotor::publish_path(). + void publish_path() + { + constexpr size_t kMaxPoints = 200; + if (++path_tick_ % 5 != 0) return; // sample ~20 Hz -> ~50 s of trail + + const Vector3d pos = state_space.position; // NWU / map frame + if (std::isnan(pos[0]) || std::isnan(pos[1]) || std::isnan(pos[2])) return; + + geometry_msgs::msg::Point p; + p.x = pos[0]; p.y = pos[1]; p.z = pos[2]; + path_marker_.points.push_back(p); + if (path_marker_.points.size() > kMaxPoints) + path_marker_.points.erase(path_marker_.points.begin()); + + // per-point alpha ramp: oldest point faint, newest solid. The tail then + // fades out smoothly so dropping the front point is barely visible. + const size_t n = path_marker_.points.size(); + path_marker_.colors.resize(n); + for (size_t i = 0; i < n; ++i) { + auto &c = path_marker_.colors[i]; + c.r = base_r_; c.g = base_g_; c.b = base_b_; + c.a = 0.05f + 0.95f * (static_cast(i + 1) / static_cast(n)); + } + + path_marker_.header.stamp = this->get_clock()->now(); + path_marker_pub_->publish(path_marker_); + } + + // HSV (h,s,v in [0,1]) -> RGB, for procedural per-robot trail colours. + static void hsv_to_rgb(float h, float s, float v, float &r, float &g, float &b) + { + const float i = std::floor(h * 6.0f); + const float f = h * 6.0f - i; + const float p = v * (1.0f - s); + const float q = v * (1.0f - f * s); + const float t = v * (1.0f - (1.0f - f) * s); + switch (static_cast(i) % 6) { + case 0: r = v; g = t; b = p; break; + case 1: r = q; g = v; b = p; break; + case 2: r = p; g = v; b = t; break; + case 3: r = p; g = q; b = v; break; + case 4: r = t; g = p; b = v; break; + default: r = v; g = p; b = q; break; + } + } + + void set_state_space() + { + // convert the internal NED state back to map frame (NWU) for broadcasting + state_space_t ss = dynamics->get_state(); + state_space.position = simulator_utils::ned_nwu_rotation(ss.position); + state_space.R = simulator_utils::ned_nwu_rotation(ss.R); + state_space.velocity = simulator_utils::ned_nwu_rotation(ss.velocity); + state_space.omega = simulator_utils::ned_nwu_rotation(ss.omega); + } + + void iteration() + { + Vector3d b1d(1, 0, 0); + Vector3d xd = simulator_utils::ned_nwu_rotation(init_vals.position); + + if (set_init_target) { + // SCP planner advances playback and replans (with collision avoidance) internally + xd = scp_planner_->reference(); + } + + desired_state_t dss = {xd, b1d}; + this->move(dss); + } +}; + +#endif diff --git a/include/mavswarm2/scp/scp_planner.hpp b/include/mavswarm2/scp/scp_planner.hpp new file mode 100644 index 0000000..1395d98 --- /dev/null +++ b/include/mavswarm2/scp/scp_planner.hpp @@ -0,0 +1,343 @@ +/* +Copyright (c) 2024 Malintha Fernando (malintha@onmail.com) + +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. +*/ + +// --------------------------------------------------------------------------- +// +// The trajectory is a discretised double integrator (states = position + +// velocity, input = acceleration). We minimise the control effort while +// honouring the boundary conditions (start state, goal at rest) and the +// per-axis velocity / acceleration limits. +// +// The pairwise keep-out ||p_i[k] - p_j[k]|| >= R is non-convex. Following the +// paper it is convexified about the previous iterate into the affine +// separating-hyperplane constraint +// xi/||xi|| . (p_i[k] - p_j[k]) >= R, xi = p_i^prev[k] - p_j[k], +// and the resulting convex problem is solved; re-linearising and re-solving a +// few times is the SCP loop. (Solved here as a weighted least-squares / penalty +// problem with Eigen, so no external QP solver is required.) +// +// This module is SELF-CONTAINED: it owns the ROS publisher/subscribers used to +// exchange predicted trajectories between robots, so the quadrotor code does not +// need to know about neighbour state at all. Drop-in for the old trajectory +// optimiser: set_goal(...) once, then call reference() every control tick. +// --------------------------------------------------------------------------- + +#ifndef MAVSWARM2_SCP_PLANNER_HPP +#define MAVSWARM2_SCP_PLANNER_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" + +namespace scp { + +using Eigen::Vector3d; +using Eigen::VectorXd; +using Eigen::MatrixXd; + +struct SCPParams { + double v_max = 1.0; // m/s velocity limit + double a_max = 2.0; // m/s^2 acceleration limit + double R = 0.5; // m collision keep-out radius + double downwash = 0.5; // ellipsoid z-scaling (1 = isotropic sphere) + double activation = 0.1; // m outward overshoot past the shell + double dt_grid = 0.15; // s trajectory discretisation step + int K_min = 10; // min / max number of segments + int K_max = 60; + int max_iters = 8; // SCP re-linearisations + int replan_every= 30; // control ticks between receding-horizon replans + // penalty weights + double w_smooth = 10.0; // control effort (acceleration) + double w_init = 1.0e6; // start state (hard) + double w_vel = 1.0e2; // start velocity + double w_goal = 1.0e3; // terminal position = goal + double w_term_vel = 1.0e2; // terminal velocity = 0 + double w_vlim = 1.0e3; // velocity limit (one-sided) + double w_alim = 5.0e2; // acceleration limit (one-sided) + double w_col = 1.0e3; // collision keep-out +}; + +// --------------------------------------------------------------------------- +// Discrete trajectory with a smooth (Catmull-Rom) interpolant. +// --------------------------------------------------------------------------- +class Trajectory { +public: + Trajectory() : h_(0.1), T_(0.0) {} + Trajectory(std::vector pts, double h) + : pts_(std::move(pts)), h_(h), + T_(h * static_cast(pts_.empty() ? 0 : pts_.size() - 1)) {} + + double getMaxTime() const { return T_; } + bool empty() const { return pts_.empty(); } + + // derivative_order: 0 = position, 1 = velocity, 2 = acceleration + Vector3d evaluate(double t, int derivative_order = 0) const + { + const int n = static_cast(pts_.size()); + if (n == 0) return Vector3d::Zero(); + if (n == 1) return (derivative_order == 0) ? pts_[0] : Vector3d::Zero(); + + if (t < 0.0) t = 0.0; + if (t > T_) t = T_; + int i = static_cast(std::floor(t / h_)); + if (i > n - 2) i = n - 2; + const double u = (t - i * h_) / h_; + + const Vector3d &P1 = pts_[i]; + const Vector3d &P2 = pts_[i + 1]; + const Vector3d P0 = (i - 1 >= 0) ? pts_[i - 1] : (2.0 * P1 - P2); + const Vector3d P3 = (i + 2 <= n - 1) ? pts_[i + 2] : (2.0 * P2 - P1); + + const Vector3d a = 2.0 * P1; + const Vector3d b = -P0 + P2; + const Vector3d c = 2.0 * P0 - 5.0 * P1 + 4.0 * P2 - P3; + const Vector3d d = -P0 + 3.0 * P1 - 3.0 * P2 + P3; + + if (derivative_order == 0) return 0.5 * (a + b * u + c * (u * u) + d * (u * u * u)); + if (derivative_order == 1) return 0.5 * (b + 2.0 * c * u + 3.0 * d * (u * u)) / h_; + return 0.5 * (2.0 * c + 6.0 * d * u) / (h_ * h_); + } + +private: + std::vector pts_; + double h_; + double T_; +}; + +// --------------------------------------------------------------------------- +// Per-robot SCP planner. Owns the neighbour-trajectory communication. +// --------------------------------------------------------------------------- +class ScpPlanner { +public: + ScpPlanner(rclcpp::Node *node, int robot_id, double control_dt, + const SCPParams &prm = SCPParams()) + : node_(node), robot_id_(robot_id), dt_(control_dt), prm_(prm) + { + if (!node_->has_parameter("num_robots")) + node_->declare_parameter("num_robots", 10); + if (!node_->has_parameter("safety_radius")) + node_->declare_parameter("safety_radius", prm_.R); + num_robots_ = node_->get_parameter("num_robots").as_int(); + prm_.R = node_->get_parameter("safety_radius").as_double(); + + path_pub_ = node_->create_publisher( + "robot_" + std::to_string(robot_id_) + "/scp_path", 10); + for (int j = 0; j < num_robots_; ++j) { + if (j == robot_id_) continue; + auto sub = node_->create_subscription( + "robot_" + std::to_string(j) + "/scp_path", 10, + [this, j](const std_msgs::msg::Float32MultiArray::SharedPtr m) { on_path(j, m); }); + path_subs_.push_back(sub); + } + } + + // Set / update the goal (NED). Plans immediately from the supplied state. + void set_goal(const Vector3d &goal, const Vector3d &p0, const Vector3d &v0) + { + goal_ = goal; + has_goal_ = true; + traj_ = solve(p0, v0, goal_); + t_play_ = 0.0; + ticks_ = 0; + publish_path(); + } + + // Desired position for this control tick (advances playback, replans as needed). + Vector3d reference() + { + if (!has_goal_) return goal_; + + if (++ticks_ >= prm_.replan_every && !traj_.empty()) { + // receding horizon: re-solve from the current reference (keeps xd smooth) + const Vector3d p0 = traj_.evaluate(t_play_, 0); + const Vector3d v0 = traj_.evaluate(t_play_, 1); + traj_ = solve(p0, v0, goal_); + t_play_ = 0.0; + ticks_ = 0; + } + const Vector3d xd = traj_.evaluate(t_play_, 0); + t_play_ = std::min(t_play_ + dt_, traj_.getMaxTime()); + publish_path(); + return xd; + } + +private: + // ----- neighbour communication ----------------------------------------- + void on_path(int j, const std_msgs::msg::Float32MultiArray::SharedPtr msg) + { + if (msg->data.size() < 4) return; + const double dtj = msg->data[0]; + std::vector s; + for (size_t i = 1; i + 2 < msg->data.size(); i += 3) + s.emplace_back(msg->data[i], msg->data[i + 1], msg->data[i + 2]); + neighbor_paths_[j] = {dtj, std::move(s)}; + } + + void publish_path() + { + std_msgs::msg::Float32MultiArray msg; + const double dtp = prm_.dt_grid; + const int M = 20; + msg.data.push_back(static_cast(dtp)); + for (int m = 0; m <= M; ++m) { + Vector3d q = traj_.empty() ? goal_ + : traj_.evaluate(std::min(t_play_ + m * dtp, traj_.getMaxTime()), 0); + msg.data.push_back(static_cast(q[0])); + msg.data.push_back(static_cast(q[1])); + msg.data.push_back(static_cast(q[2])); + } + path_pub_->publish(msg); + } + + // neighbour j's predicted position at relative time t (clamped) + Vector3d neighbor_at(int j, double t) const + { + const auto it = neighbor_paths_.find(j); + if (it == neighbor_paths_.end() || it->second.second.empty()) + return Vector3d::Constant(1e9); // unknown -> effectively absent + const double dtj = it->second.first; + const auto &s = it->second.second; + const double fm = t / std::max(dtj, 1e-6); + const int m = static_cast(std::floor(fm)); + if (m >= static_cast(s.size()) - 1) return s.back(); + const double a = fm - m; + return (1.0 - a) * s[m] + a * s[m + 1]; + } + + // ----- the SCP solve --------------------------------------------------- + Trajectory solve(const Vector3d &p0, const Vector3d &v0, const Vector3d &goal) + { + const double dist = (goal - p0).norm(); + // horizon sized to actually reach the goal at <= v_max (no overspeed) + const double T = std::max(2.0, 1.2 * dist / std::max(prm_.v_max, 1e-3)); + const int K = std::clamp(static_cast(std::round(T / prm_.dt_grid)), + prm_.K_min, prm_.K_max); + const double h = T / K; + const int nv = 3 * (K + 1); + + std::vector p(K + 1); + for (int k = 0; k <= K; ++k) + p[k] = p0 + (static_cast(k) / K) * (goal - p0); + + auto idx = [](int k, int d) { return 3 * k + d; }; + const double cz = prm_.downwash; + + for (int iter = 0; iter < prm_.max_iters; ++iter) { + MatrixXd H = MatrixXd::Zero(nv, nv); + VectorXd c = VectorXd::Zero(nv); + auto addRes = [&](double w, const std::vector> &t, double tgt) { + for (const auto &ta : t) { + c(ta.first) += w * ta.second * tgt; + for (const auto &tb : t) H(ta.first, tb.first) += w * ta.second * tb.second; + } + }; + + // boundary conditions + for (int d = 0; d < 3; ++d) addRes(prm_.w_init, {{idx(0, d), 1.0}}, p0[d]); + for (int d = 0; d < 3; ++d) addRes(prm_.w_vel, {{idx(1, d), 1.0}, {idx(0, d), -1.0}}, h * v0[d]); + for (int d = 0; d < 3; ++d) addRes(prm_.w_goal, {{idx(K, d), 1.0}}, goal[d]); + for (int d = 0; d < 3; ++d) addRes(prm_.w_term_vel, {{idx(K, d), 1.0}, {idx(K - 1, d), -1.0}}, 0.0); + + // control effort: minimise acceleration (double-integrator input) + for (int k = 1; k <= K - 1; ++k) + for (int d = 0; d < 3; ++d) + addRes(prm_.w_smooth, {{idx(k + 1, d), 1.0}, {idx(k, d), -2.0}, {idx(k - 1, d), 1.0}}, 0.0); + + // velocity limit (one-sided) + for (int k = 0; k <= K - 1; ++k) { + const Vector3d seg = p[k + 1] - p[k]; + if (seg.norm() / h > prm_.v_max && seg.norm() > 1e-9) { + const Vector3d s = seg / seg.norm(); + addRes(prm_.w_vlim, + {{idx(k + 1, 0), s[0]}, {idx(k, 0), -s[0]}, + {idx(k + 1, 1), s[1]}, {idx(k, 1), -s[1]}, + {idx(k + 1, 2), s[2]}, {idx(k, 2), -s[2]}}, h * prm_.v_max); + } + } + + // acceleration limit (one-sided) + for (int k = 1; k <= K - 1; ++k) { + const Vector3d acc = p[k + 1] - 2.0 * p[k] + p[k - 1]; + if (acc.norm() / (h * h) > prm_.a_max && acc.norm() > 1e-9) { + const Vector3d s = acc / acc.norm(); + addRes(prm_.w_alim, + {{idx(k + 1, 0), s[0]}, {idx(k, 0), -2.0 * s[0]}, {idx(k - 1, 0), s[0]}, + {idx(k + 1, 1), s[1]}, {idx(k, 1), -2.0 * s[1]}, {idx(k - 1, 1), s[1]}, + {idx(k + 1, 2), s[2]}, {idx(k, 2), -2.0 * s[2]}, {idx(k - 1, 2), s[2]}}, + h * h * prm_.a_max); + } + } + + // collision: separating hyperplane vs higher-priority neighbours (lower id). + // Prioritised planning: a robot only avoids smaller-id robots, which + // breaks the symmetry that causes reciprocal dodging / deadlock. + const double shell = prm_.R; + for (int j = 0; j < robot_id_; ++j) { + if (neighbor_paths_.find(j) == neighbor_paths_.end()) continue; + for (int k = 1; k <= K; ++k) { + const Vector3d o = neighbor_at(j, k * h); + const Vector3d e = p[k] - o; + const double ne = std::sqrt(e[0]*e[0] + e[1]*e[1] + cz*cz*e[2]*e[2]); + if (ne < shell && ne > 1e-6) { + const Vector3d a(e[0] / ne, e[1] / ne, cz * cz * e[2] / ne); + addRes(prm_.w_col, + {{idx(k, 0), a[0]}, {idx(k, 1), a[1]}, {idx(k, 2), a[2]}}, + shell + prm_.activation + a.dot(o)); + } + } + } + + for (int i = 0; i < nv; ++i) H(i, i) += 1e-9; + const VectorXd x = H.ldlt().solve(c); + + double step = 0.0; + for (int k = 0; k <= K; ++k) { + const Vector3d pk(x(idx(k, 0)), x(idx(k, 1)), x(idx(k, 2))); + step = std::max(step, (pk - p[k]).norm()); + p[k] = pk; + } + if (step < 1e-3) break; + } + return Trajectory(std::move(p), h); + } + + rclcpp::Node *node_; + int robot_id_; + double dt_; + SCPParams prm_; + int num_robots_ = 10; + + bool has_goal_ = false; + Vector3d goal_ = Vector3d::Zero(); + Trajectory traj_; + double t_play_ = 0.0; + int ticks_ = 0; + + rclcpp::Publisher::SharedPtr path_pub_; + std::vector::SharedPtr> path_subs_; + std::map>> neighbor_paths_; +}; + +} // namespace scp + +#endif // MAVSWARM2_SCP_PLANNER_HPP diff --git a/include/mavswarm2/trajectory_t.hpp b/include/mavswarm2/trajectory_t.hpp new file mode 100644 index 0000000..d24dbbb --- /dev/null +++ b/include/mavswarm2/trajectory_t.hpp @@ -0,0 +1,62 @@ +/* + +Copyright (c) 2024 Malintha Fernando (malintha@onmail.com) + +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. + +*/ + +#include +#include +#include +#include "std_msgs/msg/float32_multi_array.hpp" + +using namespace std; +using namespace Eigen; + +#ifndef MRF_DYNAMICS_TRAJECTORY_T_H +#define MRF_DYNAMICS_TRAJECTORY_T_H + +struct Trajectory_t { + vector xc; + vector yc; + vector zc; + int D; + Trajectory_t(const std_msgs::msg::Float32MultiArray& coeffs); + Trajectory_t(); + Vector3d eval(double d); + +}; + +Trajectory_t::Trajectory_t() = default; + +Trajectory_t::Trajectory_t(const std_msgs::msg::Float32MultiArray& coeffs) { + this->D = coeffs.data.size()/3; + for(int j=0;j + +using namespace Eigen; + +typedef struct { + Vector3d position; + Matrix3d R; + Vector3d omega; + Vector3d velocity; + double h; +} init_vals_t; + +typedef struct { + double gravity; + double mass; + Matrix3d J; + Matrix3d J_inv; + double F; + Vector3d M; +} params_t; + +typedef struct { + Vector3d position; + Vector3d velocity; + Vector3d acceleration; + Matrix3d R; + Vector3d omega; +} state_space_t; + +typedef struct { + Vector3d x; + Vector3d b1; +} desired_state_t; + +typedef struct +{ + double kx; + double kv; + double kr; + double komega; +} gains_t; + +typedef struct { + double F; + Vector3d M; +} control_out_t; + +typedef struct { + Vector3d position; + Vector3d velocity; + Vector3d acceleration; + Vector3d jerk; +} opt_t; + +namespace simulator_utils { + Matrix3d hat(Vector3d v); + Vector3d R2RPY(Matrix3d R); + Vector3d ned_nwu_rotation(Vector3d v); + Matrix3d ned_nwu_rotation(Matrix3d m); + Vector3d vee(Matrix3d mat); + +} + +Matrix3d simulator_utils::hat(Vector3d v) +{ + Matrix3d hat_map(3, 3); + hat_map << 0, -v(2), v(1), + v(2), 0, -v(0), + -v(1), v(0), 0; + return hat_map; +} + +Vector3d simulator_utils::vee(Matrix3d mat) +{ + Vector3d vee_vec(mat(2, 1), mat(0, 2),mat(1, 0)); + return vee_vec; +} + +Vector3d simulator_utils::R2RPY(Matrix3d R) +{ + Vector3d rpy; + rpy << atan2(R(2, 1), R(2, 2)), + atan2(-R(2, 0), sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2))), + atan2(R(1, 0), R(0, 0)); + return rpy; +} + +Vector3d simulator_utils::ned_nwu_rotation(Vector3d v) +{ + Matrix3d rot; + rot << 1, 0, 0, + 0, -1, 0, + 0, 0, -1; + return rot * v; +} + +Matrix3d simulator_utils::ned_nwu_rotation(Matrix3d m) +{ + Matrix3d rot; + rot << 1, 0, 0, + 0, -1, 0, + 0, 0, -1; + return rot * m; +} \ No newline at end of file diff --git a/multi_uav_simulator/src/cfSimUtils.cpp b/include/mavswarm2/utils.hpp old mode 100644 new mode 100755 similarity index 67% rename from multi_uav_simulator/src/cfSimUtils.cpp rename to include/mavswarm2/utils.hpp index 8f9c517..65bad32 --- a/multi_uav_simulator/src/cfSimUtils.cpp +++ b/include/mavswarm2/utils.hpp @@ -1,6 +1,49 @@ -#include "cfSimUtils.h" + +Copyright (c) 2024 Malintha Fernando (malintha@onmail.com) + +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. + +*/ + +#ifndef PROJECT_CFSIMUTILS_H +#define PROJECT_CFSIMUTILS_H + +#include +#include +#include "ros/ros.h" +#include +#include +#include +#include +#include "tf/tf.h" + +using namespace Eigen; namespace cfSimUtils { + /** + * @param gamma roll + * @param beta pitch + * @param alpha yaw + */ + + struct node { + int id, cluster; + float x, y, z; + std::string type; + node(int id, int cluster, float x, float y, float z, std::string type):id(id), cluster(cluster), x(x), y(y), + z(z), type(type) {}; + }; + Matrix3d getR(double gamma, double beta, double alpha) { Matrix3d R; @@ -71,4 +114,6 @@ namespace cfSimUtils { } return coords_list; } -} \ No newline at end of file +} + +#endif diff --git a/include/mavswarm2/visualizer.h b/include/mavswarm2/visualizer.h new file mode 100644 index 0000000..e69de29 diff --git a/include/misc/visualizer.hpp b/include/misc/visualizer.hpp new file mode 100644 index 0000000..c74c153 --- /dev/null +++ b/include/misc/visualizer.hpp @@ -0,0 +1,327 @@ +#ifndef VISUALIZER_HPP +#define VISUALIZER_HPP + +#include "gcopter/trajectory.hpp" +#include "gcopter/quickhull.hpp" +#include "gcopter/geo_utils.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// Visualizer for the planner +class Visualizer +{ +private: + // config contains the scale for some markers + ros::NodeHandle nh; + + // These are publishers for path, waypoints on the trajectory, + // the entire trajectory, the mesh of free-space polytopes, + // the edge of free-space polytopes, and spheres for safety radius + ros::Publisher routePub; + ros::Publisher wayPointsPub; + ros::Publisher trajectoryPub; + ros::Publisher meshPub; + ros::Publisher edgePub; + ros::Publisher spherePub; + +public: + ros::Publisher speedPub; + ros::Publisher thrPub; + ros::Publisher tiltPub; + ros::Publisher bdrPub; + +public: + Visualizer(ros::NodeHandle &nh_) + : nh(nh_) + { + routePub = nh.advertise("/visualizer/route", 10); + wayPointsPub = nh.advertise("/visualizer/waypoints", 10); + trajectoryPub = nh.advertise("/visualizer/trajectory", 10); + meshPub = nh.advertise("/visualizer/mesh", 1000); + edgePub = nh.advertise("/visualizer/edge", 1000); + spherePub = nh.advertise("/visualizer/spheres", 1000); + speedPub = nh.advertise("/visualizer/speed", 1000); + thrPub = nh.advertise("/visualizer/total_thrust", 1000); + tiltPub = nh.advertise("/visualizer/tilt_angle", 1000); + bdrPub = nh.advertise("/visualizer/body_rate", 1000); + } + + // Visualize the trajectory and its front-end path + template + inline void visualize(const Trajectory &traj, + const std::vector &route) + { + visualization_msgs::Marker routeMarker, wayPointsMarker, trajMarker; + + routeMarker.id = 0; + routeMarker.type = visualization_msgs::Marker::LINE_LIST; + routeMarker.header.stamp = ros::Time::now(); + routeMarker.header.frame_id = "odom"; + routeMarker.pose.orientation.w = 1.00; + routeMarker.action = visualization_msgs::Marker::ADD; + routeMarker.ns = "route"; + routeMarker.color.r = 1.00; + routeMarker.color.g = 0.00; + routeMarker.color.b = 0.00; + routeMarker.color.a = 1.00; + routeMarker.scale.x = 0.1; + + wayPointsMarker = routeMarker; + wayPointsMarker.id = -wayPointsMarker.id - 1; + wayPointsMarker.type = visualization_msgs::Marker::SPHERE_LIST; + wayPointsMarker.ns = "waypoints"; + wayPointsMarker.color.r = 1.00; + wayPointsMarker.color.g = 0.00; + wayPointsMarker.color.b = 0.00; + wayPointsMarker.scale.x = 0.35; + wayPointsMarker.scale.y = 0.35; + wayPointsMarker.scale.z = 0.35; + + trajMarker = routeMarker; + trajMarker.header.frame_id = "odom"; + trajMarker.id = 0; + trajMarker.ns = "trajectory"; + trajMarker.color.r = 0.00; + trajMarker.color.g = 0.50; + trajMarker.color.b = 1.00; + trajMarker.scale.x = 0.30; + + if (route.size() > 0) + { + bool first = true; + Eigen::Vector3d last; + for (auto it : route) + { + if (first) + { + first = false; + last = it; + continue; + } + geometry_msgs::Point point; + + point.x = last(0); + point.y = last(1); + point.z = last(2); + routeMarker.points.push_back(point); + point.x = it(0); + point.y = it(1); + point.z = it(2); + routeMarker.points.push_back(point); + last = it; + } + + routePub.publish(routeMarker); + } + + if (traj.getPieceNum() > 0) + { + Eigen::MatrixXd wps = traj.getPositions(); + for (int i = 0; i < wps.cols(); i++) + { + geometry_msgs::Point point; + point.x = wps.col(i)(0); + point.y = wps.col(i)(1); + point.z = wps.col(i)(2); + wayPointsMarker.points.push_back(point); + } + + wayPointsPub.publish(wayPointsMarker); + } + + if (traj.getPieceNum() > 0) + { + double T = 0.01; + Eigen::Vector3d lastX = traj.getPos(0.0); + for (double t = T; t < traj.getTotalDuration(); t += T) + { + geometry_msgs::Point point; + Eigen::Vector3d X = traj.getPos(t); + point.x = lastX(0); + point.y = lastX(1); + point.z = lastX(2); + trajMarker.points.push_back(point); + point.x = X(0); + point.y = X(1); + point.z = X(2); + trajMarker.points.push_back(point); + lastX = X; + } + trajectoryPub.publish(trajMarker); + } + } + + // Visualize some polytopes in H-representation + inline void visualizePolytope(const std::vector &hPolys) + { + + // Due to the fact that H-representation cannot be directly visualized + // We first conduct vertex enumeration of them, then apply quickhull + // to obtain triangle meshs of polyhedra + Eigen::Matrix3Xd mesh(3, 0), curTris(3, 0), oldTris(3, 0); + for (size_t id = 0; id < hPolys.size(); id++) + { + oldTris = mesh; + Eigen::Matrix vPoly; + geo_utils::enumerateVs(hPolys[id], vPoly); + + quickhull::QuickHull tinyQH; + const auto polyHull = tinyQH.getConvexHull(vPoly.data(), vPoly.cols(), false, true); + const auto &idxBuffer = polyHull.getIndexBuffer(); + int hNum = idxBuffer.size() / 3; + + curTris.resize(3, hNum * 3); + for (int i = 0; i < hNum * 3; i++) + { + curTris.col(i) = vPoly.col(idxBuffer[i]); + } + mesh.resize(3, oldTris.cols() + curTris.cols()); + mesh.leftCols(oldTris.cols()) = oldTris; + mesh.rightCols(curTris.cols()) = curTris; + } + + // RVIZ support tris for visualization + visualization_msgs::Marker meshMarker, edgeMarker; + + meshMarker.id = 0; + meshMarker.header.stamp = ros::Time::now(); + meshMarker.header.frame_id = "odom"; + meshMarker.pose.orientation.w = 1.00; + meshMarker.action = visualization_msgs::Marker::ADD; + meshMarker.type = visualization_msgs::Marker::TRIANGLE_LIST; + meshMarker.ns = "mesh"; + meshMarker.color.r = 0.00; + meshMarker.color.g = 0.00; + meshMarker.color.b = 1.00; + meshMarker.color.a = 0.15; + meshMarker.scale.x = 1.0; + meshMarker.scale.y = 1.0; + meshMarker.scale.z = 1.0; + + edgeMarker = meshMarker; + edgeMarker.type = visualization_msgs::Marker::LINE_LIST; + edgeMarker.ns = "edge"; + edgeMarker.color.r = 0.00; + edgeMarker.color.g = 1.00; + edgeMarker.color.b = 1.00; + edgeMarker.color.a = 1.00; + edgeMarker.scale.x = 0.02; + + geometry_msgs::Point point; + + int ptnum = mesh.cols(); + + for (int i = 0; i < ptnum; i++) + { + point.x = mesh(0, i); + point.y = mesh(1, i); + point.z = mesh(2, i); + meshMarker.points.push_back(point); + } + + for (int i = 0; i < ptnum / 3; i++) + { + for (int j = 0; j < 3; j++) + { + point.x = mesh(0, 3 * i + j); + point.y = mesh(1, 3 * i + j); + point.z = mesh(2, 3 * i + j); + edgeMarker.points.push_back(point); + point.x = mesh(0, 3 * i + (j + 1) % 3); + point.y = mesh(1, 3 * i + (j + 1) % 3); + point.z = mesh(2, 3 * i + (j + 1) % 3); + edgeMarker.points.push_back(point); + } + } + + meshPub.publish(meshMarker); + edgePub.publish(edgeMarker); + + return; + } + + // Visualize all spheres with centers sphs and the same radius + inline void visualizeSphere(const Eigen::Vector3d ¢er, + const double &radius) + { + visualization_msgs::Marker sphereMarkers, sphereDeleter; + + sphereMarkers.id = 0; + sphereMarkers.type = visualization_msgs::Marker::SPHERE_LIST; + sphereMarkers.header.stamp = ros::Time::now(); + sphereMarkers.header.frame_id = "odom"; + sphereMarkers.pose.orientation.w = 1.00; + sphereMarkers.action = visualization_msgs::Marker::ADD; + sphereMarkers.ns = "spheres"; + sphereMarkers.color.r = 0.00; + sphereMarkers.color.g = 0.00; + sphereMarkers.color.b = 1.00; + sphereMarkers.color.a = 1.00; + sphereMarkers.scale.x = radius * 2.0; + sphereMarkers.scale.y = radius * 2.0; + sphereMarkers.scale.z = radius * 2.0; + + sphereDeleter = sphereMarkers; + sphereDeleter.action = visualization_msgs::Marker::DELETE; + + geometry_msgs::Point point; + point.x = center(0); + point.y = center(1); + point.z = center(2); + sphereMarkers.points.push_back(point); + + spherePub.publish(sphereDeleter); + spherePub.publish(sphereMarkers); + } + + inline void visualizeStartGoal(const Eigen::Vector3d ¢er, + const double &radius, + const int sg) + { + visualization_msgs::Marker sphereMarkers, sphereDeleter; + + sphereMarkers.id = sg; + sphereMarkers.type = visualization_msgs::Marker::SPHERE_LIST; + sphereMarkers.header.stamp = ros::Time::now(); + sphereMarkers.header.frame_id = "odom"; + sphereMarkers.pose.orientation.w = 1.00; + sphereMarkers.action = visualization_msgs::Marker::ADD; + sphereMarkers.ns = "StartGoal"; + sphereMarkers.color.r = 1.00; + sphereMarkers.color.g = 0.00; + sphereMarkers.color.b = 0.00; + sphereMarkers.color.a = 1.00; + sphereMarkers.scale.x = radius * 2.0; + sphereMarkers.scale.y = radius * 2.0; + sphereMarkers.scale.z = radius * 2.0; + + sphereDeleter = sphereMarkers; + sphereDeleter.action = visualization_msgs::Marker::DELETEALL; + + geometry_msgs::Point point; + point.x = center(0); + point.y = center(1); + point.z = center(2); + sphereMarkers.points.push_back(point); + + if (sg == 0) + { + spherePub.publish(sphereDeleter); + ros::Duration(1.0e-9).sleep(); + sphereMarkers.header.stamp = ros::Time::now(); + } + spherePub.publish(sphereMarkers); + } +}; + +#endif \ No newline at end of file diff --git a/launch/quadrotor.launch b/launch/quadrotor.launch new file mode 100644 index 0000000..5921071 --- /dev/null +++ b/launch/quadrotor.launch @@ -0,0 +1,130 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/multi_uav_simulator/launch/simu.launch b/launch/simu.launch similarity index 100% rename from multi_uav_simulator/launch/simu.launch rename to launch/simu.launch diff --git a/multi_uav_simulator/CMakeLists.txt b/multi_uav_simulator/CMakeLists.txt deleted file mode 100755 index cb41aba..0000000 --- a/multi_uav_simulator/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -macro(use_cxx11) - if (CMAKE_VERSION VERSION_LESS "3.1") - if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=gnu++11") - endif () - else () - set(CMAKE_CXX_STANDARD 11) - endif () -endmacro(use_cxx11) - -project(multi_uav_simulator) - -find_package(Armadillo REQUIRED) -find_package(GSL REQUIRED) - -find_package(catkin REQUIRED COMPONENTS - roscpp - #rospy - std_msgs - tf - geo_controller - message_generation - simulator_utils - mav_trajectory_generation - ) - -catkin_package( -) - -include_directories( - include/${PROJECT_NAME} - ${catkin_INCLUDE_DIRS} - ${geo_controller_INCLUDE_DIRS} - ${simulator_utils_INCLUDE_DIRS} -) - -add_executable(multi_uav_simulator - src/DynamicsProvider.cpp - src/cfSimUtils.cpp - src/Quadrotor.cpp - ) - -target_link_libraries(multi_uav_simulator - GSL::gsl - GSL::gslcblas - ${catkin_LIBRARIES}) diff --git a/multi_uav_simulator/bigquad_description/bigquad.dae b/multi_uav_simulator/bigquad_description/bigquad.dae deleted file mode 100644 index ab62cd7..0000000 --- a/multi_uav_simulator/bigquad_description/bigquad.dae +++ /dev/null @@ -1,235 +0,0 @@ - - - - - Stefan Kohlbrecher - Blender 2.61.4 r43829 - - 2012-02-03T14:20:49 - 2012-02-03T14:20:49 - - Z_UP - - - - - - - - 0 0 0 1 - - - 0.1 0.1 0.1 1 - - - 1 0 0.0106101 1 - - - 0.5 0.5 0.5 1 - - - 50 - - - 1 - - - - - - 1 - - - - 1 - - - - - - - 0.15 0.15 0.15 1 - - - 0.2 0.2 0.2 1 - - - 0.48 0.48 0.48 1 - - - 1 1 1 1 - - - 33 - - - 1 - - - - - - 1 - - - - 1 - - - - - - - 0 0 0 1 - - - 0.1 0.1 0.1 1 - - - 0.05496641 0.05496641 0.05496641 1 - - - 0.5 0.5 0.5 1 - - - 50 - - - 1 - - - - - - 1 - - - - 1 - - - - - - - 0.15 0.15 0.15 1 - - - 0.23 0.23 0.23 1 - - - 0.5 0.5 0.5 1 - - - 1 1 1 1 - - - 20 - - - 1 - - - - - - 1 - - - - 1 - - - - - - - - - - - - - - - - - - - - - 0.8236175 0.673869 -1.366201 -0.8236175 0.6738689 -1.366201 -0.8236175 0.6852639 -1.361481 0.8236175 0.6852641 -1.361481 -0.8236175 0.6899839 -1.350086 0.8236175 0.689984 -1.350086 -0.8236175 0.6852639 -1.338691 0.8236175 0.685264 -1.338691 -0.8236175 0.6738689 -1.333971 0.8236175 0.673869 -1.333971 -0.8236175 0.6624739 -1.338691 0.8236175 0.662474 -1.338691 -0.8236175 0.657754 -1.350086 0.8236175 0.6577541 -1.350086 -0.8236175 0.6624739 -1.361481 0.8236175 0.6624741 -1.361481 0.8236175 0.673869 -1.350086 -0.8236175 0.6738689 -1.350086 0.8236174 -0.6738691 -1.350086 0.8236174 -0.662474 -1.361481 0.8236174 -0.6738691 -1.366201 -0.8236175 -0.6738691 -1.350086 -0.8236175 -0.6738691 -1.366201 -0.8236175 -0.6624742 -1.361481 0.8236174 -0.6577541 -1.350086 -0.8236175 -0.6577541 -1.350086 0.8236174 -0.662474 -1.338691 -0.8236175 -0.662474 -1.338691 0.8236174 -0.6738691 -1.333971 -0.8236175 -0.6738691 -1.333971 0.8236174 -0.6852641 -1.338691 -0.8236175 -0.6852641 -1.338691 0.8236174 -0.689984 -1.350086 -0.8236175 -0.689984 -1.350086 0.8236174 -0.6852641 -1.361481 -0.8236175 -0.6852641 -1.361481 0.5614412 0.08138328 0 0.5614412 0.07047998 -0.04069161 0.5614412 0.04069161 -0.07047998 0.5614412 0 -0.08138328 0.5614412 -0.04069161 -0.07047998 0.5614413 -0.07047998 -0.04069161 0.5614413 -0.08138328 0 0.5614413 -0.07047998 0.04069155 0.5614412 -0.04069167 0.07047992 0.5614412 0 0.08138328 0.5614412 0.04069155 0.07047998 0.5614412 0.07047992 0.04069167 2.084521 0.08138328 0 2.084521 0.07048004 -0.04069155 2.084521 0.04069167 -0.07047986 2.084521 0 -0.08138328 2.084521 -0.04069155 -0.07047998 2.084521 -0.07047986 -0.04069167 2.084521 -0.08138328 0 2.084521 -0.07048004 0.04069155 2.084521 -0.04069173 0.07047992 2.084521 0 0.08138328 2.084521 0.04069155 0.07048004 2.084521 0.07047992 0.04069173 0.5614412 0 0 2.084521 0 0 0.6738689 0.6899839 -1.332763 0.6852639 0.6852639 -1.332763 0.6899839 0.6738689 -1.332763 0.6852639 0.6624739 -1.332763 0.6738689 0.657754 -1.332763 0.6624739 0.6624739 -1.332763 0.657754 0.6738689 -1.332763 0.6624739 0.6852639 -1.332763 0.2994973 0.3156122 -0.303241 0.3108922 0.3108922 -0.303241 0.3156122 0.2994973 -0.303241 0.3108922 0.2881023 -0.303241 0.2994973 0.2833823 -0.303241 0.2881022 0.2881023 -0.303241 0.2833823 0.2994973 -0.303241 0.2881023 0.3108922 -0.303241 0.6738689 0.6738689 -1.332763 0.2994973 0.2994973 -0.303241 0.3156121 -0.2994973 -0.303241 0.6899838 -0.673869 -1.332763 0.6852638 -0.6624739 -1.332763 0.3108922 -0.2881023 -0.303241 0.6738688 -0.657754 -1.332763 0.2994972 -0.2833823 -0.303241 0.6624738 -0.6624739 -1.332763 0.2881022 -0.2881023 -0.303241 0.6577539 -0.673869 -1.332763 0.2833822 -0.2994973 -0.303241 0.6624738 -0.6852639 -1.332763 0.2881022 -0.3108923 -0.303241 0.6738688 -0.6899839 -1.332763 0.2994972 -0.3156122 -0.303241 0.6852638 -0.6852639 -1.332763 0.3108922 -0.3108922 -0.303241 0.2994972 -0.2994973 -0.303241 0.6738688 -0.673869 -1.332763 -0.673869 -0.6738688 -1.332763 -0.685264 -0.6852638 -1.332763 -0.6738691 -0.6899837 -1.332763 -0.2994973 -0.2994972 -0.303241 -0.2994973 -0.3156121 -0.303241 -0.3108923 -0.3108922 -0.303241 -0.689984 -0.6738687 -1.332763 -0.3156123 -0.2994972 -0.303241 -0.685264 -0.6624737 -1.332763 -0.3108923 -0.2881022 -0.303241 -0.673869 -0.6577538 -1.332763 -0.2994973 -0.2833822 -0.303241 -0.662474 -0.6624737 -1.332763 -0.2881023 -0.2881022 -0.303241 -0.6577541 -0.6738688 -1.332763 -0.2833824 -0.2994972 -0.303241 -0.662474 -0.6852638 -1.332763 -0.2881023 -0.3108922 -0.303241 -0.3156121 0.2994973 -0.303241 -0.6899837 0.6738693 -1.332763 -0.6852637 0.662474 -1.332763 -0.3108922 0.2881023 -0.303241 -0.6738687 0.6577541 -1.332763 -0.2994971 0.2833824 -0.303241 -0.6624737 0.662474 -1.332763 -0.2881022 0.2881023 -0.303241 -0.6577537 0.6738691 -1.332763 -0.2833822 0.2994973 -0.303241 -0.6624737 0.6852641 -1.332763 -0.2881022 0.3108924 -0.303241 -0.6738685 0.689984 -1.332763 -0.2994971 0.3156123 -0.303241 -0.6852637 0.6852641 -1.332763 -0.3108922 0.3108923 -0.303241 -0.2994971 0.2994973 -0.303241 -0.6738687 0.6738691 -1.332763 1.969194 0 0.1871857 2.886225 -0.3798463 0.2218515 2.671059 -0.7018645 0.2218514 1.969194 0 0.2620601 2.349041 -0.9170302 0.2218514 1.969195 -0.9925863 0.2218514 1.589348 -0.9170302 0.2218514 1.26733 -0.7018646 0.2218514 1.052164 -0.3798465 0.2218515 0.9766083 -2.36674e-7 0.2218515 1.052164 0.379846 0.2218516 1.26733 0.7018642 0.2218517 1.589348 0.9170299 0.2218517 1.969194 0.992586 0.2218518 2.34904 0.91703 0.2218517 2.671058 0.7018645 0.2218517 2.886224 0.3798464 0.2218516 2.96178 3.25924e-7 0.2218515 0 -0.5614412 0 0.08138316 -0.5614412 0 0.07047986 -0.5614412 -0.04069161 -2.69139e-7 -2.084521 0 0.07047975 -2.084521 -0.04069155 0.08138298 -2.084521 0 0.04069155 -0.5614412 -0.07047998 0.04069143 -2.084521 -0.07047992 0 -0.5614412 -0.08138328 -1.69274e-7 -2.084521 -0.08138328 -0.04069167 -0.5614412 -0.07047998 -0.04069179 -2.084521 -0.07047998 -0.07048004 -0.5614413 -0.04069167 -0.07048016 -2.084521 -0.04069167 -0.08138334 -0.5614413 0 -0.08138352 -2.084521 0 -0.07048004 -0.5614413 0.04069161 -0.07048028 -2.084521 0.04069155 -0.04069173 -0.5614412 0.07047998 -0.04069197 -2.084521 0.07047992 -1.3514e-7 -0.5614412 0.08138328 -3.81294e-7 -2.084521 0.08138328 0.04069143 -0.5614412 0.07047998 0.04069125 -2.084521 0.07048004 0.07047986 -0.5614412 0.04069167 0.07047963 -2.084521 0.04069173 0.379846 -2.886224 0.2218516 -2.61752e-7 -1.969194 0.1871857 0 -2.96178 0.2218515 -3.25223e-7 -1.969194 0.2620601 0.7018641 -2.671058 0.2218517 0.9170297 -2.34904 0.2218517 0.9925858 -1.969194 0.2218517 0.9170296 -1.589348 0.2218517 0.701864 -1.26733 0.2218517 0.3798459 -1.052164 0.2218516 -3.6789e-7 -0.9766083 0.2218515 -0.3798466 -1.052164 0.2218514 -0.7018647 -1.26733 0.2218514 -0.9170304 -1.589348 0.2218513 -0.9925865 -1.969194 0.2218513 -0.9170305 -2.34904 0.2218513 -0.7018648 -2.671059 0.2218514 -0.3798467 -2.886225 0.2218514 -2.96178 4.69959e-7 0.2218515 -2.886225 0.3798471 0.2218514 -1.969194 5.26331e-7 0.1871857 -1.969194 5.89802e-7 0.2620601 -2.671059 0.7018652 0.2218514 -2.34904 0.9170308 0.2218513 -1.969194 0.9925867 0.2218513 -1.589348 0.9170306 0.2218513 -1.26733 0.7018649 0.2218514 -1.052164 0.3798468 0.2218514 -0.9766083 4.99106e-7 0.2218515 -1.052164 -0.3798457 0.2218516 -1.26733 -0.7018638 0.2218517 -1.589348 -0.9170294 0.2218517 -1.969194 -0.9925855 0.2218517 -2.349041 -0.9170294 0.2218517 -2.671058 -0.7018638 0.2218517 -2.886224 -0.3798456 0.2218516 -0.5614412 -0.07047981 0.04069167 -0.5614412 -0.0813831 0 -2.084521 -0.08138269 0 -2.084521 -0.07047933 0.04069173 -0.5614412 -0.04069137 0.07047998 -2.084521 -0.04069095 0.07048004 -0.5614412 2.10574e-7 0.08138328 -2.084521 6.61367e-7 0.08138328 -0.5614412 0.04069179 0.07047998 -2.084521 0.04069226 0.07047992 -0.5614413 0.0704801 0.04069161 -2.084521 0.07048058 0.04069155 -0.5614413 0.0813834 0 -2.084521 0.08138382 0 -0.5614413 0.0704801 -0.04069167 -2.084521 0.07048046 -0.04069167 -0.5614412 0.04069179 -0.07047998 -2.084521 0.04069209 -0.07047998 -0.5614412 1.54427e-7 -0.08138328 -2.084521 4.49347e-7 -0.08138328 -0.5614412 -0.04069149 -0.07047998 -2.084521 -0.04069113 -0.07047992 -0.5614412 -0.07047981 -0.04069161 -2.084521 -0.07047945 -0.04069155 -2.084521 5.49213e-7 0 -0.5614412 1.61804e-7 0 2.37238e-7 0.5614412 0 -0.08138304 0.5614412 0 -0.07047975 0.5614412 -0.04069161 8.29287e-7 2.084521 0 -0.07047921 2.084521 -0.04069155 -0.08138245 2.084521 0 -0.04069137 0.5614412 -0.07047998 -0.04069083 2.084521 -0.07047992 2.29861e-7 0.5614412 -0.08138328 7.29421e-7 2.084521 -0.08138328 0.04069185 0.5614412 -0.07047998 0.04069238 2.084521 -0.07047998 0.07048016 0.5614413 -0.04069167 0.07048076 2.084521 -0.04069167 0.08138352 0.5614413 0 0.08138412 2.084521 0 0.07048022 0.5614413 0.04069161 0.07048088 2.084521 0.04069155 0.04069191 0.5614412 0.07047998 0.04069256 2.084521 0.07047992 2.86009e-7 0.5614412 0.08138328 9.41441e-7 2.084521 0.08138328 -0.04069131 0.5614412 0.07047998 -0.04069072 2.084521 0.07048004 -0.07047975 0.5614412 0.04069167 -0.07047903 2.084521 0.04069173 -0.3798453 2.886224 0.2218516 7.9091e-7 1.969194 0.1871857 8.67901e-7 2.96178 0.2218515 8.54381e-7 1.969194 0.2620601 -0.7018634 2.671058 0.2218517 -0.9170291 2.349041 0.2218517 -0.9925853 1.969195 0.2218517 -0.9170292 1.589348 0.2218517 -0.7018637 1.26733 0.2218517 -0.3798456 1.052164 0.2218516 6.30322e-7 0.9766083 0.2218515 0.3798469 1.052164 0.2218514 0.7018651 1.26733 0.2218514 0.9170309 1.589348 0.2218513 0.992587 1.969194 0.2218513 0.9170311 2.34904 0.2218513 0.7018656 2.671059 0.2218514 0.3798475 2.886225 0.2218514 0.3369342 0.5615574 -0.2994973 0.3369344 -0.5615572 -0.2994973 -0.3369344 -0.5615574 -0.2994973 -0.3369344 0.5615576 -0.2994973 0.3369346 0.5615574 0.2994973 0.3369344 -0.5615576 0.2994973 -0.3369344 -0.5615574 0.2994973 -0.3369344 0.5615572 0.2994973 0.5615572 0.3369344 -0.2994973 -0.5615572 0.3369347 -0.2994973 0.5615576 0.3369343 0.2994973 -0.5615572 0.3369344 0.2994973 0.5615572 -0.3369344 -0.2994973 -0.5615575 -0.3369343 -0.2994973 0.5615574 -0.3369347 0.2994973 -0.5615575 -0.3369344 0.2994973 - - - - - - - - - - 1 0 0 0.6302378 0 -0.7763603 0.6302378 0.548967 -0.548967 -0.6302378 0.548967 -0.548967 -0.6302378 0 -0.7763603 -1 0 0 0.6302378 0.7763603 0 -0.6302378 0.7763603 0 0.6302378 0.548967 0.548967 -0.6302378 0.548967 0.548967 0.6302378 0 0.7763603 -0.6302378 0 0.7763603 0.6302378 -0.548967 0.548967 -0.6302378 -0.548967 0.548967 0.6302378 -0.7763603 0 -0.6302378 -0.7763603 0 0.6302378 -0.548967 -0.548967 -0.6302378 -0.548967 -0.548967 1 0 0 0.6302378 0 -0.7763603 0.6302378 0.548967 -0.548967 -1 0 0 -0.6302378 0.548967 -0.548967 -0.6302378 0 -0.7763603 0.6302378 0.7763603 0 -0.6302378 0.7763603 0 0.6302378 0.548967 0.548967 -0.6302378 0.548967 0.548967 0.6302378 0 0.7763603 -0.6302378 0 0.7763603 0.6302378 -0.548967 0.548967 -0.6302378 -0.548967 0.548967 0.6302378 -0.7763603 0 -0.6302378 -0.7763603 0 0.6302378 -0.548967 -0.548967 -0.6302378 -0.548967 -0.548967 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 -2.53709e-6 1.46479e-6 1 0 0 -1 -1.46479e-6 -3.92489e-7 1 0 0 -1 -1.46479e-6 3.92489e-7 1 0 0 -1 -2.53709e-6 -1.46479e-6 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 0 0 -1 0.06228822 0.8714866 -0.486404 0.6329844 0.6329844 -0.4456618 0 0 1 0.4264656 0.4264656 0.7976318 -0.06045717 0.6646626 0.7446516 0.8714866 0.06228822 -0.486404 0.6646626 -0.06045717 0.7446516 0.6226387 -0.4941862 -0.6066775 0.4941862 -0.6226387 0.6066775 0.06045717 -0.6646626 -0.7446516 -0.06228822 -0.8714866 0.486404 -0.4264656 -0.4264656 -0.7976318 -0.6329844 -0.6329844 0.4456618 -0.6646626 0.06045717 -0.7446516 -0.8714866 -0.06228822 0.486404 -0.4941862 0.6226387 -0.6066775 -0.6226387 0.4941862 0.6066775 0 0 1 0.6646626 0.06045717 0.7446516 0.4941862 0.6226387 0.6066775 0.6226387 0.4941862 -0.6066775 0.8714866 -0.06228822 -0.486404 0 0 -1 -0.06228822 0.8714866 0.486404 0.06045717 0.6646626 -0.7446516 -0.6329844 0.6329844 0.4456618 -0.4264656 0.4264656 -0.7976318 -0.8714866 0.06228822 0.486404 -0.6646626 -0.06045717 -0.7446516 -0.6226387 -0.4941862 0.6066775 -0.4941862 -0.6226387 -0.6066775 -0.06045717 -0.6646626 0.7446516 0.06228822 -0.8714866 -0.486404 0.4264656 -0.4264656 0.7976318 0.6329844 -0.6329844 -0.4456618 0 0 -1 -0.06228822 -0.8714866 -0.486404 -0.6329844 -0.6329844 -0.4456618 0 0 1 -0.4264656 -0.4264656 0.7976318 0.06045717 -0.6646626 0.7446516 -0.8714866 -0.06228822 -0.486404 -0.6646626 0.06045717 0.7446516 -0.6226387 0.4941862 -0.6066775 -0.4941862 0.6226387 0.6066775 -0.06045717 0.6646626 -0.7446516 0.06228822 0.8714866 0.486404 0.4264656 0.4264656 -0.7976318 0.6329844 0.6329844 0.4456618 0.6646626 -0.06045717 -0.7446516 0.8714866 0.06228822 0.486404 0.4941862 -0.6226387 -0.6066775 0.6226387 -0.4941862 0.6066775 0 0 1 -0.6646626 -0.06045717 0.7446516 -0.4941862 -0.6226387 0.6066775 -0.6226387 -0.4941862 -0.6066775 -0.8714866 0.06228822 -0.486404 0 0 -1 0.06228822 -0.8714866 0.486404 -0.06045717 -0.6646626 -0.7446516 0.6329844 -0.6329844 0.4456618 0.4264656 -0.4264656 -0.7976318 0.8714866 -0.06228822 0.486404 0.6646626 0.06045717 -0.7446516 0.6226387 0.4941862 0.6066775 0.4941862 0.6226387 -0.6066775 0.06045717 0.6646626 0.7446516 -0.06228822 0.8714866 -0.486404 -0.4264656 0.4264656 0.7976318 -0.6329844 0.6329844 -0.4456618 0.03490257 -0.006942212 -0.9993667 0.04047429 -0.008050978 0.9991481 0.03431242 -0.02292704 0.9991481 0.02958893 -0.0197705 -0.9993667 0.01977074 -0.02958875 -0.9993666 0.02292686 -0.03431266 0.9991481 0.008050799 -0.04047453 0.9991481 0.00694251 -0.03490239 -0.9993667 -0.00694251 -0.03490239 -0.9993667 -0.008050799 -0.04047447 0.9991481 -0.02292686 -0.03431266 0.9991481 -0.01977074 -0.02958875 -0.9993666 -0.02958893 -0.01977056 -0.9993666 -0.03431248 -0.02292704 0.9991481 -0.04047429 -0.008051037 0.9991481 -0.03490257 -0.006942331 -0.9993666 -0.03490257 0.006942749 -0.9993666 -0.04047429 0.00805062 0.9991481 -0.03431248 0.02292662 0.999148 -0.02958899 0.01977092 -0.9993666 -0.01977074 0.02958917 -0.9993667 -0.02292686 0.03431224 0.9991482 -0.008050799 0.04047405 0.9991481 -0.00694251 0.03490281 -0.9993667 0.00694251 0.03490281 -0.9993667 0.008050799 0.04047411 0.9991481 0.0229268 0.03431224 0.9991482 0.01977074 0.02958923 -0.9993667 0.02958899 0.01977092 -0.9993667 0.03431248 0.02292662 0.9991481 0.04047429 0.00805062 0.9991481 0.03490257 0.006942749 -0.9993667 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 -2.53709e-6 1 1.46479e-6 0 -1 0 -1.46479e-6 1 -3.92489e-7 0 -1 0 -1.46479e-6 1 3.92489e-7 0 -1 0 -2.53709e-6 1 -1.46479e-6 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0.006942689 -0.03490263 -0.9993667 0.00805068 -0.04047423 0.9991481 0.02292662 -0.03431242 0.9991482 0.01977092 -0.02958899 -0.9993667 0.02958923 -0.01977068 -0.9993667 0.03431218 -0.02292686 0.9991481 0.04047405 -0.008050739 0.9991482 0.03490281 -0.00694257 -0.9993666 0.03490281 0.00694257 -0.9993667 0.04047405 0.008050799 0.9991481 0.03431218 0.02292686 0.9991481 0.02958923 0.01977068 -0.9993667 0.01977092 0.02958899 -0.9993666 0.02292662 0.03431242 0.9991481 0.00805062 0.04047423 0.9991481 0.006942689 0.03490263 -0.9993666 -0.006942212 0.03490263 -0.9993666 -0.008051097 0.04047423 0.9991481 -0.02292704 0.03431242 0.9991481 -0.0197705 0.02958893 -0.9993666 -0.02958869 0.01977074 -0.9993666 -0.03431266 0.0229268 0.9991481 -0.04047447 0.008050858 0.9991481 -0.03490233 0.00694251 -0.9993667 -0.03490233 -0.00694251 -0.9993667 -0.04047447 -0.008050858 0.9991481 -0.03431266 -0.0229268 0.9991481 -0.02958875 -0.01977074 -0.9993667 -0.0197705 -0.02958893 -0.9993667 -0.02292704 -0.03431242 0.9991481 -0.008051037 -0.04047423 0.999148 -0.006942152 -0.03490263 -0.9993667 -0.03490263 0.006942152 -0.9993667 -0.04047423 0.008051037 0.9991481 -0.03431242 0.02292704 0.9991481 -0.02958893 0.0197705 -0.9993667 -0.01977068 0.02958875 -0.9993666 -0.02292674 0.03431272 0.9991482 -0.008050858 0.04047447 0.9991481 -0.006942451 0.03490233 -0.9993666 0.00694251 0.03490239 -0.9993667 0.008050858 0.04047447 0.9991481 0.0229268 0.03431266 0.9991481 0.01977074 0.02958869 -0.9993666 0.02958893 0.0197705 -0.9993666 0.03431248 0.02292698 0.9991481 0.04047423 0.008051097 0.9991481 0.03490263 0.006942212 -0.9993666 0.03490263 -0.006942689 -0.9993666 0.04047423 -0.00805062 0.9991481 0.03431242 -0.02292662 0.9991482 0.02958899 -0.01977092 -0.9993667 0.01977068 -0.02958923 -0.9993667 0.02292686 -0.03431218 0.9991481 0.008050799 -0.04047405 0.9991481 0.00694257 -0.03490281 -0.9993667 -0.00694257 -0.03490281 -0.9993667 -0.008050799 -0.04047405 0.9991481 -0.02292686 -0.03431218 0.9991482 -0.01977068 -0.02958917 -0.9993667 -0.02958899 -0.01977092 -0.9993667 -0.03431242 -0.02292662 0.9991482 -0.04047423 -0.00805068 0.9991481 -0.03490263 -0.006942689 -0.9993667 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 2.53709e-6 -1.46479e-6 -1 0 0 1 1.46479e-6 3.92489e-7 -1 0 0 1 1.46479e-6 -3.92489e-7 -1 0 0 1 2.53709e-6 1.46479e-6 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 2.53709e-6 -1 1.46479e-6 0 1 0 1.46479e-6 -1 -3.92489e-7 0 1 0 1.46479e-6 -1 3.92489e-7 0 1 0 2.53709e-6 -1 -1.46479e-6 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 -0.006942689 0.03490263 -0.9993667 -0.00805068 0.04047423 0.9991481 -0.02292662 0.03431242 0.9991482 -0.01977092 0.02958899 -0.9993667 -0.02958917 0.01977074 -0.9993666 -0.03431218 0.02292692 0.9991483 -0.04047405 0.008050799 0.9991482 -0.03490281 0.00694257 -0.9993666 -0.03490281 -0.00694257 -0.9993667 -0.04047405 -0.008050799 0.9991481 -0.03431218 -0.02292686 0.9991481 -0.02958923 -0.01977068 -0.9993666 -0.01977092 -0.02958899 -0.9993667 -0.02292662 -0.03431242 0.9991481 -0.00805062 -0.04047423 0.9991481 -0.006942689 -0.03490263 -0.9993666 0.006942212 -0.03490263 -0.9993666 0.008051097 -0.04047423 0.9991481 0.02292698 -0.03431248 0.9991482 0.0197705 -0.02958899 -0.9993667 0.02958869 -0.01977074 -0.9993667 0.03431266 -0.0229268 0.999148 0.04047447 -0.008050858 0.9991481 0.03490233 -0.00694251 -0.9993667 0.03490239 0.006942451 -0.9993667 0.04047447 0.008050858 0.9991481 0.03431272 0.02292674 0.9991481 0.02958875 0.01977068 -0.9993667 0.0197705 0.02958893 -0.9993667 0.02292704 0.03431242 0.9991481 0.008051037 0.04047423 0.9991481 0.006942152 0.03490263 -0.9993667 -1 3.53806e-7 0 -1 3.53806e-7 0 -0.7071067 -0.707107 -1.40725e-7 -0.7071068 -0.7071068 0 0.7071068 -0.7071069 -4.92539e-7 0.7071067 -0.7071068 -4.8945e-7 1 -3.53806e-7 -1.99016e-7 1 0 -5.97047e-7 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.7071067 0.7071068 3.9156e-7 -0.7071068 0.7071067 4.92539e-7 0.7071067 0.7071068 -5.2772e-7 0.707107 0.7071065 -3.13248e-7 0 0 1 0 0 1 0 0 -1 0 0 -1 -2.65354e-7 1 0 3.53806e-7 1 6.96555e-7 -3.53806e-7 -1 -6.96555e-7 2.65354e-7 -1 0 -0.6557207 -0.6532182 0.3785516 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0 -0.6557207 0.6532182 0.3785516 -0.3785516 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 0 -0.6532182 0.7571337 0 0.6532182 0.7571337 0.3785516 -0.6532182 0.6557207 0.3785821 0.6532182 0.6557207 0.6557207 -0.6532182 0.3785516 0.6557207 0.6532182 0.3785516 0.7571337 -0.6532182 0 0.7571337 0.6532182 0 0.6557207 -0.6532182 -0.3785516 0.6557207 0.6532182 -0.3785516 0.3785516 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0 -0.6532182 -0.7571337 0 0.6532182 -0.7571337 -0.3785516 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.6557207 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 0.6532182 -0.7571337 0 -0.6532182 -0.7571337 0 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 0 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0.3785516 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 -0.3785516 -0.6532182 0.7571337 0 0.6532182 0.7571337 0 -0.6532182 0.6557207 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.3785821 0.6557207 0.6532182 0.3785821 0.6557207 -0.6532182 0 0.7571337 0.6532182 0 0.7571337 -0.6532182 -0.3785516 0.6557207 0.6532182 -0.3785516 0.6557207 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0.3785516 0.6557207 0.6532182 0.3785516 0.7571337 -0.6532182 0 0.7571337 0.6532182 0 0.6557207 -0.6532182 0.3785516 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.6557207 0 0.6532182 0.7571337 0 -0.6532182 0.7571337 -0.3785821 0.6532182 0.6557207 -0.3785821 -0.6532182 0.6557207 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.3785516 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 -0.3785516 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.6557207 0 0.6532182 -0.7571337 0 -0.6532182 -0.7571337 0.3785516 0.6532182 -0.6557207 0.3785516 -0.6532182 -0.6557207 0.6557207 0.6532182 -0.3785516 0.6557207 -0.6532182 -0.3785516 0.6532182 0.7571337 0 -0.6532182 0.7571337 0 -0.6532182 0.6557207 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.3785516 0.6557207 0.6532182 0.3785516 0.6557207 -0.6532182 0 0.7571337 0.6532182 0 0.7571337 -0.6532182 -0.3785821 0.6556902 0.6532182 -0.3785821 0.6557207 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0.3785516 -0.6532182 -0.7571337 0 0.6532182 -0.7571337 0 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 0 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0.3785516 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 -0.3785516 - - - - - - - - - - - - - - - 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 -

60 36 36 36 37 36 61 37 49 37 48 37 60 38 37 38 38 38 61 39 50 39 49 39 60 40 38 40 39 40 61 41 51 41 50 41 60 42 39 42 40 42 61 43 52 43 51 43 60 44 40 44 41 44 61 45 53 45 52 45 60 46 41 46 42 46 61 47 54 47 53 47 60 48 42 48 43 48 61 49 55 49 54 49 60 50 43 50 44 50 61 51 56 51 55 51 60 52 44 52 45 52 61 53 57 53 56 53 60 54 45 54 46 54 61 55 58 55 57 55 60 56 46 56 47 56 61 57 59 57 58 57 47 58 36 58 60 58 61 59 48 59 59 59 48 432 36 433 47 434 48 432 47 434 59 435 46 436 58 437 59 435 46 436 59 435 47 434 45 438 57 439 58 437 45 438 58 437 46 436 44 440 56 441 57 439 44 440 57 439 45 438 43 442 55 443 56 441 43 442 56 441 44 440 42 444 54 445 55 443 42 444 55 443 43 442 41 446 53 447 54 445 41 446 54 445 42 444 40 448 52 449 53 447 40 448 53 447 41 446 39 450 51 451 52 449 39 450 52 449 40 448 38 452 50 453 51 451 38 452 51 451 39 450 37 454 49 455 50 453 37 454 50 453 38 452 36 433 48 432 49 455 36 433 49 455 37 454

- - - - - 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 -

151 132 135 132 134 132 137 133 135 133 151 133 136 136 138 136 134 136 137 137 138 137 136 137 139 140 140 140 134 140 137 141 140 141 139 141 141 144 142 144 134 144 137 145 142 145 141 145 143 148 144 148 134 148 137 149 144 149 143 149 145 152 146 152 134 152 137 153 146 153 145 153 147 156 148 156 134 156 137 157 148 157 147 157 149 160 150 160 134 160 137 161 150 161 149 161 181 190 178 190 182 190 182 191 178 191 179 191 181 194 183 194 184 194 184 195 183 195 179 195 181 198 185 198 186 198 186 199 185 199 179 199 181 202 187 202 188 202 188 203 187 203 179 203 181 206 189 206 190 206 190 207 189 207 179 207 181 210 191 210 192 210 192 211 191 211 179 211 181 214 193 214 194 214 194 215 193 215 179 215 181 218 195 218 180 218 180 219 195 219 179 219 196 220 197 220 198 220 199 221 197 221 196 221 200 224 201 224 198 224 199 225 201 225 200 225 202 228 203 228 198 228 199 229 203 229 202 229 204 232 205 232 198 232 199 233 205 233 204 233 206 236 207 236 198 236 199 237 207 237 206 237 208 240 209 240 198 240 199 241 209 241 208 241 210 244 211 244 198 244 199 245 211 245 210 245 212 248 213 248 198 248 199 249 213 249 212 249 269 302 266 302 270 302 270 303 266 303 267 303 269 306 271 306 272 306 272 307 271 307 267 307 269 310 273 310 274 310 274 311 273 311 267 311 269 314 275 314 276 314 276 315 275 315 267 315 269 318 277 318 278 318 278 319 277 319 267 319 269 322 279 322 280 322 280 323 279 323 267 323 269 326 281 326 282 326 282 327 281 327 267 327 269 330 283 330 268 330 268 331 283 331 267 331

- - - - - 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 -

16 0 0 1 3 2 2 3 1 4 17 5 16 0 3 2 5 6 17 5 4 7 2 3 16 0 5 6 7 8 17 5 6 9 4 7 16 0 7 8 9 10 17 5 8 11 6 9 16 0 9 10 11 12 17 5 10 13 8 11 16 0 11 12 13 14 17 5 12 15 10 13 16 0 13 14 15 16 17 5 14 17 12 15 15 16 0 1 16 0 17 5 1 4 14 17 18 18 20 19 19 20 21 21 23 22 22 23 18 18 19 20 24 24 21 21 25 25 23 22 18 18 24 24 26 26 21 21 27 27 25 25 18 18 26 26 28 28 21 21 29 29 27 27 18 18 28 28 30 30 21 21 31 31 29 29 18 18 30 30 32 32 21 21 33 33 31 31 18 18 32 32 34 34 21 21 35 35 33 33 34 34 20 19 18 18 35 35 21 21 22 23 78 60 62 61 63 62 79 63 71 64 70 65 78 60 63 62 64 66 79 63 72 67 71 64 78 60 64 66 65 68 79 63 73 69 72 67 78 60 65 68 66 70 79 63 74 71 73 69 78 60 66 70 67 72 79 63 75 73 74 71 78 60 67 72 68 74 79 63 76 75 75 73 78 60 68 74 69 76 79 63 77 77 76 75 69 76 62 61 78 60 79 63 70 65 77 77 96 78 80 79 83 80 82 81 81 82 97 83 96 78 83 80 85 84 97 83 84 85 82 81 96 78 85 84 87 86 97 83 86 87 84 85 96 78 87 86 89 88 97 83 88 89 86 87 96 78 89 88 91 90 97 83 90 91 88 89 96 78 91 90 93 92 97 83 92 93 90 91 96 78 93 92 95 94 97 83 94 95 92 93 96 78 95 94 80 79 97 83 81 82 94 95 98 96 100 97 99 98 101 99 103 100 102 101 98 96 99 98 104 102 101 99 105 103 103 100 98 96 104 102 106 104 101 99 107 105 105 103 98 96 106 104 108 106 101 99 109 107 107 105 98 96 108 106 110 108 101 99 111 109 109 107 98 96 110 108 112 110 101 99 113 111 111 109 98 96 112 110 114 112 101 99 115 113 113 111 114 112 100 97 98 96 101 99 102 101 115 113 132 114 116 115 119 116 118 117 117 118 133 119 132 114 119 116 121 120 133 119 120 121 118 117 132 114 121 120 123 122 133 119 122 123 120 121 132 114 123 122 125 124 133 119 124 125 122 123 132 114 125 124 127 126 133 119 126 127 124 125 132 114 127 126 129 128 133 119 128 129 126 127 132 114 129 128 131 130 133 119 130 131 128 129 132 114 131 130 116 115 133 119 117 118 130 131 152 164 153 164 154 164 155 165 156 165 157 165 152 166 154 166 158 166 155 167 159 167 156 167 152 168 158 168 160 168 155 169 161 169 159 169 152 170 160 170 162 170 155 171 163 171 161 171 152 172 162 172 164 172 155 173 165 173 163 173 152 174 164 174 166 174 155 175 167 175 165 175 152 176 166 176 168 176 155 177 169 177 167 177 152 178 168 178 170 178 155 179 171 179 169 179 152 180 170 180 172 180 155 181 173 181 171 181 152 182 172 182 174 182 155 183 175 183 173 183 152 184 174 184 176 184 155 185 177 185 175 185 152 186 176 186 153 186 155 187 157 187 177 187 238 252 216 252 217 252 239 253 214 253 215 253 238 254 217 254 219 254 239 255 218 255 214 255 238 256 219 256 221 256 239 257 220 257 218 257 238 258 221 258 223 258 239 259 222 259 220 259 238 260 223 260 225 260 239 261 224 261 222 261 238 262 225 262 227 262 239 263 226 263 224 263 238 264 227 264 229 264 239 265 228 265 226 265 238 266 229 266 231 266 239 267 230 267 228 267 238 268 231 268 233 268 239 269 232 269 230 269 238 270 233 270 235 270 239 271 234 271 232 271 238 272 235 272 237 272 239 273 236 273 234 273 238 274 237 274 216 274 239 275 215 275 236 275 240 276 241 276 242 276 243 277 244 277 245 277 240 278 242 278 246 278 243 279 247 279 244 279 240 280 246 280 248 280 243 281 249 281 247 281 240 282 248 282 250 282 243 283 251 283 249 283 240 284 250 284 252 284 243 285 253 285 251 285 240 286 252 286 254 286 243 287 255 287 253 287 240 288 254 288 256 288 243 289 257 289 255 289 240 290 256 290 258 290 243 291 259 291 257 291 240 292 258 292 260 292 243 293 261 293 259 293 240 294 260 294 262 294 243 295 263 295 261 295 240 296 262 296 264 296 243 297 265 297 263 297 240 298 264 298 241 298 243 299 245 299 265 299 299 332 295 332 297 332 295 333 293 333 297 333 290 334 299 334 297 334 290 335 297 335 286 335 298 336 289 336 285 336 298 337 285 337 296 337 294 338 298 338 296 338 294 339 296 339 292 339 299 340 290 340 298 340 290 341 289 341 298 341 295 342 299 342 298 342 295 343 298 343 294 343 296 344 285 344 286 344 296 345 286 345 297 345 292 346 296 346 297 346 292 347 297 347 293 347 295 348 291 348 293 348 291 349 287 349 293 349 292 350 284 350 288 350 292 351 288 351 294 351 291 352 295 352 288 352 295 353 294 353 288 353 284 354 292 354 293 354 284 355 293 355 287 355 288 356 284 356 291 356 284 357 287 357 291 357 285 358 289 358 290 358 285 359 290 359 286 359 264 360 245 361 241 362 264 360 265 363 245 361 262 364 265 363 264 360 262 364 263 365 265 363 260 366 263 365 262 364 260 366 261 367 263 365 258 368 261 367 260 366 258 368 259 369 261 367 256 370 259 369 258 368 256 370 257 371 259 369 254 372 257 371 256 370 254 372 255 373 257 371 252 374 255 373 254 372 252 374 253 375 255 373 250 376 253 375 252 374 250 376 251 377 253 375 248 378 251 377 250 376 248 378 249 379 251 377 246 380 249 379 248 378 246 380 247 381 249 379 242 382 247 381 246 380 242 382 244 383 247 381 241 362 244 383 242 382 241 362 245 361 244 383 215 384 216 385 237 386 215 384 237 386 236 387 236 387 237 386 235 388 236 387 235 388 234 389 234 389 235 388 233 390 234 389 233 390 232 391 232 391 233 390 231 392 232 391 231 392 230 393 230 393 231 392 229 394 230 393 229 394 228 395 228 395 229 394 227 396 228 395 227 396 226 397 226 397 227 396 225 398 226 397 225 398 224 399 224 399 225 398 223 400 224 399 223 400 222 401 222 401 223 400 221 402 222 401 221 402 220 403 220 403 221 402 219 404 220 403 219 404 218 405 218 405 219 404 217 406 218 405 217 406 214 407 214 407 217 406 216 385 214 407 216 385 215 384 176 408 157 409 153 410 176 408 177 411 157 409 174 412 177 411 176 408 174 412 175 413 177 411 172 414 175 413 174 412 172 414 173 415 175 413 170 416 173 415 172 414 170 416 171 417 173 415 168 418 171 417 170 416 168 418 169 419 171 417 166 420 169 419 168 418 166 420 167 421 169 419 164 422 167 421 166 420 164 422 165 423 167 421 162 424 165 423 164 422 162 424 163 425 165 423 160 426 163 425 162 424 160 426 161 427 163 425 158 428 161 427 160 426 158 428 159 429 161 427 154 430 159 429 158 428 154 430 156 431 159 429 153 410 156 431 154 430 153 410 157 409 156 431 117 118 131 130 130 131 117 118 116 115 131 130 130 131 131 130 128 129 128 129 131 130 129 128 128 129 129 128 126 127 126 127 129 128 127 126 126 127 127 126 124 125 124 125 127 126 125 124 124 125 125 124 122 123 122 123 125 124 123 122 122 123 121 120 120 121 122 123 123 122 121 120 120 121 119 116 118 117 120 121 121 120 119 116 116 115 118 117 119 116 116 115 117 118 118 117 102 101 100 97 114 112 102 101 114 112 115 113 112 110 113 111 115 113 112 110 115 113 114 112 110 108 111 109 113 111 110 108 113 111 112 110 108 106 109 107 110 108 109 107 111 109 110 108 106 104 107 105 108 106 107 105 109 107 108 106 104 102 105 103 106 104 105 103 107 105 106 104 99 98 103 100 104 102 103 100 105 103 104 102 100 97 102 101 103 100 100 97 103 100 99 98 81 82 95 94 94 95 81 82 80 79 95 94 94 95 95 94 92 93 92 93 95 94 93 92 92 93 93 92 90 91 90 91 93 92 91 90 90 91 91 90 88 89 88 89 91 90 89 88 88 89 89 88 86 87 86 87 89 88 87 86 86 87 85 84 84 85 86 87 87 86 85 84 84 85 83 80 82 81 84 85 85 84 83 80 80 79 82 81 83 80 80 79 81 82 82 81 70 65 62 61 69 76 70 65 69 76 77 77 68 74 76 75 77 77 68 74 77 77 69 76 67 72 75 73 76 75 67 72 76 75 68 74 66 70 74 71 67 72 74 71 75 73 67 72 65 68 73 69 66 70 73 69 74 71 66 70 64 66 72 67 65 68 72 67 73 69 65 68 63 62 71 64 64 66 71 64 72 67 64 66 62 61 70 65 71 64 62 61 71 64 63 62 22 23 20 19 34 34 22 23 34 34 35 35 32 32 33 33 35 35 32 32 35 35 34 34 30 30 31 31 33 33 30 30 33 33 32 32 28 28 29 29 31 31 28 28 31 31 30 30 26 26 27 27 29 29 26 26 29 29 28 28 24 24 25 25 27 27 24 24 27 27 26 26 19 20 23 22 25 25 19 20 25 25 24 24 23 22 19 20 20 19 23 22 20 19 22 23 1 4 0 1 15 16 15 16 14 17 1 4 14 17 13 14 12 15 14 17 15 16 13 14 12 15 11 12 10 13 12 15 13 14 11 12 10 13 9 10 8 11 10 13 11 12 9 10 8 11 7 8 6 9 8 11 9 10 7 8 6 9 5 6 4 7 6 9 7 8 5 6 4 7 3 2 2 3 4 7 5 6 3 2 0 1 2 3 3 2 0 1 1 4 2 3

- - - - - 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 -

137 134 136 134 135 134 135 135 136 135 134 135 137 138 139 138 138 138 138 139 139 139 134 139 137 142 141 142 140 142 140 143 141 143 134 143 137 146 143 146 142 146 142 147 143 147 134 147 137 150 145 150 144 150 144 151 145 151 134 151 137 154 147 154 146 154 146 155 147 155 134 155 137 158 149 158 148 158 148 159 149 159 134 159 137 162 151 162 150 162 150 163 151 163 134 163 178 188 180 188 179 188 181 189 180 189 178 189 183 192 182 192 179 192 181 193 182 193 183 193 185 196 184 196 179 196 181 197 184 197 185 197 187 200 186 200 179 200 181 201 186 201 187 201 189 204 188 204 179 204 181 205 188 205 189 205 191 208 190 208 179 208 181 209 190 209 191 209 193 212 192 212 179 212 181 213 192 213 193 213 195 216 194 216 179 216 181 217 194 217 195 217 199 222 200 222 197 222 197 223 200 223 198 223 199 226 202 226 201 226 201 227 202 227 198 227 199 230 204 230 203 230 203 231 204 231 198 231 199 234 206 234 205 234 205 235 206 235 198 235 199 238 208 238 207 238 207 239 208 239 198 239 199 242 210 242 209 242 209 243 210 243 198 243 199 246 212 246 211 246 211 247 212 247 198 247 199 250 196 250 213 250 213 251 196 251 198 251 266 300 268 300 267 300 269 301 268 301 266 301 271 304 270 304 267 304 269 305 270 305 271 305 273 308 272 308 267 308 269 309 272 309 273 309 275 312 274 312 267 312 269 313 274 313 275 313 277 316 276 316 267 316 269 317 276 317 277 317 279 320 278 320 267 320 269 321 278 321 279 321 281 324 280 324 267 324 269 325 280 325 281 325 283 328 282 328 267 328 269 329 282 329 283 329

- - - 1 - - - - - - 0 0 0 - 0 0 1 0 - 0 1 0 0 - 1 0 0 0 - 0.1335572 0.1335572 0.1335572 - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/multi_uav_simulator/bigquad_description/bigquad.urdf.xacro b/multi_uav_simulator/bigquad_description/bigquad.urdf.xacro deleted file mode 100644 index d45fb85..0000000 --- a/multi_uav_simulator/bigquad_description/bigquad.urdf.xacro +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/multi_uav_simulator/cf_description/crazyflie.skp b/multi_uav_simulator/cf_description/crazyflie.skp deleted file mode 100755 index 0a37b20..0000000 Binary files a/multi_uav_simulator/cf_description/crazyflie.skp and /dev/null differ diff --git a/multi_uav_simulator/config/crazyflie_params.yaml b/multi_uav_simulator/config/crazyflie_params.yaml deleted file mode 100644 index 98642bb..0000000 --- a/multi_uav_simulator/config/crazyflie_params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -controller: - gains: [1, 0.5, 0.0015, 0.00075] - -# model parameters for crazyflie drone -model: - J: [2.3951e-5, 2.3951e-5, 3.2347e-5] - m: 0.032 - d: 0.08 - ctf: 0.0037 - gravity: 9.81 \ No newline at end of file diff --git a/multi_uav_simulator/config/initial_conditions.yaml b/multi_uav_simulator/config/initial_conditions.yaml deleted file mode 100644 index 6d593b6..0000000 --- a/multi_uav_simulator/config/initial_conditions.yaml +++ /dev/null @@ -1,32 +0,0 @@ -# define the initial positions of each robot in NWU frame -# change the initial rotation matrix to [1, 0, 0, 0, 1, 0, 0, 0, 1] for initialing the -# drones right side up. -robot_1: - position: [2, 2, -2] - velocity: [0, 0, 0] - rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] - omega: [0, 0, 0] - -robot_2: - position: [-2, -2, -2] - velocity: [0, 0, 0] - rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] - omega: [0, 0, 0] - -robot_3: - position: [-1, 1, -2] - velocity: [0, 0, 0] - rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] - omega: [0, 0, 0] - -robot_4: - position: [1, -1, -2] - velocity: [0, 0, 0] - rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] - omega: [0, 0, 0] - -robot_5: - position: [0, 0, -2] - velocity: [0, 0, 0] - rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] - omega: [0, 0, 0] \ No newline at end of file diff --git a/multi_uav_simulator/config/simulation.rviz b/multi_uav_simulator/config/simulation.rviz deleted file mode 100644 index 6c69e16..0000000 --- a/multi_uav_simulator/config/simulation.rviz +++ /dev/null @@ -1,312 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.6470588445663452 - Tree Height: 557 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 20 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot1 - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Class: rviz/Axes - Enabled: true - Length: 0.5 - Name: Axes - Radius: 0.05000000074505806 - Reference Frame: map - Show Trail: false - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot2 - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot3 - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot4 - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link2/path - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link4/path - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link3/path - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link1/path - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot5 - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link5/path - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link1/goal - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link2/goal - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link3/goal - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link4/goal - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /base_link5/goal - Name: Marker - Namespaces: - "": true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: map - Frame Rate: 10 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 8.682811737060547 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.7853981852531433 - Focal Point: - X: 0.24857836961746216 - Y: -0.6647985577583313 - Z: 0.5124149322509766 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6653982996940613 - Target Frame: - Yaw: 2.915405035018921 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 1015 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000359fc0200000008fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df000001d300000111fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000de000002b8000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002dc000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004f30000035900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: true - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1267 - X: 2413 - Y: 156 diff --git a/multi_uav_simulator/include/multi_uav_simulator/DynamicsProvider.h b/multi_uav_simulator/include/multi_uav_simulator/DynamicsProvider.h deleted file mode 100644 index 45da5ca..0000000 --- a/multi_uav_simulator/include/multi_uav_simulator/DynamicsProvider.h +++ /dev/null @@ -1,41 +0,0 @@ -// -// Created by malintha on 7/31/18. -// - -#ifndef CF_SIMULATOR_DYNAMICSPROVIDER_H -#define CF_SIMULATOR_DYNAMICSPROVIDER_H - -#include -#include "tf/tf.h" -#include "simulator_utils/simulator_utils.h" -#include - -using namespace Eigen; - -class DynamicsProvider { - -public: - DynamicsProvider(params_t params, init_vals_t init_vals); - std::vector getCurrentState(); - state_space_t get_state(); - void update(control_out_t control, double sim_time); - void reset_dynamics(); - -private: - const size_t DIMENSIONS = 18; - const gsl_odeiv2_step_type *T = gsl_odeiv2_step_rkf45; - - bool state_set = false; - state_space_t state_space; - params_t params; - init_vals_t init_vals; - double sim_time, t_start; - static int step(double t, const double* y, double* f, void *params); - gsl_odeiv2_step* s; - gsl_odeiv2_control *c; - gsl_odeiv2_evolve *e; - -}; - - -#endif \ No newline at end of file diff --git a/multi_uav_simulator/include/multi_uav_simulator/Quadrotor.h b/multi_uav_simulator/include/multi_uav_simulator/Quadrotor.h deleted file mode 100644 index 0771d2f..0000000 --- a/multi_uav_simulator/include/multi_uav_simulator/Quadrotor.h +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef PROJECT_QUADROTOR_H -#define PROJECT_QUADROTOR_H - -#include -#include -#include -#include -#include "DynamicsProvider.h" -#include "tf/tf.h" -#include -#include "ros/ros.h" -#include "simulator_utils/Waypoint.h" -#include "Trajectory_t.h" - -#include -#include - -#ifndef state -#define state -enum State { - Idle, - TakingOff, - Landing, - Hover, - Autonomous -}; -#endif - -using namespace Eigen; -using namespace std; -#include "std_msgs/Float32MultiArray.h" - - -class Quadrotor { -public: - Quadrotor(int robot_id, double frequency, ros::NodeHandle &n); - void move(const desired_state_t &d_state); - void setState(State m_state); - State getState(); - - bool initialize(double dt); - - void desired_pos_cb(const geometry_msgs::Point &pt); - - void iteration(const ros::TimerEvent &e); - void run(); - -private: - - State m_state; - double sim_time, tau, dt, frequency; - int robot_id; - std::string worldframe, localframe, robot_link_name; - gains_t gains; - std::vector traj_piece; - int xd_it; - Vector3d u, target_pos; - state_space_t x0, xd0; - - bool set_init_target; - geometry_msgs::Point target_next; - bool set_next_target = false; - // Vector3d ; - mav_trajectory_generation::Trajectory traj; - // mav_trajectory_generation::Trajectory traj_temp; - - ControllerImpl *controller; - state_space_t state_space; - ros::NodeHandle nh; - - DynamicsProvider *dynamics; - params_t params; - init_vals_t init_vals; - - ros::Publisher marker_pub, goal_pub, state_pub; - visualization_msgs::Marker m, g; - std::vector points; - ros::Subscriber desired_state_sub; - vector positions; - - bool quad_initialized = false; - // Vector3d xd; - - void do_rhp(); - void initPaths(); - void set_state_space(); - bool load_params(); - bool load_init_vals(); - void send_transform(); - void publish_path(); - void publish_state(); - static mav_trajectory_generation::Trajectory get_opt_traj(const opt_t &ps, const Vector3d& pe); -}; - -#endif diff --git a/multi_uav_simulator/include/multi_uav_simulator/Trajectory_t.h b/multi_uav_simulator/include/multi_uav_simulator/Trajectory_t.h deleted file mode 100644 index f1f27f6..0000000 --- a/multi_uav_simulator/include/multi_uav_simulator/Trajectory_t.h +++ /dev/null @@ -1,47 +0,0 @@ -// -// Created by malintha on 10/16/20. -// -#include -#include -#include -#include "std_msgs/Float32MultiArray.h" - -using namespace std; -using namespace Eigen; - -#ifndef MRF_DYNAMICS_TRAJECTORY_T_H -#define MRF_DYNAMICS_TRAJECTORY_T_H - -struct Trajectory_t { - vector xc; - vector yc; - vector zc; - int D; - Trajectory_t(const std_msgs::Float32MultiArrayConstPtr& coeffs); - Trajectory_t(); - Vector3d eval(double d); - -}; - -Trajectory_t::Trajectory_t() = default; - -Trajectory_t::Trajectory_t(const std_msgs::Float32MultiArrayConstPtr& coeffs) { - this->D = coeffs->data.size()/3; - for(int j=0;jdata[j]); - yc.push_back(coeffs->data[j+ D]); - zc.push_back(coeffs->data[j+ 2*D]); - } -} - -Vector3d Trajectory_t::eval(double t) { - Vector3d x = {0, 0, 0}; - for(int i=0;i -#include -#include "ros/ros.h" -#include -#include -#include -#include -#include "tf/tf.h" - -using namespace Eigen; - -namespace cfSimUtils { - /** - * @param gamma roll - * @param beta pitch - * @param alpha yaw - */ - - struct node { - int id, cluster; - float x, y, z; - std::string type; - node(int id, int cluster, float x, float y, float z, std::string type):id(id), cluster(cluster), x(x), y(y), - z(z), type(type) {}; - }; - - Matrix3d getR(double gamma, double beta, double alpha); - double getTargetPWM(double targetThrust); - Vector3d transformtoRPY(tf::Transform transform); - std::vector > retrievePaths(); - std::vector retrieveTopologyConfig(); - -} - -#endif diff --git a/multi_uav_simulator/package.xml b/multi_uav_simulator/package.xml deleted file mode 100755 index 1dbae14..0000000 --- a/multi_uav_simulator/package.xml +++ /dev/null @@ -1,70 +0,0 @@ - - - multi_uav_simulator - 0.0.0 - The cf_simulator package - - - - - malintha fernando - - - - - - MIT - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - geo_controller - roscpp - std_msgs - roscpp - std_msgs - geo_controller - mav_trajectory_generation - simulator_utils - - - - - - - - - diff --git a/multi_uav_simulator/src/Quadrotor.cpp b/multi_uav_simulator/src/Quadrotor.cpp deleted file mode 100644 index 03abe9b..0000000 --- a/multi_uav_simulator/src/Quadrotor.cpp +++ /dev/null @@ -1,353 +0,0 @@ -// -// Created by malintha on 8/22/18. -// -#include "Quadrotor.h" -#include -#include -#include -#include "simulator_utils/simulator_utils.h" - -mav_trajectory_generation::Trajectory Quadrotor::get_opt_traj(const opt_t &ps, const Vector3d& pe) { - mav_trajectory_generation::Vertex::Vector vertices; - mav_trajectory_generation::Vertex v_s(3), v_e(3); - const int derivative_to_optimize = mav_trajectory_generation::derivative_order::JERK; - - v_s.addConstraint(mav_trajectory_generation::derivative_order::POSITION, ps.position); - v_s.addConstraint(mav_trajectory_generation::derivative_order::VELOCITY, ps.velocity); - v_s.addConstraint(mav_trajectory_generation::derivative_order::ACCELERATION, ps.acceleration); - - v_e.makeStartOrEnd(pe, derivative_to_optimize); - - vertices.push_back(v_s); - vertices.push_back(v_e); - - mav_trajectory_generation::PolynomialOptimization<8> opt(3); - std::vector segment_times; - const double v_max = 1; - const double a_max = 4; - segment_times = estimateSegmentTimes(vertices, v_max, a_max); - ROS_DEBUG_STREAM("segement times: "<dt = 1/frequency; - this->initialize(1 / frequency); - this->u << 0,0,0; - - while (marker_pub.getNumSubscribers() < 1) { - ROS_INFO("Waiting for subscriber"); - ros::Duration(1).sleep(); - } -} - -bool Quadrotor::initialize(double dt_) { - m_state = State::Idle; - // load quad params - if (!load_params()) { - ROS_ERROR_STREAM("Could not load the drone parameters"); - return false; - } - // load init params - if (!load_init_vals()) { - ROS_ERROR_STREAM("Could not load the drone initial values"); - return false; - } - - stringstream ss; - ss << localframe << to_string(robot_id); - robot_link_name = ss.str(); - - // set initial values - state_space.position = init_vals.position; - state_space.velocity = init_vals.velocity; - state_space.R = init_vals.R; - state_space.omega = init_vals.omega; - - dynamics = new DynamicsProvider(params, init_vals); - controller = new ControllerImpl(params, init_vals, gains, dt_); - - this->setState(State::Autonomous); - initPaths(); - - desired_state_sub = nh.subscribe("desired_state", 10, &Quadrotor::desired_pos_cb, this); - state_pub = nh.advertise("current_state", 10); - ROS_DEBUG_STREAM("Drone initialized " << robot_id); - ROS_INFO_STREAM("Desired state subscriber topic: " << "robot_"<set_init_target = false; - return true; -} - -void Quadrotor::initPaths() { - marker_pub = nh.advertise( - ros::names::append(robot_link_name, "path"), 10); - - goal_pub = nh.advertise( - ros::names::append(robot_link_name, "goal"), 10); - g.header.stamp = m.header.stamp = ros::Time::now(); - g.header.frame_id = m.header.frame_id = worldframe; - m.type = visualization_msgs::Marker::LINE_STRIP; - - m.action = visualization_msgs::Marker::ADD; - m.id = this->robot_id; - m.color.r = 1; - m.color.g = 0.0; - m.color.b = 0.0; - m.color.a = 1; - m.scale.x = 0.05; - m.pose.orientation.w = 1.0; - - g.id = this->robot_id + 20; - g.type = visualization_msgs::Marker::CUBE; - g.action = visualization_msgs::Marker::ADD; - g.color.r = 1; - g.color.g = 0; - g.color.b = 0; - g.color.a = 0.5; - g.scale.x = 0.1; - g.scale.y = 0.1; - g.scale.z = 0.1; - -} - -void Quadrotor::setState(State m_state_) { - this->m_state = m_state_; -} - -void Quadrotor::move(const desired_state_t &d_state) { - control_out_t control = controller->get_control(dynamics->get_state(), d_state); - dynamics->update(control, sim_time); - - // updating the model on rviz - set_state_space(); - send_transform(); - dynamics->reset_dynamics(); - sim_time += dt; -} - -/** - * converts the dynamics state_space (NED) to NWU and stores for the quadrotor - * This is for broadcasting the transformation -*/ -void Quadrotor::set_state_space() { - state_space_t ss = dynamics->get_state(); - state_space.position = simulator_utils::ned_nwu_rotation(ss.position); - state_space.R = simulator_utils::ned_nwu_rotation(ss.R); - state_space.velocity = simulator_utils::ned_nwu_rotation(ss.velocity); - state_space.omega = simulator_utils::ned_nwu_rotation(ss.omega); -} - -void Quadrotor::send_transform() { - static tf::TransformBroadcaster br; - tf::Transform transform; - Vector3d position = state_space.position; - Matrix3d R = state_space.R; - Vector3d rpy = simulator_utils::R2RPY(R); - transform.setOrigin(tf::Vector3(position[0], position[1], -position[2])); - tf::Quaternion q; - q.setRPY(rpy[0], rpy[1], rpy[2]); - transform.setRotation(q); - if(quad_initialized && !isnan(q.x())) - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), worldframe, robot_link_name)); -} - -State Quadrotor::getState() { - return this->m_state; -} - -bool Quadrotor::load_params() { - stringstream ss; - ss << "/robot_" << to_string(this->robot_id); - string robot_name = ss.str(); - double temp; - if (!nh.getParam(ros::names::append(robot_name, "drone/model/gravity"), params.gravity)) return false; - // = temp; - vector J_; - if (!nh.getParam(ros::names::append(robot_name, "drone/model/J"), J_)) return false; - // Matrix3d J; - params.J << J_[0], 0, 0, - 0, J_[1], 0, - 0, 0, J_[2]; - params.J_inv = params.J.inverse(); - if (!nh.getParam(ros::names::append(robot_name, "drone/model/m"), params.mass)) return false; - // = temp; - params.F = 0; - Vector3d M(0, 0, 0); - params.M = M; - vector gains_; - if (!nh.getParam(ros::names::append(robot_name, "drone/controller/gains"), gains_)) return false; - this->gains = {gains_[0], gains_[1], gains_[2], gains_[3]}; - - ROS_DEBUG_STREAM("Loaded control parameters"); - return true; -} - -bool Quadrotor::load_init_vals() { - vector position, vel, R, omega; - stringstream ss; - ss << "/robot_" << to_string(this->robot_id); - string robot_name = ss.str(); - if (!nh.getParam(ros::names::append(robot_name, "position"), position)) return false; - if (!nh.getParam(ros::names::append(robot_name, "velocity"), vel)) return false; - if (!nh.getParam(ros::names::append(robot_name, "rotation"), R)) return false; - if (!nh.getParam(ros::names::append(robot_name, "omega"), omega)) return false; - if (!nh.getParam("/frame/fixed", worldframe)) return false; - if (!nh.getParam("/frame/prefix", localframe)) return false; - init_vals.position = Vector3d(position.data()); - init_vals.velocity = Vector3d(vel.data()); - init_vals.R = Matrix3d(R.data()); - init_vals.omega = Vector3d(omega.data()); - - this->x0.position = simulator_utils::ned_nwu_rotation(init_vals.position); - this->x0.velocity = simulator_utils::ned_nwu_rotation(init_vals.velocity); - this->xd0.position = this->x0.position; - this->target_pos = simulator_utils::ned_nwu_rotation(init_vals.position); - ROS_DEBUG_STREAM("Loaded the drone initialization values"); - return true; -} - -void Quadrotor::desired_pos_cb(const geometry_msgs::Point &pt) { - Vector3d p1 = {pt.x, pt.y, pt.z}; - Vector3d p2 = target_pos; - if((p2 - p1).norm() >= 0.2) { - if(!set_init_target) { - set_init_target = true; - // set init trajectory - Vector3d init_z = {0,0,0}; - opt_t ps = {target_pos, init_z, init_z, init_z}; - Vector3d pe = {pt.x, pt.y, pt.z}; - this->traj = get_opt_traj(ps, pe); - this->target_pos = pe; - ROS_DEBUG_STREAM(robot_id<<" Set init trajectory"); - } - else { - this->target_next = pt; - this->target_pos = {pt.x, pt.y, pt.z}; - this->set_next_target = true; - ROS_DEBUG_STREAM(robot_id<<" Set new target"); - } - } -} - -// if there is another target available, calculate a new trajectory -void Quadrotor::do_rhp() { - - if (set_next_target) { - Vector3d pt = {target_next.x, target_next.y, target_next.z}; - Vector3d ps, vel(0,0,0), acc(0,0,0); - // changing to a new trajectory midway - ROS_DEBUG_STREAM("calculating a new trajectory"); - ps = this->dynamics->get_state().position; - vel = this->dynamics->get_state().velocity; - acc = this->dynamics->get_state().acceleration; - - opt_t wp = {ps, vel, acc}; - - this->traj = get_opt_traj(wp, pt); - ROS_DEBUG_STREAM("Setting a new trajectory of length: "<set_next_target = false; - tau = 0; - }; -} - -// xd should be in NED frame and so does dynamics and controller. -void Quadrotor::iteration(const ros::TimerEvent &e) { - // compute the next desired state using incoming control signal and current state - Vector3d b1d(1, 0, 0); - Vector3d xd = simulator_utils::ned_nwu_rotation(init_vals.position); - - // use traj optimization to navigate to the desired state - if(this->set_init_target) { - if(set_next_target) { - do_rhp(); - } - xd = traj.evaluate(tau, mav_trajectory_generation::derivative_order::POSITION); - - if(abs(tau - traj.getMaxTime()) >= dt) { - tau = tau + dt; - } - } - - desired_state_t dss = {xd, b1d}; - this->move(dss); - this->publish_path(); - this->publish_state(); -} - -void Quadrotor::publish_path() { - constexpr int arrowLength = 0.05; - Vector3d x = this->dynamics->get_state().position; - x[1] = -x[1]; - geometry_msgs::Point p; - if(!isnan(x[0]) && !isnan(x[1])) { - p.x = x[0]; - p.y = x[1]; - p.z = x[2]; - quad_initialized = true; - } - else - { - p.x = 0; - p.y = 0; - p.z = 0; - } - - m.points.push_back(p); - if (m.points.size() > 1000) - m.points.erase(m.points.begin()); - - g.pose.position.x = this->target_pos[0]; - g.pose.position.y = -this->target_pos[1]; - g.pose.position.z = this->target_pos[2]; - g.pose.orientation.x = 0.0; - g.pose.orientation.y = 0.0; - g.pose.orientation.z = 0.0; - g.pose.orientation.w = 1.0; - - goal_pub.publish(g); - marker_pub.publish(m); -} - -void Quadrotor::run() { - ros::Timer timer = nh.createTimer(ros::Duration(1 / frequency), &Quadrotor::iteration, this); - ros::spin(); -} - -void Quadrotor::publish_state() { - simulator_utils::Waypoint wp; - geometry_msgs::Point p,v; - state_space_t ned_state = this->dynamics->get_state(); - p.x = ned_state.position[0]; - p.y = ned_state.position[1]; - p.z = ned_state.position[2]; - v.x = ned_state.velocity[0]; - v.y = ned_state.velocity[1]; - v.z = ned_state.velocity[2]; - wp.position = p; - wp.velocity = v; - wp.acceleration.x = wp.acceleration.y = wp.acceleration.z = 0; - this->state_pub.publish(wp); -} - -int main(int argc, char **argv) { - - int robot_id = std::atoi(argv[1]); - double frequency = (double)std::atof(argv[2]); - - ROS_DEBUG_STREAM("initializing : " << robot_id << endl); - ros::init(argc, argv, "~"); - ros::NodeHandle n; - Quadrotor quad(robot_id, frequency, n); - quad.run(); - - return 0; -} \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..3955195 --- /dev/null +++ b/package.xml @@ -0,0 +1,29 @@ + + + + mavswarm2 + 0.0.0 + TODO: Package description + parallels + TODO: License declaration + + ament_cmake + + rclcpp + simulator_interfaces + + + std_msgs + geometry_msgs + sensor_msgs + visualization_msgs + tf2 + tf2_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/publish_words.py b/publish_words.py new file mode 100755 index 0000000..cb2a1f5 --- /dev/null +++ b/publish_words.py @@ -0,0 +1,287 @@ +#!/usr/bin/env python3.12 +# --------------------------------------------------------------------------- +# Spell an arbitrary WORD with the 10 drones, one letter at a time. +# +# python3.12 publish_kth.py HELLO +# python3.12 publish_kth.py # defaults to "KTH" +# +# How it works, per letter: +# 1. Build the letter as exactly 10 goal slots from a vector stroke-font +# (resampled by arc length), with >= CLEARANCE between any two slots, and +# the whole glyph tilted by TILT degrees so it is not edge-on. +# 2. Read the drones' current positions from TF. +# 3. Solve the optimal drone -> slot assignment with the Hungarian algorithm +# (minimum total transit distance). +# 4. Publish the goals; wait TRANSIT_S for the drones to fly over. +# +# Goals are in the controller's NED frame (x = North, y = East, z = Down; +# negative z = up). A glyph point (h, v) maps to +# x = GX + h*sin(TILT), y = h*cos(TILT), z = -(V_BASE + v) +# +# NOTE: ROS Jazzy's rclpy is built for Python 3.12 -- run with python3.12. +# --------------------------------------------------------------------------- + +import sys +import time +import math +import rclpy +from rclpy.node import Node +from rclpy.time import Time +from geometry_msgs.msg import Point +from tf2_ros import Buffer, TransformListener + +# ---- layout / behaviour knobs --------------------------------------------- +N = 10 # drones (= slots per letter) +GX = 2.0 # North/depth offset of the letter plane +V_BASE = 1.5 # altitude of the bottom of a glyph (m) +TILT = math.radians(30) # yaw tilt of the letter plane (so it has depth) +SCALE = 2.5 # overall glyph size multiplier (bigger = more spacing) +CLEARANCE = 0.75 # min spacing between slots BEFORE scaling (> safety R) +TRANSIT_S = 10.0 # seconds to wait between letters (bigger -> longer transit) +STAGGER_S = 0.2 # small interval between successive drone commands + +# --------------------------------------------------------------------------- +# Vector stroke-font. Each glyph is a list of strokes; each stroke is a polyline +# of (h, v) vertices with h in [-1.5, 1.5] (width) and v in [0, 4.5] (height). +# These ARE the saved letter locations -- the 10 slots are sampled from them. +# --------------------------------------------------------------------------- +ALPHABET = { + "A": [[(-1.5,0),(0,4.5),(1.5,0)], [(-0.85,1.9),(0.85,1.9)]], + "B": [[(-1.5,0),(-1.5,4.5),(0.6,4.5),(1.3,3.6),(0.6,2.25),(-1.5,2.25)], + [(0.6,2.25),(1.4,1.1),(0.5,0),(-1.5,0)]], + "C": [[(1.3,3.7),(0.4,4.5),(-0.9,4.5),(-1.5,3.6),(-1.5,0.9),(-0.9,0),(0.4,0),(1.3,0.8)]], + "D": [[(-1.5,0),(-1.5,4.5),(0.3,4.5),(1.3,3.4),(1.3,1.1),(0.3,0),(-1.5,0)]], + "E": [[(1.3,4.5),(-1.5,4.5),(-1.5,0),(1.3,0)], [(-1.5,2.25),(0.8,2.25)]], + "F": [[(1.3,4.5),(-1.5,4.5),(-1.5,0)], [(-1.5,2.25),(0.8,2.25)]], + "G": [[(1.3,3.7),(0.4,4.5),(-0.9,4.5),(-1.5,3.6),(-1.5,0.9),(-0.9,0), + (0.6,0),(1.3,0.8),(1.3,2.0),(0.4,2.0)]], + "H": [[(-1.5,0),(-1.5,4.5)], [(1.5,0),(1.5,4.5)], [(-1.5,2.25),(1.5,2.25)]], + "I": [[(-1.0,4.5),(1.0,4.5)], [(0,4.5),(0,0)], [(-1.0,0),(1.0,0)]], + "J": [[(1.1,4.5),(1.1,1.0),(0.4,0),(-0.6,0),(-1.3,1.0)]], + "K": [[(-1.5,0),(-1.5,4.5)], [(-1.5,2.1),(1.3,4.5)], [(-1.5,2.1),(1.3,0)]], + "L": [[(-1.5,4.5),(-1.5,0),(1.3,0)]], + "M": [[(-1.5,0),(-1.5,4.5),(0,2.1),(1.5,4.5),(1.5,0)]], + "N": [[(-1.5,0),(-1.5,4.5),(1.5,0),(1.5,4.5)]], + "O": [[(-1.5,1.0),(-1.5,3.5),(-0.7,4.5),(0.7,4.5),(1.5,3.5),(1.5,1.0), + (0.7,0),(-0.7,0),(-1.5,1.0)]], + "P": [[(-1.5,0),(-1.5,4.5),(0.6,4.5),(1.3,3.6),(0.6,2.5),(-1.5,2.5)]], + "Q": [[(-1.5,1.0),(-1.5,3.5),(-0.7,4.5),(0.7,4.5),(1.5,3.5),(1.5,1.0), + (0.7,0),(-0.7,0),(-1.5,1.0)], [(0.3,1.3),(1.5,0)]], + "R": [[(-1.5,0),(-1.5,4.5),(0.6,4.5),(1.3,3.6),(0.6,2.5),(-1.5,2.5)], + [(-0.1,2.5),(1.3,0)]], + "S": [[(1.3,3.7),(0.4,4.5),(-0.9,4.5),(-1.5,3.7),(-0.9,2.4),(0.8,2.0), + (1.3,1.0),(0.6,0),(-0.7,0),(-1.4,0.8)]], + "T": [[(-1.5,4.5),(1.5,4.5)], [(0,4.5),(0,0)]], + "U": [[(-1.5,4.5),(-1.5,1.0),(-0.8,0),(0.8,0),(1.5,1.0),(1.5,4.5)]], + "V": [[(-1.5,4.5),(0,0),(1.5,4.5)]], + "W": [[(-1.5,4.5),(-0.8,0),(0,2.4),(0.8,0),(1.5,4.5)]], + "X": [[(-1.5,0),(1.5,4.5)], [(-1.5,4.5),(1.5,0)]], + "Y": [[(-1.5,4.5),(0,2.3),(1.5,4.5)], [(0,2.3),(0,0)]], + "Z": [[(-1.5,4.5),(1.5,4.5),(-1.5,0),(1.5,0)]], +} + + +# ---- glyph -> 10 slots ---------------------------------------------------- +def _poly_len(poly): + return sum(math.dist(poly[i], poly[i + 1]) for i in range(len(poly) - 1)) + + +def _sample_polyline(poly, count): + """`count` points evenly spaced by arc length along the polyline.""" + if count <= 1: + return [poly[len(poly) // 2]] + seg = [math.dist(poly[i], poly[i + 1]) for i in range(len(poly) - 1)] + total = sum(seg) + if total < 1e-9: + return [poly[0]] * count + out = [] + for s in range(count): + target = total * s / (count - 1) + acc = 0.0 + for i, sl in enumerate(seg): + if acc + sl >= target or i == len(seg) - 1: + r = (target - acc) / sl if sl > 1e-9 else 0.0 + r = max(0.0, min(1.0, r)) + out.append((poly[i][0] + r * (poly[i + 1][0] - poly[i][0]), + poly[i][1] + r * (poly[i + 1][1] - poly[i][1]))) + break + acc += sl + return out + + +def _resample_glyph(strokes, n=N): + """Distribute exactly n points across the strokes, by length (>=2 each).""" + lens = [_poly_len(s) for s in strokes] + total = sum(lens) or 1.0 + counts = [max(2, round(n * l / total)) for l in lens] + while sum(counts) > n: + cand = [k for k in range(len(counts)) if counts[k] > 2] or list(range(len(counts))) + i = min(cand, key=lambda k: lens[k] / counts[k]) + counts[i] -= 1 + while sum(counts) < n: + i = max(range(len(counts)), key=lambda k: lens[k] / counts[k]) + counts[i] += 1 + pts = [] + for s, c in zip(strokes, counts): + pts += _sample_polyline(s, c) + return pts + + +def _enforce_clearance(pts, clear=CLEARANCE, iters=60): + """Push apart any pair closer than `clear` (in the glyph plane = 3D).""" + p = [list(q) for q in pts] + for _ in range(iters): + moved = False + for i in range(len(p)): + for j in range(i + 1, len(p)): + dx, dy = p[j][0] - p[i][0], p[j][1] - p[i][1] + d = math.hypot(dx, dy) + if d < clear: + if d < 1e-6: + dx, dy, d = 0.0, 1.0, 1.0 + push = (clear - d) / 2.0 + 1e-3 + ux, uy = dx / d, dy / d + p[i][0] -= ux * push; p[i][1] -= uy * push + p[j][0] += ux * push; p[j][1] += uy * push + moved = True + if not moved: + break + return [(q[0], q[1]) for q in p] + + +def build_letter(ch): + """Return 10 NED goals for character `ch`, or None if unknown.""" + strokes = ALPHABET.get(ch.upper()) + if strokes is None: + return None + glyph = _enforce_clearance(_resample_glyph(strokes, N), CLEARANCE) + goals = [] + for (h, v) in glyph: + hs, vs = h * SCALE, v * SCALE # enlarge the glyph + x = GX + hs * math.sin(TILT) + y = hs * math.cos(TILT) + z = -(V_BASE + vs) + goals.append((x, y, z)) + return goals + + +# ---- Hungarian assignment (scipy if available, else built-in O(n^3)) ------ +def hungarian(cost): + try: + from scipy.optimize import linear_sum_assignment + r, c = linear_sum_assignment(cost) + ans = [0] * len(cost) + for ri, ci in zip(r, c): + ans[ri] = ci + return ans + except Exception: + pass + n = len(cost) + INF = float('inf') + u = [0.0] * (n + 1); v = [0.0] * (n + 1) + p = [0] * (n + 1); way = [0] * (n + 1) + for i in range(1, n + 1): + p[0] = i; j0 = 0 + minv = [INF] * (n + 1); used = [False] * (n + 1) + while True: + used[j0] = True; i0 = p[j0]; delta = INF; j1 = -1 + for j in range(1, n + 1): + if not used[j]: + cur = cost[i0 - 1][j - 1] - u[i0] - v[j] + if cur < minv[j]: + minv[j] = cur; way[j] = j0 + if minv[j] < delta: + delta = minv[j]; j1 = j + for j in range(n + 1): + if used[j]: + u[p[j]] += delta; v[j] -= delta + else: + minv[j] -= delta + j0 = j1 + if p[j0] == 0: + break + while True: + j1 = way[j0]; p[j0] = p[j1]; j0 = j1 + if j0 == 0: + break + ans = [0] * n + for j in range(1, n + 1): + ans[p[j] - 1] = j - 1 + return ans + + +def _ned_to_map(p): + return (p[0], -p[1], -p[2]) # NWU(map) <-> NED is the involution (x,-y,-z) + + +class WordSpeller(Node): + def __init__(self): + super().__init__('word_speller') + self.pubs = [self.create_publisher(Point, f'/mavswarm2/robot_{i}/desired_state', 10) + for i in range(N)] + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + def current_positions_map(self): + pos = [None] * N + for i in range(N): + try: + t = self.tf_buffer.lookup_transform('map', f'robot_{i}/base_link', Time()) + tr = t.transform.translation + pos[i] = (tr.x, tr.y, tr.z) + except Exception: + pos[i] = None + return pos + + def assign_and_publish(self, ch, goals_ned): + goals_map = [_ned_to_map(g) for g in goals_ned] + cur = self.current_positions_map() + n_known = sum(1 for c in cur if c is not None) + cost = [[0.0] * N for _ in range(N)] + for i in range(N): + ci = cur[i] if cur[i] is not None else goals_map[i] + for k in range(N): + cost[i][k] = math.dist(ci, goals_map[k]) + assign = hungarian(cost) + total = sum(cost[i][assign[i]] for i in range(N)) + self.get_logger().info( + f"'{ch}': tf {n_known}/{N}, total dist {total:.2f} m, drone->slot {assign}") + # issue commands one drone at a time with a small interval (not all at once) + for i in range(N): + g = goals_ned[assign[i]] + m = Point(); m.x, m.y, m.z = g + self.pubs[i].publish(m) + self.spin_for(STAGGER_S) + + def spin_for(self, seconds): + end = time.time() + seconds + while time.time() < end and rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.05) + + +def main(): + word = (sys.argv[1] if len(sys.argv) > 1 else "KTH") + rclpy.init() + node = WordSpeller() + node.get_logger().info(f"Spelling: {word}") + node.spin_for(3.0) # let TF fill / publishers match + + for ch in word: + if ch == ' ': + node.get_logger().info("(space)") + node.spin_for(TRANSIT_S) + continue + goals = build_letter(ch) + if goals is None: + node.get_logger().warn(f"no glyph for '{ch}', skipping") + continue + node.assign_and_publish(ch, goals) + node.spin_for(TRANSIT_S) + + node.get_logger().info(f"Done spelling '{word}'.") + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/readme.md b/readme.md index a35633a..47f1105 100644 --- a/readme.md +++ b/readme.md @@ -1,138 +1,58 @@ -# Mavswarm: A Lightweight Multi-Aerial Vehicle Simulator +# Mavswarm2 (ROS2 Support for Mavswarm) -Mavswarm is a lightweight and fast Multi-Aerial Vehicle simulator based on ROS (Robot Operating System). It supports simulating heterogenous quadrotor swarms of upto 10 robots on a single desktop with physics. Mavswarm also supports quadrotor control, trajectory optimization and receding horizon planning (RHP). Currently, the internal controller uses the Lee's geometric tracking controller [3] and it is tuned for two different quadrotor models out of the box. -In addition you can extend the swarm simulation with more quadrotor models by simply adding the new quadrotor model parameters through a yaml configuration file. +Mavswarm2 is the ROS2-compatible version of Mavswarm. It supports simulating heterogenous quadrotor swarms of more than 10 robots on a single desktop with physics. Tested for ROS2 Jazzy. -Consider citing our work [1][2] if you find this code helpful for your publications. - -| ![Cover Image](https://github.com/malintha/multi_uav_simulator/blob/master/cover.gif?raw=true) | -|:--:| -| *A hetergenous swarm of 5 quadrotors stabilizing from an upside-down initialization* | - -## Mostly Automated Installation - -A Makefile has been compiled to install ros, install dependancies and/or create and build a catkin workspace, and build the entire uav simulator package. - -Note: This package currently runs with ROS1 which is only compatible on older linux distros - for example, it is not supported on anything later than Ubuntu 20.04 - -**To get the makefile, run:** - -```bash -wget https://gist.githubusercontent.com/matteovidali/caab443e66425e260b6a1c1bd842d28c/raw/188effd1f7e5c4a49cf636abd96bc325a9ac9bd9/Makefile -``` - -Then, if you wish to do a full desktop installation of ROS1, this can be done with: -```bash -make ros_desktop_install -``` - -To get and install the required dependancies, run: -```bash -make deps -``` - -To create and initialize a NEW catkin_ws with all required packages -First in the makefile, change the value of 'CATKIN_WS_NAME' from '../catkin_ws' to the path where your workspace should exist. -Then, run: -```bash -make wstool-new -``` - -If you have a PRE-EXISTING catkin workspace you would like to build this module into, -First change the 'CATKIN_WS_NAME' variable to the path of your catkin workspace, then run: +**New in Mavswarm2 is fully-distributed trajectory optimization with collision avoidance using sequential convex programming!** + +All the functionality is self-contained, and does not depend on any other ros packages except for ``simulator_interfaces``. -```bash -make wstool-exists -``` +**Functionalities:** +- quadrotor control (same geometric controller as mavswarm) +- trajectory optimization +- receding horizon planning +- collision avoidance -Finally, if you would like to run a catkin build, you may run: -```bash -make build -``` -This will build the entire catkin space, so be sure to customize this if necessary +Consider citing our work [1][2] if you find this code helpful for your publications. -After building, you can launch the simulator with -```bash -make launch -``` +The internal controller uses the Lee's geometric tracking controller [3] and it is tuned for two different quadrotor models out of the box. -## Manual Installation -**Install dependencies** +**Compared to Mavswarm1, Mavswarm2 has a simpler installation, and the code is more self-contained. Everything is in the header files, so easier to manage and export as a library.**. -Install [`Eigen`](http://eigen.tuxfamily.org/index.php?title=Main_Page), [`Armadillo`](https://www.uio.no/studier/emner/matnat/fys/FYS4411/v13/guides/installing-armadillo/) and [`GNU Science Library (GSL)`](https://www.gnu.org/software/gsl/) before you continue. +| Drone stabilization | Letter formation | +|:-------------------:|:----------------:| +| | | +| *A swarm of 10 drones stabilizing from an upside-down initialization* | *10 drones forming letter "U"* | -Install eigen-coversions (make sure to change the ROS version name i.e.; melodic, kinetic). +## Installation - sudo apt install ros-noetic-eigen-conversions +**Install the dependencies** -**Building the simulator** +Install Eigen, Armadillo and GNU Science Library (GSL) before you continue. -Create a new catkin workspace and use `wstool` to download the `multi_uav_simulator` package. - - mkdir catkin_ws - wstool init src - cd src/ - wstool set --git multi_uav_simulator git@github.com:malintha/multi_uav_simulator.git -y - wstool update - -Mavswarm uses `ethz-asl/mav_trajectory_generation` package to generate trajectories for the robots. Use the`rosinstall/dependencies.rosinstall` file to download the required dependencies. - - wstool merge multi_uav_simulator/rosinstall/dependencies.rosinstall - wstool update +**Clone the repositories** -This will clone the following packages to the workspace automatically : `catkin_simple`, `eigen_catkin`, `eigen_checks`, `glog_catkin`, `mav_comm`, `mav_trajectory_generation`, `nlopt`. + mkdir -p mavswarm2/src + cd mavswarm2/src + git clone -b ros2 git@github.com:malintha/simulator_interfaces.git + git clone -b ros2 git@github.com:malintha/multi_uav_simulator.git -Now build the simulator. +**Building the simulator** cd ../ - catkin build multi_uav_simulator - - Run the simulator. - - source devel/setup.bash - roslaunch multi_uav_simulator simu.launch - - -This will open an Rviz window and show 5 drones flip from an upside-down initialization. - -**Publishing Goals** - -Mavswarm supports Receding Horizon Planning (RHP) and trajectory optimization out of the box. You can publish a new desired goal position using rostopics for any drone to see it navigating. To do so, open a new command window and publish the following message. - - rostopic pub /robot_1/desired_state geometry_msgs/Point '{x: 4, y: -1, z: 3}' - -Each drone listnes to its `desired_state` topic which uses the ROS standard `geometry_msgs/Point` message type to recieve new goal locations. The command above simply adds the goal `{x: 4, y: -1, z: 3}` into the first drone's goal list. To See RHP in working, publish a different goal before the drone reaches the previous goal. This will recalculate a new trajectory to match the robot's current speed and acceleration to ensure smooth transitions between rapidly changing goal locations. - -**Adding a new drone to the environment** - -Mavswarm uses xacro to spawn new models into the simulation environment. Out of the box, it has 5 drones in the environment and two separate quadrotor models to choose from; namely, Bigquad (the quadrotor model from [2]) and Crazyflie. If you want to add a sixth drone to the environment, simply follow the steps below. - -1) In the `launch/simu.launch` file, add a new line with the corresponding drone id. Make sure to change `` and the robot_id accordingly. For example, - - - - This adds a new Crazyflie model to the environment. If you want a larger quadrotor model, you can use `bigquad_description/bigquad.urdf.xacro` instead. + colcon build -2) Now add the following block to bind a quadrotor instance to the URDF model. Here we pass the `robot_id` and the internal controller's frequency as arguments to the quadrotor instance. In addition, we use `rosparam` to specify the quadrotor's model parameters and the controller gains. The parameters for the Bigquad and the Crazyflie can be found in `config/bigquad_params.yaml` and `config/crazyflie_params.yaml` files respectively. +**Running the simulator** - - - - - + source install/setup.bash + ros2 launch mavswarm2 quadrotor.launch -3) In order to specify the initial conditions for the new drone add a new block to the `config/initial_conditions.yaml` file as below. - robot_6: - position: [2, 4, 0] - velocity: [0, 0, 0] - rotation: [1, 0, 0, 0, -0.9995, -0.0314, 0, 0.0314, -0.9995] - omega: [0, 0, 0] +**Formation Control Goal Publishing** - The rotation and omega stands for the initial rotation matrix (3x3) and the angular velocity (3x1) of the rigid body. Here, we initialize the drone up-side down. To make it right way up, change the rotation matrix to identity: `[1, 0, 0, 0, 1, 0, 0, 0, 1]`. +The example script ``publish_words.py`` publishes locations to the drones such that it can visualize a word, one letter-at-a-time using a formation of 10 drones. It computes a bipartite matching to find the least distance matching drones, then issue collision-free trajectory waypoints to be tracked using the internal geometric controller in a decentralized manner. + python publish_words.py HELLO -4) Finally, to visualize the newly added drone, add a robot_model visualization type to RViz. Change its description to corresponding param name of the newly added element in the launch file. i.e.: "`robot_6`". [1] Our work based on this controller: diff --git a/rosinstall/dependencies.rosinstall b/rosinstall/dependencies.rosinstall deleted file mode 100644 index 6d22a50..0000000 --- a/rosinstall/dependencies.rosinstall +++ /dev/null @@ -1,8 +0,0 @@ -- git: {local-name: mav_trajectory_generation, uri: 'https://github.com/ethz-asl/mav_trajectory_generation'} -- git: {local-name: catkin_simple, uri: 'https://github.com/catkin/catkin_simple.git'} -- git: {local-name: eigen_catkin, uri: 'https://github.com/ethz-asl/eigen_catkin.git'} -- git: {local-name: eigen_checks, uri: 'https://github.com/ethz-asl/eigen_checks.git'} -- git: {local-name: glog_catkin, uri: 'https://github.com/ethz-asl/glog_catkin.git'} -- git: {local-name: mav_comm, uri: 'https://github.com/ethz-asl/mav_comm.git'} -- git: {local-name: nlopt, uri: 'https://github.com/ethz-asl/nlopt.git'} - diff --git a/simulator_utils/CMakeLists.txt b/simulator_utils/CMakeLists.txt deleted file mode 100644 index 17746f9..0000000 --- a/simulator_utils/CMakeLists.txt +++ /dev/null @@ -1,210 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(simulator_utils) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - message_generation - geometry_msgs - ) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - Waypoint.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES simulator_utils - CATKIN_DEPENDS message_runtime - # CATKIN_DEPENDS roscpp std_msgs - # DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include/${PROJECT_NAME} - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(${PROJECT_NAME} - include/${PROJECT_NAME} - src/simulator_utils.cpp - src/Drone.cpp -) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/simulator_utils_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_simulator_utils.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/simulator_utils/include/simulator_utils/Drone.h b/simulator_utils/include/simulator_utils/Drone.h deleted file mode 100644 index b666ff9..0000000 --- a/simulator_utils/include/simulator_utils/Drone.h +++ /dev/null @@ -1,28 +0,0 @@ -// -// Created by malintha on 10/14/20. -// -#ifndef SIMULATOR_UTILS_DRONE_H -#define SIMULATOR_UTILS_DRONE_H - -//#include -#include "ros/ros.h" -#include "simulator_utils/Waypoint.h" -#include "ros/console.h" - -//using namespace std; - -class Drone { - ros::Subscriber state_sub; - ros::NodeHandle nh; - void state_cb(const simulator_utils::WaypointConstPtr& wp); - -public: - int id; - simulator_utils::Waypoint state; - Drone(int id, const ros::NodeHandle &n); - simulator_utils::Waypoint get_state() const; - - -}; - -#endif \ No newline at end of file diff --git a/simulator_utils/include/simulator_utils/simulator_utils.h b/simulator_utils/include/simulator_utils/simulator_utils.h deleted file mode 100644 index 16f2e98..0000000 --- a/simulator_utils/include/simulator_utils/simulator_utils.h +++ /dev/null @@ -1,64 +0,0 @@ -#pragma once - -#include - -using namespace Eigen; - -typedef struct { - Vector3d position; - Matrix3d R; - Vector3d omega; - Vector3d velocity; - double h; -} init_vals_t; - -typedef struct { - double gravity; - double mass; - Matrix3d J; - Matrix3d J_inv; - double F; - Vector3d M; -} params_t; - -typedef struct { - Vector3d position; - Vector3d velocity; - Vector3d acceleration; - Matrix3d R; - Vector3d omega; -} state_space_t; - -typedef struct { - Vector3d x; - Vector3d b1; -} desired_state_t; - -typedef struct -{ - double kx; - double kv; - double kr; - double komega; -} gains_t; - -typedef struct { - double F; - Vector3d M; -} control_out_t; - -typedef struct { - Vector3d position; - Vector3d velocity; - Vector3d acceleration; - Vector3d jerk; -} opt_t; - -namespace simulator_utils { - Matrix3d hat(Vector3d v); - Vector3d R2RPY(Matrix3d R); - Vector3d ned_nwu_rotation(Vector3d v); - Matrix3d ned_nwu_rotation(Matrix3d m); - Vector3d vee(Matrix3d mat); - -} diff --git a/simulator_utils/msg/Waypoint.msg b/simulator_utils/msg/Waypoint.msg deleted file mode 100644 index 90e3c87..0000000 --- a/simulator_utils/msg/Waypoint.msg +++ /dev/null @@ -1,4 +0,0 @@ -# A waypoint message type for the drones to follow -geometry_msgs/Point position -geometry_msgs/Point velocity -geometry_msgs/Point acceleration \ No newline at end of file diff --git a/simulator_utils/package.xml b/simulator_utils/package.xml deleted file mode 100644 index ee70fa7..0000000 --- a/simulator_utils/package.xml +++ /dev/null @@ -1,66 +0,0 @@ - - - simulator_utils - 0.0.0 - The simulator_utils package - - - - - malintha - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - roscpp - std_msgs - roscpp - std_msgs - message_generation - message_runtime - - - - - - - diff --git a/simulator_utils/src/Drone.cpp b/simulator_utils/src/Drone.cpp deleted file mode 100644 index 8ecf9e4..0000000 --- a/simulator_utils/src/Drone.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// -// Created by malintha on 10/14/20. -// -#include -#include "Drone.h" - -using namespace std; - -simulator_utils::Waypoint Drone::get_state() const { - // ROS_DEBUG_STREAM("Robot: "<state.position.x<<" " << this->state.position.y); - return this->state; -} - -void Drone::state_cb(const simulator_utils::WaypointConstPtr &wp) { - this->state.position = wp->position; - this->state.velocity = wp->velocity; - this->state.acceleration = wp->acceleration; -} - -// TODO: pass the topic prefix as a parameter -Drone::Drone(int id, const ros::NodeHandle &n):id(id), nh(n) { - - stringstream ss; - ss << "/robot_"<state_sub = nh.subscribe(ss.str(), 10, - &Drone::state_cb, - this); - - ROS_DEBUG_STREAM("Robot: "<id<<" Waiting for quadrotor states."); - ros::topic::waitForMessage(ss.str(), ros::Duration(3)); -} \ No newline at end of file diff --git a/simulator_utils/src/simulator_utils.cpp b/simulator_utils/src/simulator_utils.cpp deleted file mode 100644 index 548d576..0000000 --- a/simulator_utils/src/simulator_utils.cpp +++ /dev/null @@ -1,45 +0,0 @@ -#include "simulator_utils.h" -#include - -Matrix3d simulator_utils::hat(Vector3d v) -{ - Matrix3d hat_map(3, 3); - hat_map << 0, -v(2), v(1), - v(2), 0, -v(0), - -v(1), v(0), 0; - return hat_map; -} - -Vector3d simulator_utils::vee(Matrix3d mat) -{ - Vector3d vee_vec(mat(2, 1), mat(0, 2),mat(1, 0)); - return vee_vec; -} - -Vector3d simulator_utils::R2RPY(Matrix3d R) -{ - Vector3d rpy; - rpy << atan2(R(2, 1), R(2, 2)), - atan2(-R(2, 0), sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2))), - atan2(R(1, 0), R(0, 0)); - return rpy; -} - -Vector3d simulator_utils::ned_nwu_rotation(Vector3d v) -{ - Matrix3d rot; - rot << 1, 0, 0, - 0, -1, 0, - 0, 0, -1; - return rot * v; -} - -Matrix3d simulator_utils::ned_nwu_rotation(Matrix3d m) -{ - Matrix3d rot; - rot << 1, 0, 0, - 0, -1, 0, - 0, 0, -1; - return rot * m; -} - diff --git a/src/quadrotor.cpp b/src/quadrotor.cpp new file mode 100644 index 0000000..37541e6 --- /dev/null +++ b/src/quadrotor.cpp @@ -0,0 +1,36 @@ +/* +Copyright (c) 2024 Malintha Fernando (malinthafer@gmail.com) + +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. +*/ + +#include "quadrotor.hpp" +#include "rclcpp/rclcpp.hpp" + +/* +ros2 run mavswarm2 mavswarm2 0 50 --ros-args --log-level DEBUG +*/ +int main(int argc, char **argv) { + + rclcpp::init(argc, argv); + int robot_id = std::atoi(argv[1]); + double frequency = (double)std::atof(argv[2]); + auto logger = rclcpp::get_logger("logger"); + + RCLCPP_INFO(logger, "Initialing drone: %d %3f", robot_id, frequency); + + rclcpp::spin(std::make_shared(robot_id, frequency)); + rclcpp::shutdown(); + + return 0; +}