├── LICENSE ├── README.md ├── _config.yml └── main_sequence ├── CMakeLists.txt ├── README.md ├── action └── uas_navigation.action ├── launch └── main_sequence.launch ├── msg ├── attitude.msg └── route.msg ├── package.xml ├── scripts ├── .readData_IWR1443.py.swp ├── 1443config.cfg ├── __pycache__ │ └── constant_params.cpython-35.pyc ├── constant_params.py ├── control_mode_hub.py ├── library │ ├── AziFromPos.py │ ├── Record_Coordinates.py │ ├── __init__.py │ └── __pycache__ │ │ ├── AziFromPos.cpython-35.pyc │ │ ├── Record_Coordinates.cpython-35.pyc │ │ └── __init__.cpython-35.pyc ├── navigation_action_server.py ├── readData_IWR1443.py └── serial_port_hub.py ├── test.txt └── workingfile └── LatLong.txt /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 无人载具上位机端程序 2 | ================ 3 | 此package为使用ROS为框架开发的无人载具上位机程序。目前正在开发阶段，功能尚未完成。使用ROS的主要目的在于试图消除因不同硬件平台带来的低通用性。在Kinetic、Melodic等版本的ROS、在Nvidia TX系列、树莓派、PC等Ubuntu系统平台无差别运行。 4 | 5 | ------- 6 | 7 | ## 概述 8 | 本package运行在运行ROS的上位机中，通过串口与下位机(STM32F103ZE)相连接；下位机负责动力控制、姿态解算与遥控信号接收等功能，上位机负责自动导航模式的路径规划。 9 | * [下位机端工程文件Github仓库](https://github.com/matreshka15/UAS-Project-STM32) 10 | ### 重要！所有开发下位机过程中的开发日志以及手册均已存放在下面地址 11 | * [开发无人船过程中参考的传感器手册以及算法资料](https://github.com/matreshka15/unmanned-ship-datasheets) 12 | * 开发日志记录了项目从一开始的立项到后面一步步测试成功的大大小小细节。前后由于放弃了旧的姿态算法、选取了新的姿态算法，因此前期关于姿态的说明仅供参考用。 13 | * 通信协议的部分已摘抄出来，放在超链接处的Repo目录下。即：整体框架与通信协议.docx 14 | 15 | ## 开发环境 16 | 初期在Nvidia Jetson TX1开发板上进行开发。(系统：Ubuntu 18.04；ROS版本：melodic)，与ROS kinetic兼容性有待测试。但实际开发过程中发现在Kinetic平台未报错误。 17 | 开发中期使用Nvidia Jetson Nano, 由于Nvidia官方社区支持Ubuntu版本为18.04，因此本Project将专注在ROS melodic平台上开发。 18 | 19 | ## 相关问题 20 | * Nvidia 在TX1开发板上使用USB串口(CH341，/dev/ttyUSB0)时接收到的数据不正确，但串口设置正常。实验相同的硬件连接插在windows平台上可接收到正确数据。 21 | * 解决方案：使用TX1开发板上的ttyTHS2串口进行数据通信。ttyTHS1和ttyTHS3分别是控制台串口和蓝牙模块，不宜使用。 22 | * 当波特率很高时，误码率也会大幅提高，因而出现丢包、错包等情况。此外由于使用的串口为TTL电平，且连接线较长，波特率高时会发生干扰。等待解决。 23 | * 解决方案：波特率降低为115200。 24 | * rospy中没有像roscpp的ros::spinOnce()方法。rospy中的回调函数是在一个单独的线程中进行的，不需spin()。还请注意。 25 | 26 | ## 开发进度 27 | * main_sequence package 28 | 包内主要涵盖：路径规划、上位机与下位机的通信（基于自创Eastar protocol协议，协议处于测试阶段。若需要协议框图请联系开发者） 29 | * control_mode_hub.py--驱动系统状态切换的switcher.遥控器可选择手动操作模式、自动导航模式、计点模式，switcher的作用即接收遥控器模式的切换，并且通知相应server进行相关操作。 30 | * eastar_protocol.py--系统的串口操作hub.所有串口的输入输出脚本均在此脚本中进行 31 | * navigation_action_server.py--系统的core组件。负责实际路径的规划、GPS坐标点的记录等核心操作。 32 | * constant_params.py--系统常量存放处。 33 | * 注意--串口等设置参数。均在参数服务器中，在launch文件内修改。 34 | 35 | ## 联系开发者 36 | 37 | |Author|Eastello| 38 | |---|--- 39 | |E-mail|8523429@qq.com 40 | **** 41 | -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-cayman -------------------------------------------------------------------------------- /main_sequence/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(main_sequence) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | std_msgs 13 | actionlib_msgs 14 | message_generation 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | add_message_files( 52 | FILES 53 | attitude.msg 54 | route.msg 55 | ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | #location_feedback.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | add_action_files( 65 | FILES 66 | uas_navigation.action 67 | # Action2.action 68 | ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | generate_messages( 72 | DEPENDENCIES 73 | std_msgs 74 | actionlib_msgs 75 | ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES Mainsequence 109 | CATKIN_DEPENDS rospy std_msgs message_runtime actionlib_msgs 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/Mainsequence.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/Mainsequence_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables for installation 169 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 170 | # install(TARGETS ${PROJECT_NAME}_node 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark libraries for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 176 | # install(TARGETS ${PROJECT_NAME} 177 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark cpp header files for installation 183 | # install(DIRECTORY include/${PROJECT_NAME}/ 184 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 185 | # FILES_MATCHING PATTERN "*.h" 186 | # PATTERN ".svn" EXCLUDE 187 | # ) 188 | 189 | ## Mark other files for installation (e.g. launch and bag files, etc.) 190 | # install(FILES 191 | # # myfile1 192 | # # myfile2 193 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 194 | # ) 195 | 196 | ############# 197 | ## Testing ## 198 | ############# 199 | 200 | ## Add gtest based cpp test target and link libraries 201 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_Mainsequence.cpp) 202 | # if(TARGET ${PROJECT_NAME}-test) 203 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 204 | # endif() 205 | 206 | ## Add folders to be run by python nosetests 207 | # catkin_add_nosetests(test) 208 | -------------------------------------------------------------------------------- /main_sequence/README.md: -------------------------------------------------------------------------------- 1 | 上位机ROS系统的核心Package 2 | ======================== 3 | ### 说明 4 | * workingfile/目录下用于存放无人船运行过程中需要的文件，例如：路径的经纬度记录文件、GPS坐标点记录文件等。 5 | 6 | ----------- 7 | ### Jetson开发环境配置 8 | * WiFi链接 9 | 搜索WiFi信号:nmcli dev wifi 10 | 连接WiFi:sudo nmcli dev wifi connect wifi_name password 12345678 11 | 12 | * 修改DNS 13 | sudo nano /etc/systemd/resolved.conf 14 | 修改好了之后：systemctl restart systemd-resolved.service 15 | 16 | * 加速访问GitHub 17 | http://tool.chinaz.com/dns/ 18 | 查询以下域名映射,并分别取访问速度较快的一个ip 19 | github.global.ssl.fastly.net -> 20 | assets-cdn.github.com -> 21 | TTL值越大证明响应速度越快（“TTL”的值越大越好才对，因为“TTL”的值越大，说明发送数据包经过路由器越少，而经过路由器越少，说明越快到达目的地，速度当然也就越快。） 22 | 将查询到的ip和域名设置到host中sudo gedit /etc/hosts 23 | 在hosts中加入查询结果 24 | 151.101.229.194 github.global.ssl.fastly.net 25 | 203.208.39.99 assets-cdn.github.com 26 | 保存,退出,并重启网络 27 | /etc/init.d/networking restart 28 | * 创建swap空间 29 | $ sudo fallocate -l 8G /mnt/8GB.swap 30 | $ sudo mkswap /mnt/8GB.swap 31 | $ sudo swapon /mnt/8GB.swap 32 | 然后sudo nano /etc/fstab 33 | /mnt/8GB.swap none swap sw 0 0 34 | 35 | * 配置frp穿透: 36 | https://www.jianshu.com/p/a921e85280ed 37 | 启动frp: ./frpc -c ./frpc.ini 38 | 39 | * 解压缩tar.gz文件 40 | tar -zxvf java.tar.gz 41 | 解压到指定的文件夹tar -zxvf java.tar.gz -C /usr/java 42 | x : 从 tar 包中把文件提取出来 43 | z : 表示 tar 包是被 gzip 压缩过的，所以解压时需要用 gunzip 解压 44 | v : 显示详细信息 45 | f xxx.tar.gz : 指定被处理的文件是 xxx.tar.gz 46 | 47 | * 换源： 48 | deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ bionic main restricted universe multiverse 49 | deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ bionic main restricted universe multiverse 50 | deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ bionic-updates main restricted universe multiverse 51 | deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ bionic-updates main restricted universe multiverse 52 | deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ bionic-backports main restricted universe multiverse 53 | deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ bionic-backports main restricted universe multiverse 54 | deb https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ bionic-security main restricted universe multiverse 55 | deb-src https://mirrors.tuna.tsinghua.edu.cn/ubuntu-ports/ bionic-security main restricted universe multiverse 56 | 57 | * OpenGl模组 58 | apt-get install python3-pyqt4.qtopengl 59 | pip3 install PyOpenGl PyOpenGl_accerate 60 | * PyQt5模块 61 | pip3 install python-qt5 62 | ----------------- 63 | -------------------------------------------------------------------------------- /main_sequence/action/uas_navigation.action: -------------------------------------------------------------------------------- 1 | #goal 2 | uint8 navigation_mode 3 | uint8 control_mode 4 | --- 5 | #result 6 | --- 7 | #feedback 8 | uint32 current_index 9 | float64 current_latitude 10 | float64 current_longitude 11 | -------------------------------------------------------------------------------- /main_sequence/launch/main_sequence.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 |     8 | 9 | 10 | 11 | 12 |     13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /main_sequence/msg/attitude.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float64 latitude 3 | float64 longitude 4 | float64 hAcc 5 | uint8 fixtype 6 | uint16 yaw 7 | uint16 pitch 8 | uint16 roll 9 | uint16 control_mode 10 | 11 | -------------------------------------------------------------------------------- /main_sequence/msg/route.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint16 nav_distance 3 | uint16 nav_yaw 4 | uint8 EndOfNav 5 | #navigation calculated route 6 | -------------------------------------------------------------------------------- /main_sequence/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | main_sequence 4 | 0.0.0 5 | The main_sequence package 6 | 7 | 8 | 9 | 10 | estello 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | message_generation 54 | std_msgs 55 | actionlib 56 | actionlib_msgs 57 | rospy 58 | std_msgs 59 | message_runtime 60 | rospy 61 | actionlib 62 | actionlib_msgs 63 | std_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /main_sequence/scripts/.readData_IWR1443.py.swp: -------------------------------------------------------------------------------- 1 | b0nano 2.9.3O#eastareastar-desktop/home/eastar/UAS/src/main_sequence/scripts/readData_IWR1443.pyU -------------------------------------------------------------------------------- /main_sequence/scripts/1443config.cfg: -------------------------------------------------------------------------------- 1 | % *************************************************************** 2 | % Created for SDK ver:01.02 3 | % Created using Visualizer ver:2.0.0.2 4 | % Frequency:77 5 | % Platform:xWR14xx 6 | % Scene Classifier:best_range_res 7 | % Azimuth Resolution(deg):15 + Elevation 8 | % Range Resolution(m):0.047 9 | % Maximum unambiguous Range(m):2.41 10 | % Maximum Radial Velocity(m/s):1 11 | % Radial velocity resolution(m/s):0.13 12 | % Frame Duration(msec):50 13 | % Range Detection Threshold (dB):15 14 | % Range Peak Grouping:disabled 15 | % Doppler Peak Grouping:disabled 16 | % Static clutter removal:enabled 17 | % *************************************************************** 18 | sensorStop 19 | flushCfg 20 | dfeDataOutputMode 1 21 | channelCfg 15 7 0 22 | adcCfg 2 1 23 | adcbufCfg 0 1 0 1 24 | profileCfg 0 77 284 7 40 0 0 100 1 64 2000 0 0 30 25 | chirpCfg 0 0 0 0 0 0 0 1 26 | chirpCfg 1 1 0 0 0 0 0 4 27 | chirpCfg 2 2 0 0 0 0 0 2 28 | frameCfg 0 2 16 0 50 1 0 29 | lowPower 0 0 30 | guiMonitor 1 0 0 0 0 0 31 | cfarCfg 0 2 8 4 3 0 1195 32 | peakGrouping 1 0 0 1 56 33 | multiObjBeamForming 1 0.5 34 | clutterRemoval 1 35 | calibDcRangeSig 0 -5 8 256 36 | compRangeBiasAndRxChanPhase 0.0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 37 | measureRangeBiasAndRxChanPhase 0 1.5 0.2 38 | CQRxSatMonitor 0 3 4 99 0 39 | CQSigImgMonitor 0 31 4 40 | analogMonitor 1 1 41 | sensorStart -------------------------------------------------------------------------------- /main_sequence/scripts/__pycache__/constant_params.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matreshka15/ROS-based-USV-project/5539a969d51ce7ff27fab825fa80583910981204/main_sequence/scripts/__pycache__/constant_params.cpython-35.pyc -------------------------------------------------------------------------------- /main_sequence/scripts/constant_params.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding: utf-8 3 | 4 | control_manual = 0 5 | control_auto = 1 6 | control_record = 2 7 | mode_start_from_the_beginning = 0 8 | mode_start_from_the_nearest = 1 9 | # 10 | control_mode_dict = {control_manual:'manual',control_auto:'auto',control_record:'recording'} 11 | -------------------------------------------------------------------------------- /main_sequence/scripts/control_mode_hub.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import actionlib 3 | from constant_params import * 4 | from main_sequence.msg import * 5 | import rospy 6 | 7 | control_mode = 0 8 | 9 | def callback(attitude): 10 | control_mode = attitude.control_mode 11 | 12 | 13 | #this param is used to define the strategy uas will take 14 | prvNavmode = 0 15 | 16 | if __name__ == '__main__': 17 | rospy.init_node('control_hub') 18 | rospy.Subscriber('control_hub_listener',attitude,callback) 19 | client = actionlib.SimpleActionClient('GPS_nav',uas_navigationAction) 20 | print('FSM:Control mode switcher awating action server') 21 | client.wait_for_server() 22 | rate = rospy.Rate(20) 23 | timeCounter = 0 24 | print('FSM:Control mode switcher successfully started up.') 25 | while not rospy.is_shutdown(): 26 | if(control_mode == control_manual):#遥控模式 27 | pass 28 | elif(control_mode == control_auto):#自动模式 29 | goal.control_mode = control_mode 30 | goal.navigation_mode = prvNavmode 31 | client.send_goal(goal) 32 | while True: 33 | client.waitForResult(rospy.Duration(5)) 34 | if(client.getState() == actionlib.SimpleClinetGoalState.SUCCEEDED): 35 | print("FSM:Target location arrived.") 36 | while control_mode == control_auto: 37 | pass 38 | break 39 | if(control_mode != control_auto): 40 | client.cancel_goal() 41 | print("FSM:Navgation has been cancelled") 42 | break 43 | elif(control_mode == control_record):#采点模式 44 | goal.control_mode = control_mode 45 | client.send_goal(goal) 46 | while(control_mode == control_record): 47 | pass 48 | client.cancel_goal() 49 | if(timeCounter >= 40): 50 | print('FSM:Current control mode:',control_mode_dict.get(control_mode,'Exception')) 51 | timeCounter = 0 52 | timeCounter += 1 53 | rate.sleep() 54 | -------------------------------------------------------------------------------- /main_sequence/scripts/library/AziFromPos.py: -------------------------------------------------------------------------------- 1 | '''Copyright 2019 李东豫<8523429@qq.com> 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License.''' 14 | import math 15 | 16 | #测试通过 17 | def DDD2DMS(number): 18 | D = number//1 19 | temp = number%1 20 | M = (temp*60)//1 21 | temp = (temp*60) %1 22 | S = (temp*60) 23 | return D+(M/100)+(S/10000) 24 | 25 | #测试通过 26 | def angleFromCoordinate(long1, lat1, long2, lat2): 27 | lat1 = math.radians(DDD2DMS(lat1)) 28 | lat2 = math.radians(DDD2DMS(lat2)) 29 | long1 = math.radians(DDD2DMS(long1)) 30 | long2 = math.radians(DDD2DMS(long2)) 31 | y = math.sin(long2-long1)*math.cos(lat2) 32 | x = math.cos(lat1)*math.sin(lat2)-math.sin(lat1)*math.cos(lat2)*math.cos(long2-long1) 33 | deltaLon = long2-long1 34 | theta = math.atan2(y,x) 35 | theta = math.degrees(theta) 36 | theta = (theta+360)%360 37 | return theta 38 | 39 | def distanceFromCoordinate(lon1, lat1, lon2, lat2): # 经度1，纬度1，经度2，纬度2 （十进制度数） 40 | """ 41 | Calculate the great circle distance between two points 42 | on the earth (specified in decimal degrees) 43 | """ 44 | # 将十进制度数转化为弧度 45 | lon1, lat1, lon2, lat2 = map(math.radians, [lon1, lat1, lon2, lat2]) 46 | 47 | # haversine公式 48 | dlon = lon2 - lon1 49 | dlat = lat2 - lat1 50 | a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2 51 | c = 2 * math.asin(math.sqrt(a)) 52 | r = 6371 # 地球平均半径，单位为公里 53 | return c * r * 1000 *100 54 | 55 | -------------------------------------------------------------------------------- /main_sequence/scripts/library/Record_Coordinates.py: -------------------------------------------------------------------------------- 1 | '''Copyright 2019 李东豫<8523429@qq.com> 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License.''' 14 | # -*- coding: utf-8 -* 15 | 16 | #记录点函数 17 | #传入参数： 18 | #startbit:记录点标志位； 19 | #file:打开的文件 20 | #index:写入点的索引号 21 | def start(file,index,longtitude,latitude,height): 22 | #print(index + ':' + longtitude + ',' + latitude + ',' + height + '\n') 23 | file.write(str(index) + ':' + str(longtitude) + ',' + str(latitude) + ',' + str(height) + '\n') 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /main_sequence/scripts/library/__init__.py: -------------------------------------------------------------------------------- 1 | #For different directories to import files 2 | -------------------------------------------------------------------------------- /main_sequence/scripts/library/__pycache__/AziFromPos.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matreshka15/ROS-based-USV-project/5539a969d51ce7ff27fab825fa80583910981204/main_sequence/scripts/library/__pycache__/AziFromPos.cpython-35.pyc -------------------------------------------------------------------------------- /main_sequence/scripts/library/__pycache__/Record_Coordinates.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matreshka15/ROS-based-USV-project/5539a969d51ce7ff27fab825fa80583910981204/main_sequence/scripts/library/__pycache__/Record_Coordinates.cpython-35.pyc -------------------------------------------------------------------------------- /main_sequence/scripts/library/__pycache__/__init__.cpython-35.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matreshka15/ROS-based-USV-project/5539a969d51ce7ff27fab825fa80583910981204/main_sequence/scripts/library/__pycache__/__init__.cpython-35.pyc -------------------------------------------------------------------------------- /main_sequence/scripts/navigation_action_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | FILE_LOCATION = "../workingfile/" 3 | RECORD_LOCATION = "../workingfile/" 4 | from library import AziFromPos 5 | from library import Record_Coordinates 6 | from constant_params import * 7 | import actionlib 8 | from main_sequence.msg import * 9 | import rospy 10 | import math 11 | import os 12 | 13 | 14 | #define global variables 15 | class navigation_data(): 16 | latitude = 0 17 | longitude = 0 18 | fixtype = 0 19 | nav_action_data = navigation_data() 20 | #路线规划描述： 21 | class Route: 22 | yaw = 0 #单位：° 23 | distance = 0#单位：厘米,u16型数据 24 | EndOfNav = 0#导航结束标志位 25 | def __init__(self): 26 | self.yaw = 0 27 | self.distance = 0 28 | self.EndOfNav = 0 29 | prvRoute = Route() 30 | 31 | #define general function 32 | def acquire_mapdata(): 33 | try: 34 | f = open((FILE_LOCATION,'LatLong.txt'),'r') 35 | Mapdata = {} 36 | if(f.read() != ''): 37 | for line in f: 38 | startIndex = line.find(':') 39 | endIndex = line.find(',') 40 | name = int(line[:startIndex]) 41 | Position0 = float(line[startIndex+1:endIndex]) 42 | startIndex = endIndex 43 | line = line[endIndex+1:] 44 | endIndex = line.find(',') 45 | Position1 = float(line[:endIndex]) 46 | Mapdata[name] = [Position0,Position1] 47 | for key in Mapdata.keys(): 48 | print("NAV:Point#%d:%s"%(key,str(Mapdata[key]))) 49 | print("NAV: %d Points In Total."%len(Mapdata.keys())) 50 | else: 51 | print('NAV:Warning:Mapdata Empty!') 52 | except FileNotFoundError: 53 | print("NAV:Map Data Not Found!") 54 | return Mapdata 55 | 56 | 57 | #define subscriber of vehicle's attitude 58 | #define callback of subscriber 59 | def subscriber_callback(attitude_data): 60 | nav_action_data.latitude = attitude_data.latitude 61 | nav_action_data.longitude = attitude_data.longitude 62 | nav_action_data.fixtype = attitude_data.fixtype 63 | 64 | 65 | #define publisher part 66 | nav_route = route() 67 | 68 | #define action 69 | nav_result = uas_navigationResult() 70 | nav_feedback = uas_navigationFeedback() 71 | def navigation(goal): 72 | if(goal.control_mode == control_record): 73 | if(nav_action_data.fixtype): 74 | index = 0 75 | os.chdir(RECORD_LOCATION) 76 | Coordinates_Saving_File = open(("LatLong_Record.txt"),"w+") 77 | print("NAV:Coordinate Saving File Created.") 78 | print("NAV:Recording Coordinates") 79 | while not server.is_preempt_requested(): 80 | index += 1 81 | time.sleep(1)#采点时间间隔设置 82 | Record_Coordinates.start(Coordinates_Saving_File,index,nav_action_data.longitude,nav_action_data.latitude,0) 83 | Coordinates_Saving_File.close() 84 | print('NAV:%d Coordinate(s) saved'%index) 85 | os.rename('LatLong_Record.txt','LatLong.txt') 86 | os.chdir(CWD)#switch back to original working directory 87 | server.set_preempted(nav_result,"NAV:New Coordinates Saved") 88 | else: 89 | print("GPS not fixed") 90 | elif(goal.control_mode == control_auto): 91 | if(nav_action_data.fixtype): 92 | os.chdir(CWD) 93 | MapData = acquire_mapdata() 94 | if(goal.navigation_mode == mode_start_from_the_beginning): 95 | nextPoint = min(Mapdata.keys()) 96 | elif(goal.navigation_mode == mode_start_from_the_nearest): 97 | for key in Mapdata.keys(): 98 | distance = AziFromPos.distanceFromCoordinate(nav_action_data.longitude,nav_action_data.latitude,Mapdata[key][0],Mapdata[key][1]) 99 | if(key==1): 100 | minimum = distance 101 | indexMin = 1 102 | else: 103 | if(distance < minimum): 104 | minimum = distance 105 | indexMin = key 106 | nextPoint = indexMin 107 | while True: 108 | if(server.is_preempt_requested()): 109 | server.set_preempted(nav_feedback,'NAV:GPS Navigation Halt') 110 | break 111 | 112 | #After this part of function,initial start point will be selected 113 | distance = AziFromPos.distanceFromCoordinate(nav_action_data.longitude,nav_action_data.latitude,Mapdata[nextPoint][0],Mapdata[nextPoint][1]) 114 | if distance >=255: 115 | distance = 255 116 | angle = AziFromPos.angleFromCoordinate(nav_action_data.longitude,nav_action_data.latitude,Mapdata[nextPoint][0],Mapdata[nextPoint][1]) 117 | prvRoute.yaw = round(angle) 118 | prvRoute.distance = round(distance) 119 | if(distance > MaxDistance): 120 | #如果超出范围 121 | pass 122 | #----根据夹角调转机头----# 123 | elif(nextPoint == len(Mapdata.keys())): 124 | #未超出范围，则已达到目标点。 125 | nextPoint = nextPoint 126 | prvRoute.EndOfNav = 1 127 | server.set_succeeded(nav_result) 128 | while not server.is_preempt_requested(): 129 | pass 130 | else: 131 | server.set_preempted(nav_result,'NAV:GPS Navigation Halt') 132 | break 133 | break 134 | else: 135 | prvRoute.EndOfNav = 0 136 | nextPoint += 1 137 | 138 | nav_feedback.current_index = nextPoint 139 | nav_feedback.current_latitude = nav_action_data.latitude 140 | nav_feedback.current_longitude = nav_action_data.longitude 141 | server.publish_feedback(nav_feedback) 142 | 143 | nav_route.EndOfNav = prvRoute.EndOfNav 144 | nav_route.nav_yaw = prvRoute.yaw 145 | nav_route.nav_distance = prvRoute.distance 146 | pub.publish(nav_route) 147 | rate.sleep() 148 | else: 149 | print("NAV:GPS not fixed") 150 | 151 | 152 | #Initial working directory 153 | CWD = os.getcwd() 154 | 155 | if __name__ == '__main__': 156 | rospy.init_node('main_execute_module',anonymous=True) 157 | #fetch parameters 158 | MaxDistance = rospy.get_param('~MaxDistance') 159 | #start nav data listener 160 | rospy.Subscriber('nav_action_listener',attitude,subscriber_callback) 161 | pub = rospy.Publisher('navigation_route',route,queue_size=5) 162 | rate = rospy.Rate(5) #5Hz 163 | server = actionlib.SimpleActionServer('GPS_nav',uas_navigationAction,navigation,False) 164 | server.start() 165 | print("NAV:nav_center:all module successfully started up") 166 | rospy.spin() 167 | -------------------------------------------------------------------------------- /main_sequence/scripts/readData_IWR1443.py: -------------------------------------------------------------------------------- 1 | import serial 2 | import time 3 | import numpy as np 4 | import pyqtgraph as pg 5 | import matplotlib.pyplot as plt 6 | from mpl_toolkits.mplot3d import Axes3D 7 | from pyqtgraph.Qt import QtGui 8 | 9 | # Change the configuration file name 10 | configFileName = '1443config.cfg' 11 | 12 | CLIport = {} 13 | Dataport = {} 14 | byteBuffer = np.zeros(2**15,dtype = 'uint8') 15 | byteBufferLength = 0; 16 | 17 | 18 | # ------------------------------------------------------------------ 19 | 20 | # Function to configure the serial ports and send the data from 21 | # the configuration file to the radar 22 | def serialConfig(configFileName): 23 | 24 | global CLIport 25 | global Dataport 26 | # Open the serial ports for the configuration and the data ports 27 | 28 | # Raspberry pi 29 | CLIport = serial.Serial('/dev/ttyACM0', 115200) 30 | Dataport = serial.Serial('/dev/ttyACM1', 921600) 31 | 32 | # Windows 33 | #CLIport = serial.Serial('COM21', 115200) 34 | #Dataport = serial.Serial('COM22', 921600) 35 | 36 | # Read the configuration file and send it to the board 37 | config = [line.rstrip('\r\n') for line in open(configFileName)] 38 | for i in config: 39 | CLIport.write((i+'\n').encode()) 40 | print(i) 41 | time.sleep(0.01) 42 | 43 | return CLIport, Dataport 44 | 45 | # ------------------------------------------------------------------ 46 | 47 | # Function to parse the data inside the configuration file 48 | def parseConfigFile(configFileName): 49 | configParameters = {} # Initialize an empty dictionary to store the configuration parameters 50 | 51 | # Read the configuration file and send it to the board 52 | config = [line.rstrip('\r\n') for line in open(configFileName)] 53 | for i in config: 54 | 55 | # Split the line 56 | splitWords = i.split(" ") 57 | 58 | # Hard code the number of antennas, change if other configuration is used 59 | numRxAnt = 4 60 | numTxAnt = 3 61 | 62 | # Get the information about the profile configuration 63 | if "profileCfg" in splitWords[0]: 64 | startFreq = int(float(splitWords[2])) 65 | idleTime = int(splitWords[3]) 66 | rampEndTime = float(splitWords[5]) 67 | freqSlopeConst = float(splitWords[8]) 68 | numAdcSamples = int(splitWords[10]) 69 | numAdcSamplesRoundTo2 = 1; 70 | 71 | while numAdcSamples > numAdcSamplesRoundTo2: 72 | numAdcSamplesRoundTo2 = numAdcSamplesRoundTo2 * 2; 73 | 74 | digOutSampleRate = int(splitWords[11]); 75 | 76 | # Get the information about the frame configuration 77 | elif "frameCfg" in splitWords[0]: 78 | 79 | chirpStartIdx = int(splitWords[1]); 80 | chirpEndIdx = int(splitWords[2]); 81 | numLoops = int(splitWords[3]); 82 | numFrames = int(splitWords[4]); 83 | framePeriodicity = int(splitWords[5]); 84 | 85 | 86 | # Combine the read data to obtain the configuration parameters 87 | numChirpsPerFrame = (chirpEndIdx - chirpStartIdx + 1) * numLoops 88 | configParameters["numDopplerBins"] = numChirpsPerFrame / numTxAnt 89 | configParameters["numRangeBins"] = numAdcSamplesRoundTo2 90 | configParameters["rangeResolutionMeters"] = (3e8 * digOutSampleRate * 1e3) / (2 * freqSlopeConst * 1e12 * numAdcSamples) 91 | configParameters["rangeIdxToMeters"] = (3e8 * digOutSampleRate * 1e3) / (2 * freqSlopeConst * 1e12 * configParameters["numRangeBins"]) 92 | configParameters["dopplerResolutionMps"] = 3e8 / (2 * startFreq * 1e9 * (idleTime + rampEndTime) * 1e-6 * configParameters["numDopplerBins"] * numTxAnt) 93 | configParameters["maxRange"] = (300 * 0.9 * digOutSampleRate)/(2 * freqSlopeConst * 1e3) 94 | configParameters["maxVelocity"] = 3e8 / (4 * startFreq * 1e9 * (idleTime + rampEndTime) * 1e-6 * numTxAnt) 95 | 96 | return configParameters 97 | 98 | # ------------------------------------------------------------------ 99 | 100 | # Funtion to read and parse the incoming data 101 | def readAndParseData14xx(Dataport, configParameters): 102 | global byteBuffer, byteBufferLength 103 | 104 | # Constants 105 | OBJ_STRUCT_SIZE_BYTES = 12; 106 | BYTE_VEC_ACC_MAX_SIZE = 2**15; 107 | MMWDEMO_UART_MSG_DETECTED_POINTS = 1; 108 | MMWDEMO_UART_MSG_RANGE_PROFILE = 2; 109 | maxBufferSize = 2**15; 110 | magicWord = [2, 1, 4, 3, 6, 5, 8, 7] 111 | 112 | # Initialize variables 113 | magicOK = 0 # Checks if magic number has been read 114 | dataOK = 0 # Checks if the data has been read correctly 115 | frameNumber = 0 116 | detObj = {} 117 | 118 | readBuffer = Dataport.read(Dataport.in_waiting) 119 | byteVec = np.frombuffer(readBuffer, dtype = 'uint8') 120 | byteCount = len(byteVec) 121 | 122 | # Check that the buffer is not full, and then add the data to the buffer 123 | if (byteBufferLength + byteCount) < maxBufferSize: 124 | byteBuffer[byteBufferLength:byteBufferLength + byteCount] = byteVec[:byteCount] 125 | byteBufferLength = byteBufferLength + byteCount 126 | 127 | # Check that the buffer has some data 128 | if byteBufferLength > 16: 129 | 130 | # Check for all possible locations of the magic word 131 | possibleLocs = np.where(byteBuffer == magicWord[0])[0] 132 | 133 | # Confirm that is the beginning of the magic word and store the index in startIdx 134 | startIdx = [] 135 | for loc in possibleLocs: 136 | check = byteBuffer[loc:loc+8] 137 | if np.all(check == magicWord): 138 | startIdx.append(loc) 139 | 140 | # Check that startIdx is not empty 141 | if startIdx: 142 | # Remove the data before the first start index 143 | if startIdx[0] > 0: 144 | try: 145 | byteBuffer[:byteBufferLength-startIdx[0]] = byteBuffer[startIdx[0]:byteBufferLength] 146 | byteBufferLength = byteBufferLength - startIdx[0] 147 | except: 148 | pass 149 | #print(len(byteBuffer[:byteBufferLength-startIdx[0]])) 150 | #print(byteBuffer[:byteBufferLength-startIdx[0]]) 151 | #print('--------------------------------------') 152 | #print(len(byteBuffer[startIdx[0]:byteBufferLength])) 153 | #print(byteBuffer[startIdx[0]:byteBufferLength]) 154 | # Check that there have no errors with the byte buffer length 155 | if byteBufferLength < 0: 156 | byteBufferLength = 0 157 | 158 | # word array to convert 4 bytes to a 32 bit number 159 | word = [1, 2**8, 2**16, 2**24] 160 | 161 | # Read the total packet length 162 | totalPacketLen = np.matmul(byteBuffer[12:12+4],word) 163 | 164 | # Check that all the packet has been read 165 | if (byteBufferLength >= totalPacketLen) and (byteBufferLength != 0): 166 | magicOK = 1 167 | 168 | # If magicOK is equal to 1 then process the message 169 | if magicOK: 170 | # word array to convert 4 bytes to a 32 bit number 171 | word = [1, 2**8, 2**16, 2**24] 172 | 173 | # Initialize the pointer index 174 | idX = 0 175 | 176 | # Read the header 177 | magicNumber = byteBuffer[idX:idX+8] 178 | idX += 8 179 | version = format(np.matmul(byteBuffer[idX:idX+4],word),'x') 180 | idX += 4 181 | totalPacketLen = np.matmul(byteBuffer[idX:idX+4],word) 182 | idX += 4 183 | platform = format(np.matmul(byteBuffer[idX:idX+4],word),'x') 184 | idX += 4 185 | frameNumber = np.matmul(byteBuffer[idX:idX+4],word) 186 | idX += 4 187 | timeCpuCycles = np.matmul(byteBuffer[idX:idX+4],word) 188 | idX += 4 189 | numDetectedObj = np.matmul(byteBuffer[idX:idX+4],word) 190 | idX += 4 191 | numTLVs = np.matmul(byteBuffer[idX:idX+4],word) 192 | idX += 4 193 | 194 | # UNCOMMENT IN CASE OF SDK 2 195 | #subFrameNumber = np.matmul(byteBuffer[idX:idX+4],word) 196 | #idX += 4 197 | 198 | # Read the TLV messages 199 | for tlvIdx in range(numTLVs): 200 | 201 | # word array to convert 4 bytes to a 32 bit number 202 | word = [1, 2**8, 2**16, 2**24] 203 | 204 | # Check the header of the TLV message 205 | tlv_type = np.matmul(byteBuffer[idX:idX+4],word) 206 | idX += 4 207 | tlv_length = np.matmul(byteBuffer[idX:idX+4],word) 208 | idX += 4 209 | 210 | # Read the data depending on the TLV message 211 | if tlv_type == MMWDEMO_UART_MSG_DETECTED_POINTS: 212 | 213 | # word array to convert 4 bytes to a 16 bit number 214 | word = [1, 2**8] 215 | tlv_numObj = np.matmul(byteBuffer[idX:idX+2],word) 216 | idX += 2 217 | tlv_xyzQFormat = 2**np.matmul(byteBuffer[idX:idX+2],word) 218 | idX += 2 219 | 220 | # Initialize the arrays 221 | rangeIdx = np.zeros(tlv_numObj,dtype = 'int16') 222 | dopplerIdx = np.zeros(tlv_numObj,dtype = 'int16') 223 | peakVal = np.zeros(tlv_numObj,dtype = 'int16') 224 | x = np.zeros(tlv_numObj,dtype = 'int16') 225 | y = np.zeros(tlv_numObj,dtype = 'int16') 226 | z = np.zeros(tlv_numObj,dtype = 'int16') 227 | 228 | for objectNum in range(tlv_numObj): 229 | 230 | # Read the data for each object 231 | rangeIdx[objectNum] = np.matmul(byteBuffer[idX:idX+2],word) 232 | idX += 2 233 | dopplerIdx[objectNum] = np.matmul(byteBuffer[idX:idX+2],word) 234 | idX += 2 235 | peakVal[objectNum] = np.matmul(byteBuffer[idX:idX+2],word) 236 | idX += 2 237 | x[objectNum] = np.matmul(byteBuffer[idX:idX+2],word) 238 | idX += 2 239 | y[objectNum] = np.matmul(byteBuffer[idX:idX+2],word) 240 | idX += 2 241 | z[objectNum] = np.matmul(byteBuffer[idX:idX+2],word) 242 | idX += 2 243 | 244 | # Make the necessary corrections and calculate the rest of the data 245 | rangeVal = rangeIdx * configParameters["rangeIdxToMeters"] 246 | dopplerIdx[dopplerIdx > (configParameters["numDopplerBins"]/2 - 1)] = dopplerIdx[dopplerIdx > (configParameters["numDopplerBins"]/2 - 1)] - 65535 247 | dopplerVal = dopplerIdx * configParameters["dopplerResolutionMps"] 248 | #x[x > 32767] = x[x > 32767] - 65536 249 | #y[y > 32767] = y[y > 32767] - 65536 250 | #z[z > 32767] = z[z > 32767] - 65536 251 | x = x / tlv_xyzQFormat 252 | y = y / tlv_xyzQFormat 253 | z = z / tlv_xyzQFormat 254 | 255 | # Store the data in the detObj dictionary 256 | detObj = {"numObj": tlv_numObj, "rangeIdx": rangeIdx, "range": rangeVal, "dopplerIdx": dopplerIdx, \ 257 | "doppler": dopplerVal, "peakVal": peakVal, "x": x, "y": y, "z": z} 258 | 259 | dataOK = 1 260 | 261 | 262 | #print(detObj['range'].mean()) 263 | #print(detObj['range']) 264 | else: 265 | idX += tlv_length 266 | 267 | 268 | 269 | 270 | # Remove already processed data 271 | if idX > 0 and byteBufferLength > idX: 272 | shiftSize = idX 273 | 274 | 275 | byteBuffer[:byteBufferLength - shiftSize] = byteBuffer[shiftSize:byteBufferLength] 276 | byteBufferLength = byteBufferLength - shiftSize 277 | 278 | # Check that there are no errors with the buffer length 279 | if byteBufferLength < 0: 280 | byteBufferLength = 0 281 | 282 | 283 | return dataOK, frameNumber, detObj 284 | 285 | # ------------------------------------------------------------------ 286 | # Funtion to update the data and display in the plot 287 | def update(READY2PROCESS): # new plot will be plotted when cycleCounter equals DRAWCYCLE 288 | 289 | dataOk = 0 290 | global detObj 291 | global pointsComing 292 | global pointsLeaving 293 | x = [] 294 | y = [] 295 | z = [] 296 | X = [] 297 | Xcoming=[] 298 | Xleaving=[] 299 | Y = [] 300 | Ycoming=[] 301 | Yleaving=[] 302 | Z = [] 303 | Zcoming=[] 304 | Zleaving=[] 305 | DopplerIdx = [] 306 | # Read and parse the received data 307 | try: 308 | dataOk, frameNumber, detObj = readAndParseData14xx(Dataport, configParameters) 309 | except Exception as e: 310 | print("Error",e) 311 | if dataOk: 312 | #print(detObj) 313 | #x = -detObj["x"] 314 | #y = detObj["y"] 315 | #z = detObj["z"] 316 | if(READY2PROCESS == True): 317 | # Collect Data 318 | for cnt in range(len(frameData)): 319 | X=np.hstack((X,-frameData[cnt]["x"])) 320 | Y=np.hstack((Y,frameData[cnt]["y"])) 321 | Z=np.hstack((Z,frameData[cnt]["z"])) 322 | DopplerIdx =np.hstack((DopplerIdx,frameData[cnt]["doppler"])) 323 | #DataCollected. Classify or Process Data 324 | #classify different moving direction 325 | for index in range(len(DopplerIdx)): 326 | if(DopplerIdx[index] > 0): 327 | Xcoming = np.hstack((Xcoming,X[index])) 328 | Ycoming = np.hstack((Ycoming,Y[index])) 329 | Zcoming = np.hstack((Zcoming,Z[index])) 330 | else: 331 | Xleaving = np.hstack((Xleaving,X[index])) 332 | Yleaving = np.hstack((Yleaving,Y[index])) 333 | Zleaving = np.hstack((Zleaving,Z[index])) 334 | #Plot Data 335 | if(FIGURE3D == True): 336 | pointsComing.remove() 337 | pointsLeaving.remove() 338 | pointsComing = plot3d.scatter(Xcoming,Ycoming,Zcoming,c='r',s=3) 339 | pointsLeaving = plot3d.scatter(Xleaving,Yleaving,Zleaving,c='b',s=3) 340 | plt.draw() 341 | if(FIGURE2D == True): 342 | s.setData(X,Y) 343 | QtGui.QApplication.processEvents() 344 | return dataOk 345 | 346 | 347 | # ------------------------- MAIN ----------------------------------------- 348 | 349 | # Configurate the serial port 350 | CLIport, Dataport = serialConfig(configFileName) 351 | #Congigurate the display of plot(3D\2D) 352 | FIGURE3D= 1 # Quite slow plotting 353 | FIGURE2D= 0 # 2D ploting is way faster 354 | # Get the configuration parameters from the configuration file 355 | configParameters = parseConfigFile(configFileName) 356 | 357 | # Prepare for plotting 358 | if(FIGURE2D or FIGURE3D): 359 | # START QtAPPfor the plot 360 | app = QtGui.QApplication([]) 361 | #Start 2D plotting(faster than matplotlib) 362 | if(FIGURE2D==True): 363 | # Set the plot 364 | pg.setConfigOption('background','w') 365 | win = pg.GraphicsWindow(title="2D scatter plot") 366 | p = win.addPlot() 367 | p.setXRange(-0.5,0.5) 368 | p.setYRange(0,1.5) 369 | p.setLabel('left',text = 'Y position (m)') 370 | p.setLabel('bottom', text= 'X position (m)') 371 | s = p.plot([],[],pen=None,symbol='+') 372 | 373 | #matplotlib 374 | if(FIGURE3D==True): 375 | plt.ion() 376 | plot3d = plt.subplot(111,projection='3d') 377 | plot3d.set_zlabel('Z') 378 | plot3d.set_ylabel('Y') 379 | plot3d.set_xlabel('X') 380 | plot3d.set_zlim(0,2) 381 | plt.xlim(-0.5,0.5) 382 | plt.ylim(0,1.5) 383 | origin = plot3d.scatter(0,0,0,c='b') 384 | pointsComing = plot3d.scatter(0,0,0,c='r') 385 | pointsLeaving = plot3d.scatter(0,0,0,c='b') 386 | 387 | # Set Params for radar data processing 388 | 389 | #System params 390 | READY2PROCESS = 0 391 | # Main loop 392 | detObj = {} 393 | frameData = {} 394 | currentIndex = 0 395 | cycleCounter = 0 396 | cycle = 0.01 397 | RunUponStart = True # Enable scripts to run on start 398 | # Set the time span between plotting 399 | # which thus also sets the frames radar used for data processing. 400 | DRAWCYCLE = 0.1 401 | #Set the Sampling time 402 | RadarProcessFrame = int(DRAWCYCLE / cycle) 403 | print("Data will be processed with %d frames prepared"%(RadarProcessFrame)) 404 | 405 | while True: 406 | try: 407 | # Update the data and check if the data is okay 408 | dataOk = update(READY2PROCESS) 409 | if dataOk: 410 | if(RunUponStart): 411 | print("Analysing data:") 412 | for key in detObj.keys(): 413 | print(key) 414 | # Store the current frame into frameData 415 | frameData[currentIndex] = detObj 416 | currentIndex += 1 417 | if(currentIndex >= RadarProcessFrame): 418 | currentIndex = 0 419 | frameData = {} 420 | frameData[currentIndex] = detObj 421 | currentIndex += 1 422 | elif(currentIndex == RadarProcessFrame-1): 423 | READY2PROCESS = 1 424 | else: 425 | READY2PROCESS = 0 426 | 427 | if(cycleCounter > DRAWCYCLE-cycle): 428 | cycleCounter = 0 429 | time.sleep(cycle) # Sampling frequency of 1/cycle Hz 430 | cycleCounter += cycle 431 | RunUponStart = 0 432 | # Stop the program and close everything if Ctrl + c is pressed 433 | except KeyboardInterrupt: 434 | CLIport.write(('sensorStop\n').encode()) 435 | CLIport.close() 436 | Dataport.close() 437 | if(FIGURE2D): 438 | win.close() 439 | if(FIGURE3D): 440 | plt.close() 441 | break 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | -------------------------------------------------------------------------------- /main_sequence/scripts/serial_port_hub.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import serial 4 | #message type:include almost all GPS\Imu data needed 5 | from main_sequence.msg import * 6 | #configuration 7 | frame_length = 22 8 | 9 | 10 | 11 | 12 | class attitude_buff(): 13 | latitude = 0 14 | longitude = 0 15 | fixtype = 0 16 | hAcc = 0 17 | yaw = 0 18 | pitch = 0 19 | roll = 0 20 | speed = 0 21 | angleSpeed = 0 22 | dataProcessed = 0 23 | control_mode = 0 24 | 25 | 26 | def callback(Route): 27 | sendFile = list() 28 | sumup = 0 29 | for cnt in range(20): 30 | sendFile.append(0) #初始化数组 31 | sendFile[0]=0x63 32 | sendFile[1]=0x73 33 | sendFile[2]=(Route.yaw&0xff00)>>8 34 | sendFile[3]=Route.yaw&0xff 35 | sendFile[4]=(Route.distance&0xff00)>>8 36 | sendFile[5]=Route.distance&0xff 37 | sendFile[6]=(Route.EndOfNav << 7)&0xff 38 | #这里用来后续填充 39 | for cnt in range(17): 40 | sumup+=sendFile[2+cnt] 41 | sumup = sumup & 0xff 42 | sendFile[19] = sumup 43 | sendBytes = bytes() 44 | for cnt in range(20): 45 | sendBytes += bytes([sendFile[cnt]]) 46 | ser.write(sendBytes) 47 | 48 | #initialize data needed 49 | revData = [] 50 | int_revData = [] 51 | attitude_buff = attitude_buff() 52 | real_attitude = attitude() 53 | one_frame_received = 0 54 | CRC_validation_passed = 0 55 | for cnt in range(frame_length): 56 | revData.append(0) 57 | int_revData.append(0) 58 | 59 | 60 | timeCounter = 0 61 | if __name__ == '__main__': 62 | try: 63 | pub = rospy.Publisher('attitude_info',attitude,queue_size=10) 64 | rospy.init_node('serial_port_hub',anonymous=True) 65 | #fetch parameters 66 | baudrate = rospy.get_param('~baudrate') 67 | SERIALPORT = rospy.get_param('~serial_port') 68 | #initiate 69 | rospy.Subscriber('navigation_data_publisher',route,callback) 70 | rate = rospy.Rate(10) #5Hz 71 | print("SERIAL:Initiating phase 1") 72 | #timeout protection 73 | ser = serial.Serial(SERIALPORT,baudrate,timeout=1) 74 | print(ser) 75 | portNotFound = 0 76 | while not rospy.is_shutdown(): 77 | one_frame_received = 0 78 | ser.flushInput() 79 | for cnt in range(frame_length): 80 | revData[0] = ser.read(1) 81 | if revData[0] == b'': 82 | if(portNotFound == False): 83 | portNotFound = True 84 | print("SERIAL:No data coming in serial port.") 85 | print("SERIAL:Please check port:",ser.name) 86 | elif revData[0] == b's': 87 | portNotFound = False 88 | for cnt2 in range(frame_length - 1): 89 | revData[1+cnt2] = ser.read(1)#Byte型 90 | if(b'sc' in revData[2:]): 91 | break 92 | for cnt3 in range(frame_length):#int型 93 | int_revData[cnt3] = int.from_bytes(revData[cnt3],byteorder='big',signed=False) 94 | one_frame_received = 1 95 | break 96 | else: 97 | portNotFound = False 98 | 99 | if(one_frame_received == True): 100 | #CheckSum prosedure 101 | sumUp = 0 102 | CRC_validation_passed = 0 103 | for cnt in range(frame_length - 3): 104 | sumUp += int_revData[2+cnt] 105 | sumUp = sumUp & 0xff 106 | if(sumUp == int_revData[frame_length - 1]): 107 | CRC_validation_passed = True 108 | else: 109 | CRC_validation_passed = False 110 | print("SERIAL:sumUp = %d,CheckSum = %d"%(sumUp,int_revData[frame_length - 1])) 111 | print(int_revData) 112 | if(CRC_validation_passed == True): 113 | #Interpret data into NavSatFix 114 | attitude_buff.longitude = bytes() 115 | attitude_buff.latitude = bytes() 116 | attitude_buff.yaw = bytes() 117 | attitude_buff.pitch = bytes() 118 | attitude_buff.roll = bytes() 119 | attitude_buff.hAcc = bytes(revData[16]) 120 | #control field 121 | attitude_buff.fixtype = bytes(revData[20]) 122 | attitude_buff.control_mode = bytes(revData[20]) 123 | for cnt in range(4): 124 | attitude_buff.longitude += revData[cnt+2] 125 | attitude_buff.latitude += revData[cnt+2+4] 126 | for cnt in range(2): 127 | attitude_buff.yaw += revData[cnt+10] 128 | attitude_buff.pitch += revData[cnt+12] 129 | attitude_buff.roll += revData[cnt+14] 130 | real_attitude.yaw = int.from_bytes(attitude_buff.yaw,byteorder='big',signed=False) 131 | real_attitude.pitch = int.from_bytes(attitude_buff.pitch,byteorder='big',signed=True) 132 | real_attitude.roll = int.from_bytes(attitude_buff.roll,byteorder='big',signed=True) 133 | real_attitude.fixtype = (0xC0 & int.from_bytes(attitude_buff.fixtype,byteorder='big',signed=False)) >> 6 134 | real_attitude.control_mode = (0x30 & int.from_bytes(attitude_buff.control_mode,byteorder='big',signed=False)) >> 4 135 | real_attitude.hAcc = int.from_bytes(attitude_buff.hAcc,byteorder='big',signed=False)/10 136 | real_attitude.longitude = int.from_bytes(attitude_buff.longitude,byteorder='big',signed=False)/1e7 137 | real_attitude.latitude = int.from_bytes(attitude_buff.latitude,byteorder='big',signed=False)/1e7 138 | pub.publish(real_attitude) 139 | timeCounter += 1 140 | if(timeCounter>=100):#report status every (number) seconds 141 | print("SERIAL:serial port hub working.") 142 | timeCounter = 0 143 | rate.sleep() 144 | 145 | except rospy.ROSInterruptException: 146 | ser.close() 147 | print("SERIAL:Node shuting down") 148 | 149 | 150 | -------------------------------------------------------------------------------- /main_sequence/test.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matreshka15/ROS-based-USV-project/5539a969d51ce7ff27fab825fa80583910981204/main_sequence/test.txt -------------------------------------------------------------------------------- /main_sequence/workingfile/LatLong.txt: -------------------------------------------------------------------------------- 1 | 1:122.0798896612399,37.53323475687386,0 2 | 2:122.0798243073986,37.53303502625283,0 3 | 3:122.0801317267036,37.53286829811549,0 4 | 4:122.0801106192154,37.53321923653363,0 5 | --------------------------------------------------------------------------------