├── 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.3 O# eastar eastar-desktop /home/eastar/UAS/src/main_sequence/scripts/readData_IWR1443.py U
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------