├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── custom.md │ └── feature_request.md └── workflows │ └── workflow.yml ├── .gitignore ├── LICENSE.txt ├── README.md ├── cyberdog_gps ├── CMakeLists.txt ├── README.md ├── include │ ├── bcm_gps │ │ └── bcm_gps.hpp │ ├── bcmgps_base │ │ └── bcmgps_base.hpp │ └── bcmgps_plugin │ │ └── bcmgps_plugin.hpp ├── package.xml ├── plugin.xml └── src │ ├── bcm_gps.cpp │ └── bcmgps_plugin.cpp ├── cyberdog_lidar ├── CMakeLists.txt ├── README.md ├── doc │ ├── cyberdog_lidar_cn.md │ ├── cyberdog_lidar_en.md │ └── image │ │ ├── cyberdog_lidar_function.png │ │ ├── cyberdog_lidar_function.svg │ │ ├── cyberdog_lidar_module.png │ │ ├── cyberdog_lidar_module.svg │ │ └── cyberdog_lidar_scan.png ├── include │ ├── lidar_base │ │ └── lidar_base.hpp │ └── lidar_plugin │ │ └── ydlidar_plugin.hpp ├── package.xml ├── plugin.xml └── src │ └── ydlidar_plugin.cpp ├── cyberdog_tof ├── CMakeLists.txt ├── README.md ├── include │ ├── tof_base │ │ └── tof_base.hpp │ └── tof_plugin │ │ └── tof_plugin.hpp ├── package.xml ├── plugin.xml └── src │ └── tof_plugin.cpp ├── cyberdog_ultrasonic ├── CMakeLists.txt ├── README.md ├── include │ ├── ultrasonic_base │ │ └── ultrasonic_base.hpp │ └── ultrasonic_plugin │ │ └── ultrasonic_plugin.hpp ├── package.xml ├── plugin.xml └── src │ └── ultrasonic_plugin.cpp └── sensor_manager ├── CMakeLists.txt ├── include └── sensor_manager │ ├── self_check.hpp │ └── sensor_manager.hpp ├── package.xml └── src ├── main.cpp └── sensor_manager.cpp /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Desktop (please complete the following information):** 27 | - OS: [e.g. iOS] 28 | - Browser [e.g. chrome, safari] 29 | - Version [e.g. 22] 30 | 31 | **Smartphone (please complete the following information):** 32 | - Device: [e.g. iPhone6] 33 | - OS: [e.g. iOS8.1] 34 | - Browser [e.g. stock browser, safari] 35 | - Version [e.g. 22] 36 | 37 | **Additional context** 38 | Add any other context about the problem here. 39 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/custom.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Custom issue template 3 | about: Describe this issue template's purpose here. 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | 11 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /.github/workflows/workflow.yml: -------------------------------------------------------------------------------- 1 | name: GitHub Actions CI 2 | run-name: ${{ github.actor }} is run GitHub Actions 🚀 3 | on: [push] 4 | defaults: 5 | run: 6 | shell: bash 7 | jobs: 8 | build-job: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: Login to Docker Hub 12 | uses: docker/login-action@v2 13 | with: 14 | registry: ghcr.io 15 | username: ${{ github.actor }} 16 | password: ${{ secrets.GIT_TOKEN }} 17 | - name: Install dependence 18 | run: | 19 | sudo apt install -y qemu-user-static binfmt-support 20 | - name: Download code 21 | uses: actions/checkout@v3 22 | - name: Build and code test 23 | run: | 24 | docker pull ghcr.io/miroboticslab/cyberdog:v1 25 | docker run -i -v $GITHUB_WORKSPACE:/home/ros2/src ghcr.io/miroboticslab/cyberdog:v1 bash -c \ 26 | "cd /home/ros2 && source /opt/ros2/galactic/setup.bash \ 27 | && colcon build --packages-up-to sensor_manager cyberdog_gps cyberdog_ultrasonic cyberdog_tof cyberdog_lidar \ 28 | && colcon test --event-handlers console_cohesion+ --return-code-on-test-failure --packages-select sensor_manager cyberdog_gps cyberdog_ultrasonic cyberdog_tof cyberdog_lidar" 29 | # colcon build test 30 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # cache and binary files during ros build 2 | build/ 3 | log/ 4 | install/ 5 | 6 | # files create by macOS 7 | *.DS_Store 8 | 9 | # vs code workspace 10 | .vscode/ 11 | 12 | # swp files 13 | *.swp 14 | 15 | .github 16 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 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 (C) 2023 Xiaomi Corporation. 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 | # cyberdog_sensors 2 | 3 | -------------------------------------------------------------------------------- /cyberdog_gps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_gps) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | include_directories(/opt/ros2/galactic/include) 9 | link_directories(/opt/ros2/galactic/lib) 10 | 11 | # find dependencies 12 | find_package(ament_cmake REQUIRED) 13 | find_package(ament_cmake_ros REQUIRED) 14 | find_package(pluginlib REQUIRED) 15 | find_package(params REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(cyberdog_common REQUIRED) 18 | find_package(protocol REQUIRED) 19 | find_package(cyberdog_system REQUIRED) 20 | 21 | 22 | 23 | add_library(${PROJECT_NAME} SHARED 24 | src/bcmgps_plugin.cpp 25 | src/bcm_gps.cpp 26 | ) 27 | 28 | target_include_directories(${PROJECT_NAME} PUBLIC 29 | $ 30 | $) 31 | 32 | target_link_libraries(${PROJECT_NAME} 33 | ${cyberdog_log_LIBRARIES} 34 | ) 35 | 36 | target_link_libraries(${PROJECT_NAME} bream_vendor) 37 | 38 | # define project name to get relative share path 39 | target_compile_definitions(${PROJECT_NAME} PRIVATE "PACKAGE_NAME=\"${PROJECT_NAME}\"") 40 | target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 41 | 42 | ament_target_dependencies( 43 | ${PROJECT_NAME} 44 | pluginlib 45 | params 46 | rclcpp 47 | cyberdog_common 48 | ament_index_cpp 49 | protocol 50 | ) 51 | 52 | pluginlib_export_plugin_description_file(${PROJECT_NAME} plugin.xml) 53 | 54 | install( 55 | DIRECTORY include/ 56 | DESTINATION include/ 57 | ) 58 | 59 | install( 60 | TARGETS ${PROJECT_NAME} 61 | EXPORT export_${PROJECT_NAME} 62 | ARCHIVE DESTINATION lib 63 | LIBRARY DESTINATION lib 64 | RUNTIME DESTINATION bin 65 | ) 66 | 67 | if(BUILD_TESTING) 68 | find_package(ament_lint_auto REQUIRED) 69 | ament_lint_auto_find_test_dependencies() 70 | find_package(ament_cmake_gtest REQUIRED) 71 | endif() 72 | 73 | ament_export_include_directories(include) 74 | ament_export_libraries(${PROJECT_NAME}) 75 | ament_export_targets(export_${PROJECT_NAME}) 76 | ament_package() 77 | -------------------------------------------------------------------------------- /cyberdog_gps/README.md: -------------------------------------------------------------------------------- 1 | # bcm_gps 2 | 3 | 该模块主要提供GPS的驱动封装 4 | 5 | - 外部参数通过`cyberdog_ception_bridge`模块中`params/bcmgps_config.toml`载入 6 | - GPS模块自身日志保存在`cyberdog_ception_bridge`模块中`logs`路径下 7 | 8 | ### 主要接口部分如下： 9 | 10 | ```cpp 11 | namespace bcm_gps 12 | { 13 | typedef LD2BRM_PvtPvtPolledPayload GPS_Payload; 14 | using NMEA_callback = std::function; 15 | using PAYLOAD_callback = std::function payload)>; 16 | 17 | class GPS 18 | { 19 | public: 20 | explicit GPS(PAYLOAD_callback PAYLOAD_cb = nullptr, NMEA_callback NMEA_cb = nullptr); 21 | ~GPS(); 22 | bool Open(); 23 | void Start(); 24 | void Stop(); 25 | void Close(); 26 | bool Ready(); 27 | void SetCallback(NMEA_callback NMEA_cb); 28 | void SetCallback(PAYLOAD_callback PAYLOAD_cb); 29 | 30 | void SetL5Bias(uint32_t biasCm); 31 | void SetLteFilterEn(bool enable = true); 32 | }; // class GPS 33 | } // namespace bcm_gps 34 | ``` 35 | 36 | #### public函数： 37 | 38 | 构造函数：初始化GPS对象 39 | 40 | ```cpp 41 | explicit GPS(PAYLOAD_callback PAYLOAD_cb = nullptr, NMEA_callback NMEA_cb = nullptr); 42 | ``` 43 | 44 | - `PAYLOAD_cb`：payload形式回调函数 45 | - `NMEA_cb`：NMEA标准格式字符串回调函数 46 | 47 | 48 | 49 | 开启设备：返回是否开启成功（构造函数会自动开启设备，主要用于手动调用Close()后重新开启） 50 | 51 | ```cpp 52 | bool Open(); 53 | ``` 54 | 55 | 56 | 57 | 打开定位 58 | 59 | ```cpp 60 | void Start(); 61 | ``` 62 | 63 | 64 | 65 | 关闭定位 66 | 67 | ```cpp 68 | void Stop(); 69 | ``` 70 | 71 | 72 | 73 | 关闭设备（关闭后会清空注册的回调函数，重新打开需要Open()后重新注册回调函数获取数据） 74 | 75 | ```cpp 76 | void Close(); 77 | ``` 78 | 79 | 80 | 81 | 获取硬件设备准备状态：返回准备是否成功 82 | 83 | ```cpp 84 | bool Ready(); 85 | ``` 86 | 87 | 88 | 89 | 注册回调函数 90 | 91 | ```cpp 92 | void SetCallback(NMEA_callback NMEA_cb); 93 | void SetCallback(PAYLOAD_callback PAYLOAD_cb); 94 | ``` 95 | 96 | - `PAYLOAD_cb`：payload形式回调函数 97 | - `NMEA_cb`：NMEA标准格式字符串回调函数 98 | 99 | 100 | 101 | Input L5 bias in centimeter 102 | 103 | ```cpp 104 | void SetL5Bias(uint32_t biasCm); 105 | ``` 106 | 107 | - biasCm：L5 bias in centimeter 108 | 109 | 110 | 111 | Enable/Disable LTE Filter 112 | 113 | ```cpp 114 | void SetLteFilterEn(bool enable = true); 115 | ``` 116 | 117 | - enable：Enable/Disable 118 | 119 | 120 | 121 | #### GPS_Payload消息结构 122 | 123 | ```cpp 124 | /** @brief BRM-NAV-PVT (0x01 0x07) 125 | * 126 | */ 127 | PACK( 128 | typedef struct 129 | { 130 | U4 iTOW; 131 | U2 year; 132 | U1 month; 133 | U1 day; 134 | U1 hour; 135 | U1 min; 136 | U1 sec; 137 | X1 valid; 138 | U4 tAcc; 139 | I4 nano; 140 | U1 fixType; 141 | X1 flags; 142 | X1 flags2; 143 | U1 numSV; 144 | I4 lon; 145 | I4 lat; 146 | I4 height; 147 | I4 hMSL; 148 | U4 hAcc; 149 | U4 vAcc; 150 | I4 velN; 151 | I4 velE; 152 | I4 velD; 153 | I4 gSpeed; 154 | I4 headMot; 155 | U4 sAcc; 156 | U4 headAcc; 157 | U2 pDOP; 158 | I1 leapS; 159 | U1 reserved1[5]; 160 | I4 headVeh; 161 | I2 magDec; 162 | U2 magAcc; 163 | }) LD2BRM_PvtPvtPolledPayload; 164 | ``` 165 | 166 | 167 | 168 | ### 外部参数 169 | 170 | 外部参数通过`cyberdog_ception_bridge`模块中`params/bcmgps_config.toml`载入 171 | 172 | ```toml 173 | skip_download = false # need in first line 174 | onlyfirst_download = true 175 | tty = "/dev/ttyTHS0" 176 | patch_path = "/usr/sbin/bream.patch" 177 | 178 | #uint8_t infMsgMask[6] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F}; 179 | #full logging (GLLIO, GLLAPI, RAWDATA, DEVKF, USR2, ) 180 | infMsgMask = [31, 31, 31, 31, 31, 31] 181 | 182 | #1(L1 Best) 2(L1 Auto) 3(L1 ULP), 4(L1L5 Best) 5(L1L5 Auto) 183 | PowerModePreset = 4 184 | 185 | MsgRate = [ 186 | 1, # Report GPGGA rate 187 | 1, # Report GPRMC rate 188 | 1, # Report GPGSV rate 189 | 1, # Report NAVEOE rate 190 | 1, # Report NAVPVT rate 191 | 1, # Report PGLOR SPEED rate 192 | 1, # Report PGLOR FIX rate 193 | 1, # Report PGLOR SAT rate 194 | 1, # Report PGLOR LSQ rate 195 | 1, # Report PGLOR PWR rate 196 | 1, # Report PGLOR STA rate 197 | 1, # Report NAVODO rate 198 | 1, # Report NAVSAT rate 199 | 1, # Report NAVDOP rate 200 | 1, # Report CBEE status rate 201 | 1, # Report ASC SUBFRAMES rate 202 | 1, # Report ASC MEAS rate 203 | 1 # Report ASC AGC rate 204 | ] 205 | 206 | AckAiding = true 207 | ``` 208 | 209 | - `skip_download`：是否跳过下载patch（请置于首行） 210 | - `onlyfirst_download`：仅编译后首次下载（该选项为`true`将会在下载后将`skip_download`置为`true`） 211 | - `tty`：设备号 212 | - `patch_path`：下载patch路径 213 | - `infMsgMask`：GPS模块日志配置 214 | - `PowerModePreset`：GPS频段选择 215 | - 1：(L1 Best) 216 | - 2：(L1 Auto) 217 | - 3：(L1 ULP) 218 | - 4：(L1L5 Best) (默认) 219 | - 5：(L1L5 Auto) 220 | - `MsgRate`：消息频率设置（0为关闭） 221 | - `AckAiding`：true/false 222 | 223 | 224 | 225 | ### Example 226 | 227 | ```cpp 228 | class Example 229 | { 230 | public: 231 | std::shared_ptr gps_; 232 | std::shared_ptr payload_; 233 | 234 | Example() 235 | { 236 | gps_ = std::make_shared(std::bind( 237 | &Example::payload_callback, this, std::placeholders::_1)); 238 | } 239 | 240 | void Func() 241 | { 242 | gps_->Start(); 243 | gps_->Stop(); 244 | gps_->Close(); 245 | } 246 | void payload_callback(std::shared_ptr payload) 247 | { 248 | payload_ = payload; 249 | } 250 | }; // class example 251 | ``` 252 | 253 | -------------------------------------------------------------------------------- /cyberdog_gps/include/bcm_gps/bcm_gps.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // Licensed under the Apache License, Version 2.0 (the "License"); 3 | // you may not use this file except in compliance with the License. 4 | // You may obtain a copy of the License at 5 | // 6 | // http://www.apache.org/licenses/LICENSE-2.0 7 | // 8 | // Unless required by applicable law or agreed to in writing, software 9 | // distributed under the License is distributed on an "AS IS" BASIS, 10 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | // See the License for the specific language governing permissions and 12 | // limitations under the License. 13 | 14 | #ifndef BCM_GPS__BCM_GPS_HPP_ 15 | #define BCM_GPS__BCM_GPS_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "bream_vendor/bream.h" 24 | #include "bream_vendor/bream_callbacks.hpp" 25 | namespace bcm_gps 26 | { 27 | class GPS 28 | { 29 | public: 30 | explicit GPS(PAYLOAD_callback PAYLOAD_cb = nullptr, NMEA_callback NMEA_cb = nullptr); 31 | ~GPS(); 32 | bool Open(); 33 | void Start(); 34 | void Stop(); 35 | void Close(); 36 | bool Ready(); 37 | bool IsStarted(); 38 | bool IsOpened(); 39 | void SetCallback(NMEA_callback NMEA_cb); 40 | void SetCallback(PAYLOAD_callback PAYLOAD_cb); 41 | 42 | void SetL5Bias(uint32_t biasCm); 43 | void SetLteFilterEn(bool enable = true); 44 | 45 | private: 46 | inline static int all_num_; 47 | inline static int init_num_; 48 | inline static int start_num_; 49 | inline static bool ready_; 50 | inline static bool main_running_; 51 | inline static std::thread main_T_; 52 | 53 | int id_; 54 | bool opened_ = false; 55 | bool start_ = false; 56 | bool Init(); 57 | void MainThread(); 58 | }; // class GPS 59 | } // namespace bcm_gps 60 | 61 | #endif // BCM_GPS__BCM_GPS_HPP_ 62 | -------------------------------------------------------------------------------- /cyberdog_gps/include/bcmgps_base/bcmgps_base.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // Licensed under the Apache License, Version 2.0 (the "License"); 3 | // you may not use this file except in compliance with the License. 4 | // You may obtain a copy of the License at 5 | // 6 | // http://www.apache.org/licenses/LICENSE-2.0 7 | // 8 | // Unless required by applicable law or agreed to in writing, software 9 | // distributed under the License is distributed on an "AS IS" BASIS, 10 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | // See the License for the specific language governing permissions and 12 | // limitations under the License. 13 | 14 | #ifndef BCMGPS_BASE__BCMGPS_BASE_HPP_ 15 | #define BCMGPS_BASE__BCMGPS_BASE_HPP_ 16 | 17 | #include 18 | #include 19 | #include "protocol/msg/gps_payload.hpp" 20 | 21 | namespace cyberdog 22 | { 23 | 24 | namespace sensor 25 | { 26 | 27 | class Cyberdog_GPS_payload 28 | { 29 | public: 30 | uint32_t iTOW; // the GPS Timestamps 31 | uint8_t fixType; // GNSSfix Type: 32 | uint8_t numSV; // Number of satellites used (range: 0-12) 33 | double lon; // Longitude 34 | double lat; // Latitude 35 | }; // class Cyberdog_GPS_payload 36 | 37 | class GpsBase 38 | { 39 | public: 40 | virtual int32_t Init(bool simulator = false) = 0; 41 | std::function Open, Start, Stop, Close; 42 | virtual int32_t Open_() = 0; 43 | virtual int32_t Start_() = 0; 44 | virtual int32_t Stop_() = 0; 45 | virtual int32_t Close_() = 0; 46 | virtual int32_t SelfCheck() = 0; 47 | virtual int32_t LowPowerOn() = 0; 48 | virtual int32_t LowPowerOff() = 0; 49 | virtual ~GpsBase() {} 50 | void SetPayloadCallback( 51 | std::function payload)> cb) 53 | { 54 | payload_callback_ = cb; 55 | } 56 | 57 | protected: 58 | std::function payload)> payload_callback_; 59 | GpsBase() {} 60 | }; // class GpsBase 61 | } // namespace sensor 62 | } // namespace cyberdog 63 | 64 | #endif // BCMGPS_BASE__BCMGPS_BASE_HPP_ 65 | -------------------------------------------------------------------------------- /cyberdog_gps/include/bcmgps_plugin/bcmgps_plugin.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // Licensed under the Apache License, Version 2.0 (the "License"); 3 | // you may not use this file except in compliance with the License. 4 | // You may obtain a copy of the License at 5 | // 6 | // http://www.apache.org/licenses/LICENSE-2.0 7 | // 8 | // Unless required by applicable law or agreed to in writing, software 9 | // distributed under the License is distributed on an "AS IS" BASIS, 10 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | // See the License for the specific language governing permissions and 12 | // limitations under the License. 13 | 14 | #ifndef BCMGPS_PLUGIN__BCMGPS_PLUGIN_HPP_ 15 | #define BCMGPS_PLUGIN__BCMGPS_PLUGIN_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include "bcm_gps/bcm_gps.hpp" 21 | #include "bcmgps_base/bcmgps_base.hpp" 22 | #include "cyberdog_common/cyberdog_log.hpp" 23 | #include "cyberdog_system/robot_code.hpp" 24 | 25 | namespace cyberdog 26 | { 27 | namespace sensor 28 | { 29 | namespace SYS = cyberdog::system; 30 | 31 | class GpsCarpo : public cyberdog::sensor::GpsBase 32 | { 33 | using SwitchState = enum {open = 0, start, stop, close, }; // [类型]切换状态 34 | 35 | public: 36 | int32_t Init(bool simulator = false) override; 37 | int32_t Open_() override; 38 | int32_t Start_() override; 39 | int32_t Stop_() override; 40 | int32_t Close_() override; 41 | int32_t SelfCheck() override; 42 | int32_t LowPowerOn() override; 43 | int32_t LowPowerOff() override; 44 | 45 | public: 46 | enum class GpsCode : int32_t 47 | { 48 | kDemoError1 = 21 49 | }; 50 | 51 | private: 52 | bool is_open = false; 53 | bool is_start = false; 54 | bool is_stop = false; 55 | std::shared_ptr> code_{nullptr}; 56 | std::map state_msg_; // 状态消息 57 | std::thread gps_pub_thread_simulator; 58 | void UpdateSimulationData(); // 更新模拟数据 59 | std::shared_ptr bcmgps_; 60 | void BCMGPS_Payload_callback(std::shared_ptr payload); 61 | LOGGER_MINOR_INSTANCE("cyberdog_gps"); 62 | }; // class Cyberdog_BCMGPS 63 | } // namespace sensor 64 | } // namespace cyberdog 65 | 66 | #endif // BCMGPS_PLUGIN__BCMGPS_PLUGIN_HPP_ 67 | -------------------------------------------------------------------------------- /cyberdog_gps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_gps 5 | 1.0.0 6 | GPS pluginlib plugins 7 | ZhengJunyuan 8 | Apache License, Version 2.0 9 | 10 | ament_cmake_ros 11 | 12 | pluginlib 13 | rclcpp 14 | params 15 | cyberdog_common 16 | cyberdog_system 17 | ament_index_cpp 18 | bream_vendor 19 | protocol 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | -------------------------------------------------------------------------------- /cyberdog_gps/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is a bcm_gps plugin. 4 | 5 | -------------------------------------------------------------------------------- /cyberdog_gps/src/bcm_gps.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // Licensed under the Apache License, Version 2.0 (the "License"); 3 | // you may not use this file except in compliance with the License. 4 | // You may obtain a copy of the License at 5 | // 6 | // http://www.apache.org/licenses/LICENSE-2.0 7 | // 8 | // Unless required by applicable law or agreed to in writing, software 9 | // distributed under the License is distributed on an "AS IS" BASIS, 10 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | // See the License for the specific language governing permissions and 12 | // limitations under the License. 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "ament_index_cpp/get_package_share_directory.hpp" 22 | #include "cyberdog_common/cyberdog_toml.hpp" 23 | #include "bcm_gps/bcm_gps.hpp" 24 | #include "bream_vendor/bream_callbacks.hpp" 25 | #include "bream_vendor/patch_downloader.h" 26 | #include "bream_vendor/bream_helper.h" 27 | #include "cyberdog_common/cyberdog_log.hpp" 28 | 29 | 30 | bcm_gps::GPS::GPS(PAYLOAD_callback PAYLOAD_cb, NMEA_callback NMEA_cb) 31 | { 32 | if (all_num_ == 0) { 33 | NMEA_callbacks_ = std::map(); 34 | PAYLOAD_callbacks_ = std::map(); 35 | } 36 | id_ = all_num_++; 37 | Open(); 38 | SetCallback(PAYLOAD_cb); 39 | SetCallback(NMEA_cb); 40 | } 41 | 42 | bcm_gps::GPS::~GPS() 43 | { 44 | // Close(); 45 | INFO("[cyberdog_gps] destroy GPS"); 46 | } 47 | 48 | bool bcm_gps::GPS::Open() 49 | { 50 | if (opened_ || (ready_ == false && Init() == false)) { 51 | return opened_; 52 | } 53 | INFO("[cyberdog_gps]: GPS%d Opened", id_); 54 | opened_ = true; 55 | init_num_++; 56 | return true; 57 | } 58 | 59 | void bcm_gps::GPS::Start() 60 | { 61 | if (ready_ == false) { 62 | ERROR("[cyberdog_gps]: GPS module not ready yet"); 63 | return; 64 | } 65 | if (start_) {return;} 66 | if (start_num_++ == 0) { 67 | BreamHelper::GetInstance().GnssStart(); 68 | start_ = true; 69 | INFO("[cyberdog_gps]: GnssStart"); 70 | } 71 | } 72 | 73 | void bcm_gps::GPS::Stop() 74 | { 75 | if (ready_ == false) { 76 | ERROR("[cyberdog_gps][Stop]: GPS module not ready yet"); 77 | return; 78 | } 79 | if (start_ == false) {return;} 80 | start_ = false; 81 | if (--start_num_ == 0) { 82 | INFO("[cyberdog_gps]: GnssStop"); 83 | BreamHelper::GetInstance().GnssStop(); 84 | } 85 | } 86 | 87 | void bcm_gps::GPS::Close() 88 | { 89 | Stop(); 90 | if (opened_ == false) {return;} 91 | if (NMEA_callbacks_.count(id_) != 0) {NMEA_callbacks_.erase(id_);} 92 | if (PAYLOAD_callbacks_.count(id_) != 0) {PAYLOAD_callbacks_.erase(id_);} 93 | if (ready_ && opened_ && --init_num_ == 0) { 94 | INFO("[cyberdog_gps]: Last usage close, exit main thread"); 95 | main_running_ = false; 96 | main_T_.join(); 97 | } 98 | opened_ = false; 99 | } 100 | 101 | bool bcm_gps::GPS::Ready() 102 | { 103 | return ready_; 104 | } 105 | 106 | bool bcm_gps::GPS::IsOpened() 107 | { 108 | return opened_; 109 | } 110 | 111 | bool bcm_gps::GPS::IsStarted() 112 | { 113 | return start_; 114 | } 115 | 116 | 117 | void bcm_gps::GPS::SetCallback(NMEA_callback NMEA_cb) 118 | { 119 | if (NMEA_cb == nullptr) {return;} 120 | if (NMEA_callbacks_.count(id_) != 0) {NMEA_callbacks_.at(id_) = NMEA_cb;} else { 121 | NMEA_callbacks_.insert(std::map::value_type(id_, NMEA_cb)); 122 | } 123 | INFO("[cyberdog_gps]: GPS%dSuccess SetCallback NMEA", id_); 124 | } 125 | 126 | void bcm_gps::GPS::SetCallback(PAYLOAD_callback PAYLOAD_cb) 127 | { 128 | if (PAYLOAD_cb == nullptr) {return;} 129 | if (PAYLOAD_callbacks_.count(id_) != 0) {PAYLOAD_callbacks_.at(id_) = PAYLOAD_cb;} else { 130 | PAYLOAD_callbacks_.insert(std::map::value_type(id_, PAYLOAD_cb)); 131 | } 132 | INFO("[cyberdog_gps]: GPS%d Success SetCallback PAYLOAD", id_); 133 | } 134 | 135 | void bcm_gps::GPS::SetL5Bias(uint32_t biasCm) 136 | { 137 | BreamHelper::GetInstance().SetL5Bias(biasCm); 138 | } 139 | 140 | void bcm_gps::GPS::SetLteFilterEn(bool enable) 141 | { 142 | BreamHelper::GetInstance().SetLteFilterEn(enable); 143 | } 144 | 145 | void change(char * ptr, const char * str) 146 | { 147 | while (*str != '\0') { 148 | *ptr = *str; 149 | ptr++; 150 | str++; 151 | } 152 | } 153 | 154 | // make "skip_download = true" in bcmgps_config.toml 155 | void SetSkip(std::string path) 156 | { 157 | try { 158 | FILE * fp1; 159 | if ((fp1 = fopen(path.c_str(), "rw+")) != NULL) { 160 | char tmp[40]; 161 | fread(tmp, sizeof(char), 40, fp1); 162 | for (int a = 0; a < 40; a++) { 163 | if (tmp[a] == 'f') { 164 | change(&tmp[a], "true "); 165 | break; 166 | } 167 | } 168 | fseek(fp1, 0, 0); 169 | fwrite(tmp, sizeof(char), 40, fp1); 170 | fclose(fp1); 171 | INFO("[cyberdog_gps]: :Write file: %s", path.c_str()); 172 | return; 173 | } 174 | } catch (...) { 175 | } 176 | ERROR("[cyberdog_gps]: Cant write file"); 177 | } 178 | 179 | bool bcm_gps::GPS::Init() 180 | { 181 | INFO("[cyberdog_gps]: BCM_GPS Init"); 182 | // change toml11vondor to cyberdog_common 183 | toml::value value; 184 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 185 | auto local_config_dir = local_share_dir + std::string("/toml_config/sensors/bcmgps_config.toml"); 186 | if (access(local_config_dir.c_str(), F_OK) != 0) { 187 | ERROR("[cyberdog_gps]: %s do not exist!", local_config_dir.c_str()); 188 | ERROR("[cyberdog_gps]: init failed"); 189 | return false; 190 | } 191 | if (!cyberdog::common::CyberdogToml::ParseFile( 192 | std::string(local_share_dir) + 193 | "/toml_config/sensors/bcmgps_config.toml", value)) 194 | { 195 | ERROR("[cyberdog_gps]: fail to read data from toml"); 196 | } 197 | bool skip_download; 198 | if (!cyberdog::common::CyberdogToml::Get(value, "skip_download", skip_download)) { 199 | ERROR("[cyberdog_gps]: fail to read key skip_download from toml"); 200 | } 201 | bool onlyfirst_download; 202 | if (!cyberdog::common::CyberdogToml::Get(value, "onlyfirst_download", onlyfirst_download)) { 203 | ERROR("[cyberdog_gps]: fail to read key onlyfirst_download from toml"); 204 | } 205 | std::string spi_str; 206 | if (!cyberdog::common::CyberdogToml::Get(value, "spi", spi_str)) { 207 | ERROR("[cyberdog_gps]: fail to read key spi from toml"); 208 | } 209 | std::string patch_path; 210 | if (!cyberdog::common::CyberdogToml::Get(value, "patch_path", patch_path)) { 211 | ERROR("[cyberdog_gps]: fail to read key patch_path from toml"); 212 | } 213 | 214 | std::vector infMsgMask_vec; 215 | 216 | if (!cyberdog::common::CyberdogToml::Get(value, "infMsgMask", infMsgMask_vec)) { 217 | ERROR("[cyberdog_gps]: fail to read key infMsgMask from toml"); 218 | } 219 | uint8_t PowerModePreset; 220 | if (!cyberdog::common::CyberdogToml::Get(value, "PowerModePreset", PowerModePreset)) { 221 | ERROR("[cyberdog_gps]: fail to read key PowerModePreset from toml"); 222 | } 223 | std::vector MsgRate_vec; 224 | if (!cyberdog::common::CyberdogToml::Get(value, "MsgRate", MsgRate_vec)) { 225 | ERROR("[cyberdog_gps]: fail to read key MsgRate from toml"); 226 | } 227 | bool AckAiding; 228 | if (!cyberdog::common::CyberdogToml::Get(value, "AckAiding", AckAiding)) { 229 | ERROR("[cyberdog_gps]: fail to read key AckAiding from toml"); 230 | } 231 | 232 | if (access(patch_path.c_str(), F_OK) != 0) { 233 | ERROR("[cyberdog_gps]: %s do not exist!", patch_path.c_str()); 234 | ERROR("[cyberdog_gps]: init failed"); 235 | return false; 236 | } 237 | 238 | // reset entire chip 239 | LD2OS_initGpio(); 240 | int portNumber = -1; 241 | const int baudrate = 3000000; 242 | const char * tty = spi_str.c_str(); 243 | LoDi2SerialConnection connType = LODI2_SERIAL_SPI; 244 | 245 | if (!skip_download) { 246 | INFO("[cyberdog_gps]: Start load patch: %s", patch_path.c_str()); 247 | // Open serial port 248 | INFO("[cyberdog_gps]: tty: %s", tty); 249 | 250 | LD2OS_open(connType, portNumber, baudrate, tty); 251 | // Download patch 252 | INFO("[cyberdog_gps]: down load patch: %s", patch_path.c_str()); 253 | if (false == Bream_LoadPatch(patch_path.c_str())) { 254 | INFO("[cyberdog_gps]: Cant load patch: %s", patch_path.c_str()); 255 | ready_ = false; 256 | return false; 257 | } 258 | INFO("[cyberdog_gps]: Success load patch: %s", patch_path.c_str()); 259 | 260 | } else { 261 | INFO("[cyberdog_gps]: Skip load patch: %s", patch_path.c_str()); 262 | } 263 | if (onlyfirst_download == true && skip_download == false) {SetSkip(local_config_dir);} 264 | 265 | if (connType == LODI2_SERIAL_UART) { 266 | // Reopen and send command to switch baudrate from default 115200 267 | LD2OS_open(connType, portNumber, 115200, tty); 268 | 269 | BreamHelper::GetInstance().SetBaudrate(baudrate); 270 | // delay needed because MCU needs some time in receiving CFG-PRT and handling it. 271 | // This delay can be replaced by checking ACK for CFG-PRT 272 | LD2OS_delay(100); 273 | // Reopen in new baudrate 274 | LD2OS_open(connType, portNumber, baudrate, tty); 275 | } 276 | // Register callbacks and start listener loop 277 | main_T_ = std::thread(std::bind(&GPS::MainThread, this)); 278 | // If SPI connection, run read thread first and then send first packet to mcu 279 | if (connType == LODI2_SERIAL_SPI) { 280 | LD2OS_delay(200); 281 | BreamHelper::GetInstance().SetBaudrate(baudrate, 4); 282 | } 283 | 284 | // Config GNSS 285 | INFO("[cyberdog_gps]: ConfigGNSS"); 286 | uint8_t infMsgMask[6]; 287 | for (int a = 0; a < 6 && a < static_cast(infMsgMask_vec.size()); a++) { 288 | infMsgMask[a] = infMsgMask_vec[a]; 289 | } 290 | BreamHelper::GetInstance().SetLogging(infMsgMask); 291 | BreamHelper::GetInstance().EnableBlindGalSearch(); 292 | 293 | // 1(L1 Best) 2(L1 Auto) 3(L1 ULP), 4(L1L5 Best) 5(L1L5 Auto) 294 | switch (PowerModePreset) { 295 | case 1: BreamHelper::GetInstance().SetPowerModePreset(0, 0); break; 296 | case 2: BreamHelper::GetInstance().SetPowerModePreset(0, 1); break; 297 | case 3: BreamHelper::GetInstance().SetPowerModePreset(0, 3); break; 298 | default: 299 | case 4: BreamHelper::GetInstance().SetPowerModePreset(1, 0); break; 300 | case 5: BreamHelper::GetInstance().SetPowerModePreset(1, 1); break; 301 | } 302 | 303 | for (int a = static_cast(MsgRate_vec.size()); a < 18; a++) { 304 | MsgRate_vec.push_back(0); 305 | } 306 | int index = 0; 307 | BreamHelper::GetInstance().SetMsgRate(0xF0, 0x00, MsgRate_vec[index++]); // Report GPGGA 308 | BreamHelper::GetInstance().SetMsgRate(0xF0, 0x04, MsgRate_vec[index++]); // Report GPRMC 309 | BreamHelper::GetInstance().SetMsgRate(0xF0, 0x03, MsgRate_vec[index++]); // Report GPGSV 310 | BreamHelper::GetInstance().SetMsgRate(0x01, 0x61, MsgRate_vec[index++]); // Report NAVEOE 311 | BreamHelper::GetInstance().SetMsgRate(0x01, 0x07, MsgRate_vec[index++]); // Report NAVPVT 312 | BreamHelper::GetInstance().SetMsgRate(0xF1, 0x00, MsgRate_vec[index++]); // Report PGLOR SPEED 313 | BreamHelper::GetInstance().SetMsgRate(0xF1, 0x01, MsgRate_vec[index++]); // Report PGLOR FIX 314 | BreamHelper::GetInstance().SetMsgRate(0xF1, 0x02, MsgRate_vec[index++]); // Report PGLOR SAT 315 | BreamHelper::GetInstance().SetMsgRate(0xF1, 0x03, MsgRate_vec[index++]); // Report PGLOR LSQ 316 | BreamHelper::GetInstance().SetMsgRate(0xF1, 0x04, MsgRate_vec[index++]); // Report PGLOR PWR 317 | BreamHelper::GetInstance().SetMsgRate(0xF1, 0x05, MsgRate_vec[index++]); // Report PGLOR STA 318 | BreamHelper::GetInstance().SetMsgRate(0x01, 0x09, MsgRate_vec[index++]); // Report NAVODO 319 | BreamHelper::GetInstance().SetMsgRate(0x01, 0x35, MsgRate_vec[index++]); // Report NAVSAT 320 | BreamHelper::GetInstance().SetMsgRate(0x01, 0x04, MsgRate_vec[index++]); // Report NAVDOP 321 | BreamHelper::GetInstance().SetMsgRate(0x01, 0x60, MsgRate_vec[index++]); // Report CBEE status 322 | BreamHelper::GetInstance().SetMsgRate(0x02, 0x13, MsgRate_vec[index++]); // Report ASC SUBFRAMES 323 | BreamHelper::GetInstance().SetMsgRate(0x02, 0x15, MsgRate_vec[index++]); // Report ASC MEAS 324 | BreamHelper::GetInstance().SetMsgRate(0x02, 0x80, MsgRate_vec[index++]); // Report ASC AGC 325 | BreamHelper::GetInstance().SetAckAiding(AckAiding); 326 | BreamHelper::GetInstance().GetVer(); 327 | INFO("[cyberdog_gps]: Finish Config"); 328 | ready_ = true; 329 | return true; 330 | } 331 | 332 | void bcm_gps::GPS::MainThread() 333 | { 334 | INFO("[cyberdog_gps]: MainThread Start"); 335 | RegisterBreamCallbacks(); 336 | main_running_ = true; 337 | 338 | uint8_t rxBuff[1024]; 339 | uint32_t rxLen = 0; 340 | while (main_running_) { 341 | bool ret = LD2OS_readFromSerial(rxBuff, &rxLen, sizeof(rxBuff), -1); 342 | if (!ret) {break;} 343 | if (rxLen) { 344 | BreamHandler::GetInstance().AsicData(rxBuff, rxLen); 345 | } 346 | } 347 | ready_ = false; 348 | LD2OS_freeGpio(); 349 | LD2OS_close(); 350 | LD2OS_closeLog(); 351 | INFO("[cyberdog_gps]: MainThread Exit"); 352 | } 353 | -------------------------------------------------------------------------------- /cyberdog_gps/src/bcmgps_plugin.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // Licensed under the Apache License, Version 2.0 (the "License"); 3 | // you may not use this file except in compliance with the License. 4 | // You may obtain a copy of the License at 5 | // 6 | // http://www.apache.org/licenses/LICENSE-2.0 7 | // 8 | // Unless required by applicable law or agreed to in writing, software 9 | // distributed under the License is distributed on an "AS IS" BASIS, 10 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | // See the License for the specific language governing permissions and 12 | // limitations under the License. 13 | 14 | #include 15 | 16 | #include "bcmgps_plugin/bcmgps_plugin.hpp" 17 | #include "pluginlib/class_list_macros.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "cyberdog_common/cyberdog_log.hpp" 20 | 21 | 22 | int32_t cyberdog::sensor::GpsCarpo::Init(bool simulator) 23 | { 24 | this->state_msg_.insert({SwitchState::open, "Open"}); 25 | this->state_msg_.insert({SwitchState::start, "Start"}); 26 | this->state_msg_.insert({SwitchState::stop, "Stop"}); 27 | this->state_msg_.insert({SwitchState::close, "Close"}); 28 | const SYS::ModuleCode kModuleCode = SYS::ModuleCode::kGPS; 29 | code_ = std::make_shared>(kModuleCode); 30 | if (!simulator) { 31 | this->Open = std::bind(&cyberdog::sensor::GpsCarpo::Open_, this); 32 | this->Start = std::bind(&cyberdog::sensor::GpsCarpo::Start_, this); 33 | this->Stop = std::bind(&cyberdog::sensor::GpsCarpo::Stop_, this); 34 | this->Close = std::bind(&cyberdog::sensor::GpsCarpo::Close_, this); 35 | } else { 36 | auto Simulator = [this](SwitchState now_state) -> bool { 37 | INFO("%s gps ...", this->state_msg_[now_state].c_str()); 38 | switch (now_state) { 39 | case SwitchState::open: 40 | case SwitchState::stop: 41 | break; 42 | case SwitchState::start: 43 | gps_pub_thread_simulator = 44 | std::thread(std::bind(&cyberdog::sensor::GpsCarpo::UpdateSimulationData, this)); 45 | break; 46 | case SwitchState::close: 47 | if ((&gps_pub_thread_simulator != nullptr) && 48 | gps_pub_thread_simulator.joinable()) 49 | { 50 | gps_pub_thread_simulator.join(); 51 | } 52 | break; 53 | default: 54 | WARN("gps not recognized state"); 55 | break; 56 | } 57 | 58 | INFO("gps %s ok", this->state_msg_[now_state].c_str()); 59 | return code_->GetKeyCode(SYS::KeyCode::kOK); 60 | }; 61 | this->Open = std::bind(Simulator, SwitchState::open); 62 | this->Start = std::bind(Simulator, SwitchState::start); 63 | this->Stop = std::bind(Simulator, SwitchState::stop); 64 | this->Close = std::bind(Simulator, SwitchState::close); 65 | } 66 | return code_->GetKeyCode(SYS::KeyCode::kOK); 67 | } 68 | 69 | 70 | int32_t cyberdog::sensor::GpsCarpo::Open_() 71 | { 72 | if (is_open == true) { 73 | INFO("[cyberdog_gps] gps open successfully"); 74 | return code_->GetKeyCode(SYS::KeyCode::kOK); 75 | } 76 | bcmgps_ = std::make_shared(); 77 | bcmgps_->SetCallback( 78 | std::bind( 79 | &GpsCarpo::BCMGPS_Payload_callback, this, 80 | std::placeholders::_1)); 81 | is_open = bcmgps_->IsOpened(); 82 | if (is_open == true) { 83 | INFO("[cyberdog_gps] gps open successfully"); 84 | } else { 85 | INFO("[cyberdog_gps] gps open failed"); 86 | } 87 | if (is_open) { 88 | return code_->GetKeyCode(SYS::KeyCode::kOK); 89 | } else { 90 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 91 | } 92 | } 93 | 94 | int32_t cyberdog::sensor::GpsCarpo::Start_() 95 | { 96 | if (bcmgps_ != nullptr) {bcmgps_->Start();} 97 | is_start = bcmgps_->IsStarted(); 98 | if (is_start == true) { 99 | INFO("[cyberdog_gps] gps start successfully"); 100 | } else { 101 | INFO("[cyberdog_gps] gps start failed"); 102 | } 103 | if (is_start) { 104 | return code_->GetKeyCode(SYS::KeyCode::kOK); 105 | } else { 106 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 107 | } 108 | } 109 | 110 | int32_t cyberdog::sensor::GpsCarpo::Stop_() 111 | { 112 | if (bcmgps_ != nullptr) {bcmgps_->Stop();} 113 | is_stop = !(bcmgps_->IsStarted()); 114 | if (is_stop == true) { 115 | INFO("[cyberdog_gps] gps stop successfully"); 116 | } else { 117 | INFO("[cyberdog_gps] gps stop failed"); 118 | } 119 | if (is_stop) { 120 | return code_->GetKeyCode(SYS::KeyCode::kOK); 121 | } else { 122 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 123 | } 124 | } 125 | 126 | int32_t cyberdog::sensor::GpsCarpo::Close_() 127 | { 128 | Stop_(); 129 | INFO("[cyberdog_gps] gps close successfully"); 130 | if (is_stop) { 131 | return code_->GetKeyCode(SYS::KeyCode::kOK); 132 | } else { 133 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 134 | } 135 | /* 136 | if (bcmgps_ != nullptr) {bcmgps_->Close();} 137 | bool is_close = !(bcmgps_->IsOpened()); 138 | if (is_close == true) { 139 | INFO("[cyberdog_gps] gps close successfully"); 140 | } else { 141 | INFO("[cyberdog_gps] gps close failed"); 142 | } 143 | return is_close; 144 | */ 145 | } 146 | 147 | int32_t cyberdog::sensor::GpsCarpo::SelfCheck() 148 | { 149 | if (is_start) { 150 | return code_->GetKeyCode(SYS::KeyCode::kOK); 151 | } else { 152 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 153 | } 154 | } 155 | 156 | int32_t cyberdog::sensor::GpsCarpo::LowPowerOn() 157 | { 158 | return code_->GetKeyCode(SYS::KeyCode::kOK); 159 | } 160 | int32_t cyberdog::sensor::GpsCarpo::LowPowerOff() 161 | { 162 | return code_->GetKeyCode(SYS::KeyCode::kOK); 163 | } 164 | 165 | void cyberdog::sensor::GpsCarpo::BCMGPS_Payload_callback( 166 | std::shared_ptr payload) 167 | { 168 | // auto cyberdog_payload = std::make_shared(); 169 | auto cyberdog_payload = std::make_shared(); 170 | struct timespec time_stu; 171 | clock_gettime(CLOCK_REALTIME, &time_stu); 172 | cyberdog_payload->sec = time_stu.tv_sec; 173 | cyberdog_payload->nanosec = time_stu.tv_nsec; 174 | cyberdog_payload->itow = payload->iTOW; 175 | cyberdog_payload->lat = payload->lat * 1e-7; 176 | cyberdog_payload->lon = payload->lon * 1e-7; 177 | cyberdog_payload->fix_type = payload->fixType; 178 | cyberdog_payload->num_sv = payload->numSV; 179 | if (payload_callback_ != nullptr) {payload_callback_(cyberdog_payload);} else { 180 | INFO("[cyberdog_gps]: payload_callback_==nullptr "); 181 | } 182 | } 183 | void cyberdog::sensor::GpsCarpo::UpdateSimulationData() 184 | { 185 | auto cyberdog_payload = std::make_shared(); 186 | while (true) { 187 | if (!rclcpp::ok()) { 188 | WARN("[cyberdog_gps]: !rclcpp::ok()"); 189 | break; 190 | } 191 | // INFO("[cyberdog_gps]: publish gps payload succeed"); 192 | std::this_thread::sleep_for(std::chrono::microseconds(100000)); 193 | 194 | struct timespec time_stu; 195 | clock_gettime(CLOCK_REALTIME, &time_stu); 196 | cyberdog_payload->sec = time_stu.tv_sec; 197 | cyberdog_payload->nanosec = time_stu.tv_nsec; 198 | cyberdog_payload->itow = static_cast(0); 199 | cyberdog_payload->lat = static_cast<_Float64>(0); 200 | cyberdog_payload->lon = static_cast<_Float64>(0); 201 | cyberdog_payload->fix_type = static_cast(0); 202 | cyberdog_payload->num_sv = static_cast(0); 203 | payload_callback_(cyberdog_payload); 204 | // INFO("[cyberdog_gps]: publish gps payload succeed"); 205 | } 206 | } 207 | PLUGINLIB_EXPORT_CLASS(cyberdog::sensor::GpsCarpo, cyberdog::sensor::GpsBase) 208 | -------------------------------------------------------------------------------- /cyberdog_lidar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_lidar) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_ros REQUIRED) 11 | find_package(pluginlib REQUIRED) 12 | find_package(params REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(cyberdog_common REQUIRED) 15 | find_package(protocol REQUIRED) 16 | find_package(ydlidar_sdk REQUIRED) 17 | find_package(filters REQUIRED) 18 | find_package(cyberdog_system REQUIRED) 19 | 20 | link_directories(${YDLIDAR_SDK_LIBRARY_DIRS}) 21 | 22 | add_library(${PROJECT_NAME} SHARED 23 | src/ydlidar_plugin.cpp 24 | ) 25 | 26 | target_link_libraries(${PROJECT_NAME} 27 | ${YDLIDAR_SDK_LIBRARIES}) 28 | 29 | target_include_directories(${PROJECT_NAME} PUBLIC 30 | $ 31 | $) 32 | 33 | # define project name to get relative share path 34 | target_compile_definitions(${PROJECT_NAME} PRIVATE "PACKAGE_NAME=\"${PROJECT_NAME}\"") 35 | target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 36 | 37 | ament_target_dependencies( 38 | ${PROJECT_NAME} 39 | pluginlib 40 | params 41 | rclcpp 42 | cyberdog_common 43 | ament_index_cpp 44 | protocol 45 | filters 46 | ) 47 | 48 | pluginlib_export_plugin_description_file(${PROJECT_NAME} plugin.xml) 49 | 50 | install( 51 | DIRECTORY include/ 52 | DESTINATION include/ 53 | ) 54 | 55 | install( 56 | TARGETS ${PROJECT_NAME} 57 | EXPORT export_${PROJECT_NAME} 58 | ARCHIVE DESTINATION lib 59 | LIBRARY DESTINATION lib 60 | RUNTIME DESTINATION bin 61 | ) 62 | 63 | if(BUILD_TESTING) 64 | find_package(ament_lint_auto REQUIRED) 65 | ament_lint_auto_find_test_dependencies() 66 | find_package(ament_cmake_gtest REQUIRED) 67 | endif() 68 | 69 | ament_export_include_directories(include) 70 | ament_export_libraries(${PROJECT_NAME}) 71 | ament_export_targets(export_${PROJECT_NAME}) 72 | ament_package() 73 | -------------------------------------------------------------------------------- /cyberdog_lidar/README.md: -------------------------------------------------------------------------------- 1 | #
cyberdog_lidar 设计文档
2 | ``` 3 | Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 4 | 5 | Licensed under the Apache License, Version 2.0 (the 'License'); 6 | you may not use this file except in compliance with the License. 7 | You may obtain a copy of the License at 8 | 9 | http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | Unless required by applicable law or agreed to in writing, software 12 | distributed under the License is distributed on an 'AS IS' BASIS, 13 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | See the License for the specific language governing permissions and 15 | limitations under the License. 16 | ``` 17 | ## 目录 18 | * [1. 修订](#1-修订) 19 | * [2. 概述](#2-概述) 20 | * [3. 设计](#3-设计) 21 | * [3.1. 功能设计](#31-功能设计) 22 | * [3.2. 模块设计](#32-模块设计) 23 | --- 24 | ## 1. 修订 25 | 26 |
27 | 28 | 项目|软件版本|协议版本|修订日期|修订人员|备注 29 | :--:|:--|:--|:--:|:--:|:--: 30 | 雷达驱动|V1.1.0.0|V1.0.0.0|2023-02-06|尚子涵|无 31 | 32 |
33 | 34 | ## 2. 概述 35 | 36 |
37 | 38 | ![](./doc/image/cyberdog_lidar_scan.png) 39 | 40 |
41 | 42 | 如上图所示，仿生机器人雷达驱动主要用于：需要实时反馈仿生机器人所处环境中的障碍物距离及距离探测值强度信息的场景。 43 | 44 | ## 3. 设计 45 | ### 3.1. 功能设计 46 | 47 |
48 | 49 | ![](./doc/image/cyberdog_lidar_function.png) 50 | 51 |
52 | 53 | 如上图所示， 仿生机器人雷达驱动工作流程主要如下： 54 | 1. 解析雷达配置参数并按照参数配置雷达软硬件； 55 | 2. 初始化雷达； 56 | 3. 如果初始化成功则继续下一步反之退出； 57 | 4. 采集雷达原始数据； 58 | 5. 如果需要滤波则对雷达原始数据进行过滤（拖尾滤波）并发布，反之直接发布原始数据； 59 | 6. 如果程序收到终止信号则退出，反之进行第4步。 60 | 61 | #### 3.2 模块设计 62 | 63 |
64 | 65 | ![](./doc/image/cyberdog_lidar_module.png) 66 | 67 |
68 | 69 | 如上图所示， 仿生机器人雷达驱动架构组成及各组成部分主要功能如下： 70 | 1. Lidar SDK：提供 Linux 下雷达固件数据采集及解析功能； 71 | 2. Utils：提供常用工具，如日志； 72 | 3. 雷达软硬件配置功能模块：提供雷达硬件及软件的配置功能； 73 | 4. 雷达数据采集功能模块：提供雷达数据采集及解析功能； 74 | 5. 雷达数据过滤功能模块：提供雷达数据过滤（拖尾滤波）功能； 75 | 6. 发布雷达数据：提供雷达数据发布功能。 76 | 77 | --- 78 | --- 79 | 80 | #
cyberdog_interactive design document
81 | ``` 82 | Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 83 | 84 | Licensed under the Apache License, Version 2.0 (the 'License'); 85 | you may not use this file except in compliance with the License. 86 | You may obtain a copy of the License at 87 | 88 | http://www.apache.org/licenses/LICENSE-2.0 89 | 90 | Unless required by applicable law or agreed to in writing, software 91 | distributed under the License is distributed on an 'AS IS' BASIS, 92 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 93 | See the License for the specific language governing permissions and 94 | limitations under the License. 95 | ``` 96 | ## Directory 97 | * [1. Revise](#1-revise) 98 | * [2. Overview](#2-overview) 99 | * [3. Design](#3-design) 100 | * [3.1. Feature design](#31-feature-design) 101 | * [3.2. modular design](#32-modular-design) 102 | --- 103 | ## 1. Revise 104 | 105 |
106 | 107 | Item|Software Version|Protocol Version|Revision Date|Reviser|Remarks 108 | :--:|:--|:--|:--:|:--:|:--: 109 | cyberdog_lidar|V1.1.0.0|V1.0.0.0|2023-02-06|ShangZihan|none 110 | 111 |
112 | 113 | ## 2. Overview 114 | 115 |
116 | 117 | ![](./doc/image/cyberdog_lidar_scan.png) 118 | 119 |
120 | 121 | As shown in the figure above, the bionic robot radar driver is mainly used in scenarios where real-time feedback of obstacle distance and distance detection value intensity information in the bionic robot's environment is required. 122 | 123 | ## 3. Design 124 | ### 3.1. Feature design 125 | 126 |
127 | 128 | ![](./doc/image/cyberdog_lidar_function.png) 129 | 130 |
131 | 132 | As shown in the figure above, the workflow of the bionic robot radar driver is mainly as follows: 133 | 1. Analyze the radar configuration parameters and configure the radar software and hardware according to the parameters; 134 | 2. Initialize the radar; 135 | 3. If the initialization is successful, continue to the next step, otherwise exit; 136 | 4. Collect raw radar data; 137 | 5. If filtering is required, filter the raw radar data (smearing filter) and release it, otherwise release the original data directly; 138 | 6. If the program receives the termination signal, exit, otherwise, go to step 4. 139 | 140 | ### 3.2 Technology architecture 141 | 142 |
143 | 144 | ![](./doc/image/cyberdog_lidar_module.png) 145 | 146 |
147 | 148 | As shown in the figure above, the composition of the bionic robot radar drive architecture and the main functions of each component are as follows: 149 | 1. Lidar SDK: Provides radar firmware data collection and analysis functions under Linux; 150 | 2. Utils: Provide common tools, such as logs; 151 | 3. Radar software and hardware configuration function module: provide radar hardware and software configuration functions; 152 | 4. Radar data acquisition function module: provide radar data acquisition and analysis functions; 153 | 5. Radar data filtering function module: provide radar data filtering (smearing filter) function; 154 | 6. Release radar data: Provide radar data release function. -------------------------------------------------------------------------------- /cyberdog_lidar/doc/image/cyberdog_lidar_function.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/sensors/cce5fd141e0f5050e8ff51ac38f1796569bc233c/cyberdog_lidar/doc/image/cyberdog_lidar_function.png -------------------------------------------------------------------------------- /cyberdog_lidar/doc/image/cyberdog_lidar_function.svg: -------------------------------------------------------------------------------- 1 |
解析雷达配置参数
初始化
是否成功
yes
no
采集数据
yes
no
是否
需要滤波
滤波
发布数据
yes
no
是否收到
SIGINT信号？
依据参数配置雷达硬件及软件
初始化雷达
 -------------------------------------------------------------------------------- /cyberdog_lidar/doc/image/cyberdog_lidar_module.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/sensors/cce5fd141e0f5050e8ff51ac38f1796569bc233c/cyberdog_lidar/doc/image/cyberdog_lidar_module.png -------------------------------------------------------------------------------- /cyberdog_lidar/doc/image/cyberdog_lidar_module.svg: -------------------------------------------------------------------------------- 1 |
雷达驱动模块梳理
模块实现
发布雷达数据
内部调用
雷达数据过滤功能模块雷达软硬件配置功能模块雷达数据采集功能模块
依赖工具
Lidar SDKUtils -------------------------------------------------------------------------------- /cyberdog_lidar/doc/image/cyberdog_lidar_scan.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/sensors/cce5fd141e0f5050e8ff51ac38f1796569bc233c/cyberdog_lidar/doc/image/cyberdog_lidar_scan.png -------------------------------------------------------------------------------- /cyberdog_lidar/include/lidar_base/lidar_base.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #ifndef LIDAR_BASE__LIDAR_BASE_HPP_ 16 | #define LIDAR_BASE__LIDAR_BASE_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace cyberdog 24 | { 25 | namespace sensor 26 | { 27 | /*! \file lidar_base.hpp 28 | \brief 雷达基础模块。 29 | \details 创建及初始化雷达基础模块。 30 | \author 尚子涵 31 | \author Shang Zihan 32 | \version 1.1.0.0 33 | \date 2023-02-06 34 | \pre 初始化设备。 35 | \copyright [2023]-[2023] [Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.] 36 | */ 37 | class LidarBase 38 | { 39 | public: 40 | virtual int32_t Init(bool simulator = false) = 0; 41 | std::function Open, Start, Stop, Close; 42 | virtual ~LidarBase() {} 43 | void SetPayloadCallback( 44 | std::function payload)> cb) 46 | { 47 | payload_callback_ = cb; 48 | } 49 | 50 | public: 51 | virtual int32_t Open_() = 0; 52 | virtual int32_t Start_() = 0; 53 | virtual int32_t Stop_() = 0; 54 | virtual int32_t Close_() = 0; 55 | virtual int32_t SelfCheck() = 0; 56 | virtual int32_t LowPowerOn() = 0; 57 | virtual int32_t LowPowerOff() = 0; 58 | std::function payload)> payload_callback_; 59 | LidarBase() {} 60 | }; // class LidarBase 61 | } // namespace sensor 62 | } // namespace cyberdog 63 | 64 | #endif // LIDAR_BASE__LIDAR_BASE_HPP_ 65 | -------------------------------------------------------------------------------- /cyberdog_lidar/include/lidar_plugin/ydlidar_plugin.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #ifndef LIDAR_PLUGIN__YDLIDAR_PLUGIN_HPP_ 16 | #define LIDAR_PLUGIN__YDLIDAR_PLUGIN_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "lidar_base/lidar_base.hpp" 31 | #include "cyberdog_system/robot_code.hpp" 32 | 33 | namespace cyberdog 34 | { 35 | namespace sensor 36 | { 37 | namespace SYS = cyberdog::system; 38 | 39 | /*! \file ydlidar_plugin.hpp 40 | \brief 雷达插件模块。 41 | \details 创建及初始化雷达插件模块。 42 | \author 尚子涵 43 | \author Shang Zihan 44 | \version 1.1.0.0 45 | \date 2023-02-06 46 | \pre 初始化设备。 47 | \copyright [2023]-[2023] [Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.] 48 | */ 49 | class YdlidarCarpo : public cyberdog::sensor::LidarBase 50 | { 51 | using SwitchState = enum {open = 0, start, stop, close, }; /*!< [类型]切换状态 */ 52 | using ScanMsg = sensor_msgs::msg::LaserScan; /*!< [topic 类型]激光数据 */ 53 | using Filter = filters::FilterChain; /*!< 过滤激光数据 */ 54 | 55 | public: 56 | int32_t Init(bool simulator = false) override; 57 | enum class YdlidarCode : int32_t 58 | { 59 | kDemoError1 = 21 60 | }; 61 | 62 | private: 63 | float frequency_; /*!< 频率 */ 64 | LaserScan scan_sdk; /*!< 激光数据 */ 65 | std::map state_msg_; /*!< 状态消息 */ 66 | std::atomic sensor_state_ {SwitchState::close}; /*!< node 状态 */ 67 | std::shared_ptr update_data_thread_ptr_ {nullptr}; /*!< 更新数据线程 */ 68 | std::shared_ptr lidar_ptr_ {nullptr}; /*!< SDK雷达对象 */ 69 | ScanMsg raw_scan_; /*!< 原始激光数据 */ 70 | ScanMsg filter_scan_; /*!< 过滤激光数据 */ 71 | bool filter_ {false}; /*!< 是否过滤激光数据 */ 72 | std::shared_ptr filter_ptr_ {nullptr}; /*!< 激光过滤器 */ 73 | std::shared_ptr> code_{nullptr}; 74 | 75 | private: 76 | int32_t Open_() override; 77 | int32_t Start_() override; 78 | int32_t Stop_() override; 79 | int32_t Close_() override; 80 | int32_t SelfCheck() override; 81 | int32_t LowPowerOn() override; 82 | int32_t LowPowerOff() override; 83 | void UpdateData(); /*!< 更新数据 */ 84 | void UpdateSimulationData(); /*!< 更新模拟数据 */ 85 | 86 | LOGGER_MINOR_INSTANCE("Ydlidar"); 87 | }; // class LidarCarpo 88 | } // namespace sensor 89 | } // namespace cyberdog 90 | 91 | #endif // LIDAR_PLUGIN__YDLIDAR_PLUGIN_HPP_ 92 | -------------------------------------------------------------------------------- /cyberdog_lidar/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_lidar 5 | 1.0.0 6 | lidar pluginlib plugins 7 | ShangZihan 8 | Apache License, Version 2.0 9 | 10 | ament_cmake_ros 11 | 12 | pluginlib 13 | rclcpp 14 | params 15 | cyberdog_common 16 | cyberdog_system 17 | ament_index_cpp 18 | protocol 19 | filters 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | -------------------------------------------------------------------------------- /cyberdog_lidar/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is a ydlidar plugin. 4 | 5 | -------------------------------------------------------------------------------- /cyberdog_lidar/src/ydlidar_plugin.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "lidar_plugin/ydlidar_plugin.hpp" 21 | 22 | int32_t cyberdog::sensor::YdlidarCarpo::Init(bool simulator) 23 | { 24 | this->state_msg_.insert({SwitchState::open, "Open"}); 25 | this->state_msg_.insert({SwitchState::start, "Start"}); 26 | this->state_msg_.insert({SwitchState::stop, "Stop"}); 27 | this->state_msg_.insert({SwitchState::close, "Close"}); 28 | const SYS::ModuleCode kModuleCode = SYS::ModuleCode::kLidar; 29 | code_ = std::make_shared>(kModuleCode); 30 | 31 | if (!simulator) { 32 | this->Open = std::bind(&cyberdog::sensor::YdlidarCarpo::Open_, this); 33 | this->Start = std::bind(&cyberdog::sensor::YdlidarCarpo::Start_, this); 34 | this->Stop = std::bind(&cyberdog::sensor::YdlidarCarpo::Stop_, this); 35 | this->Close = std::bind(&cyberdog::sensor::YdlidarCarpo::Close_, this); 36 | } else { 37 | auto Simulator = [this](SwitchState now_state) -> bool { 38 | INFO("%s ydlidar ...", this->state_msg_[now_state].c_str()); 39 | switch (now_state) { 40 | case SwitchState::open: 41 | case SwitchState::stop: 42 | case SwitchState::close: 43 | break; 44 | case SwitchState::start: 45 | if (this->update_data_thread_ptr_ == nullptr) { 46 | this->update_data_thread_ptr_ = std::make_shared( 47 | std::bind(&cyberdog::sensor::YdlidarCarpo::UpdateSimulationData, this)); 48 | this->update_data_thread_ptr_->detach(); 49 | } 50 | break; 51 | default: 52 | WARN("Ydlidar not recognized state"); 53 | break; 54 | } 55 | 56 | this->sensor_state_ = now_state; 57 | INFO("ydlidar %s ok", this->state_msg_[now_state].c_str()); 58 | return code_->GetKeyCode(SYS::KeyCode::kOK); 59 | }; 60 | this->Open = std::bind(Simulator, SwitchState::open); 61 | this->Start = std::bind(Simulator, SwitchState::start); 62 | this->Stop = std::bind(Simulator, SwitchState::stop); 63 | this->Close = std::bind(Simulator, SwitchState::close); 64 | } 65 | return code_->GetKeyCode(SYS::KeyCode::kOK); 66 | } 67 | 68 | int32_t cyberdog::sensor::YdlidarCarpo::Open_() 69 | { 70 | INFO("Open ydlidar ..."); 71 | std::string lidar_config_dir = ament_index_cpp::get_package_share_directory("params") + 72 | "/toml_config/sensors/lidar.toml"; 73 | INFO("Params config file dir:%s", lidar_config_dir.c_str()); 74 | 75 | if (access(lidar_config_dir.c_str(), F_OK)) { 76 | ERROR("Params config file does not exist"); 77 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 78 | } 79 | 80 | if (access(lidar_config_dir.c_str(), R_OK)) { 81 | ERROR("Params config file does not have read permissions"); 82 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 83 | } 84 | 85 | // if (access(lidar_config_dir.c_str(), W_OK)) { 86 | // ERROR("Params config file does not have write permissions"); 87 | // return code_->GetKeyCode(SYS::KeyCode::kFailed); 88 | // } 89 | 90 | toml::value params_toml; 91 | if (!cyberdog::common::CyberdogToml::ParseFile( 92 | lidar_config_dir.c_str(), params_toml)) 93 | { 94 | ERROR("Params config file is not in toml format"); 95 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 96 | } 97 | 98 | this->filter_ = toml::find_or( 99 | params_toml, "dylidar", "filter", false); 100 | this->filter_ptr_ = std::make_shared("sensor_msgs::msg::LaserScan"); 101 | this->raw_scan_.header.frame_id = toml::find_or( 102 | params_toml, "dylidar", "frame_id", "laser_frame"); 103 | 104 | INFO("this->raw_scan_.header.frame_id = %s", this->raw_scan_.header.frame_id.c_str()); 105 | 106 | if (this->lidar_ptr_ == nullptr) { 107 | this->lidar_ptr_ = std::make_shared(); 108 | } 109 | 110 | std::string str_optvalue; 111 | str_optvalue = toml::find_or(params_toml, "dylidar", "port", "/dev/ydlidar"); 112 | this->lidar_ptr_->setlidaropt(LidarPropSerialPort, str_optvalue.c_str(), str_optvalue.size()); 113 | INFO("[Open] dylidar->port = %s", str_optvalue.c_str()); 114 | 115 | str_optvalue = toml::find_or(params_toml, "dylidar", "ignore_array", ""); 116 | this->lidar_ptr_->setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(), str_optvalue.size()); 117 | INFO("[Open] dylidar->ignore_array = %s", str_optvalue.c_str()); 118 | 119 | int int_optvalue; 120 | int_optvalue = toml::find_or(params_toml, "dylidar", "baudrate", 512000); 121 | this->lidar_ptr_->setlidaropt(LidarPropSerialBaudrate, &int_optvalue, sizeof(int)); 122 | INFO("[Open] dylidar->baudrate = %d", int_optvalue); 123 | 124 | int_optvalue = 125 | toml::find_or( 126 | params_toml, "dylidar", "lidar_type", static_cast(LidarTypeID::TYPE_TOF)); 127 | this->lidar_ptr_->setlidaropt(LidarPropLidarType, &int_optvalue, sizeof(int)); 128 | INFO("[Open] dylidar->lidar_type = %d", int_optvalue); 129 | 130 | int_optvalue = 131 | toml::find_or( 132 | params_toml, "dylidar", "device_type", 133 | static_cast(DeviceTypeID::YDLIDAR_TYPE_SERIAL)); 134 | this->lidar_ptr_->setlidaropt(LidarPropDeviceType, &int_optvalue, sizeof(int)); 135 | INFO("[Open] dylidar->device_type = %d", int_optvalue); 136 | 137 | int_optvalue = toml::find_or(params_toml, "dylidar", "sample_rate", 9); 138 | this->lidar_ptr_->setlidaropt(LidarPropSampleRate, &int_optvalue, sizeof(int)); 139 | INFO("[Open] dylidar->sample_rate = %d", int_optvalue); 140 | 141 | int_optvalue = toml::find_or(params_toml, "dylidar", "abnormal_check_count", 4); 142 | this->lidar_ptr_->setlidaropt(LidarPropAbnormalCheckCount, &int_optvalue, sizeof(int)); 143 | INFO("[Open] dylidar->sample_rate = %d", int_optvalue); 144 | 145 | bool bool_optvalue; 146 | bool_optvalue = toml::find_or(params_toml, "dylidar", "resolution_fixed", false); 147 | this->lidar_ptr_->setlidaropt(LidarPropFixedResolution, &bool_optvalue, sizeof(bool)); 148 | INFO("[Open] dylidar->resolution_fixed = %s", bool_optvalue ? "True" : "False"); 149 | 150 | bool_optvalue = toml::find_or(params_toml, "dylidar", "reversion", true); 151 | this->lidar_ptr_->setlidaropt(LidarPropReversion, &bool_optvalue, sizeof(bool)); 152 | INFO("[Open] dylidar->reversion = %s", bool_optvalue ? "True" : "False"); 153 | 154 | bool_optvalue = toml::find_or(params_toml, "dylidar", "inverted", true); 155 | this->lidar_ptr_->setlidaropt(LidarPropInverted, &bool_optvalue, sizeof(bool)); 156 | INFO("[Open] dylidar->inverted = %s", bool_optvalue ? "True" : "False"); 157 | 158 | bool_optvalue = toml::find_or(params_toml, "dylidar", "auto_reconnect", true); 159 | this->lidar_ptr_->setlidaropt(LidarPropAutoReconnect, &bool_optvalue, sizeof(bool)); 160 | INFO("[Open] dylidar->auto_reconnect = %s", bool_optvalue ? "True" : "False"); 161 | 162 | bool_optvalue = toml::find_or(params_toml, "dylidar", "isSingleChannel", false); 163 | this->lidar_ptr_->setlidaropt(LidarPropSingleChannel, &bool_optvalue, sizeof(bool)); 164 | INFO("[Open] dylidar->isSingleChannel = %s", bool_optvalue ? "True" : "False"); 165 | 166 | bool_optvalue = toml::find_or(params_toml, "dylidar", "intensity", false); 167 | this->lidar_ptr_->setlidaropt(LidarPropIntenstiy, &bool_optvalue, sizeof(bool)); 168 | INFO("[Open] dylidar->intensity = %s", bool_optvalue ? "True" : "False"); 169 | 170 | bool_optvalue = toml::find_or(params_toml, "dylidar", "support_motor_dtr", false); 171 | this->lidar_ptr_->setlidaropt(LidarPropSupportMotorDtrCtrl, &bool_optvalue, sizeof(bool)); 172 | INFO("[Open] dylidar->support_motor_dtr = %s", bool_optvalue ? "True" : "False"); 173 | 174 | float float_optvalue; 175 | float_optvalue = 176 | toml::find_or(params_toml, "dylidar", "angle_max", static_cast(180.0f)); 177 | this->lidar_ptr_->setlidaropt(LidarPropMaxAngle, &float_optvalue, sizeof(float)); 178 | INFO("[Open] dylidar->angle_max = %f", float_optvalue); 179 | 180 | float_optvalue = 181 | toml::find_or(params_toml, "dylidar", "angle_min", static_cast(-180.0f)); 182 | this->lidar_ptr_->setlidaropt(LidarPropMinAngle, &float_optvalue, sizeof(float)); 183 | INFO("[Open] dylidar->angle_min = %f", float_optvalue); 184 | 185 | float_optvalue = 186 | toml::find_or(params_toml, "dylidar", "range_max", static_cast(64.f)); 187 | this->lidar_ptr_->setlidaropt(LidarPropMaxRange, &float_optvalue, sizeof(float)); 188 | INFO("[Open] dylidar->range_max = %f", float_optvalue); 189 | 190 | float_optvalue = 191 | toml::find_or(params_toml, "dylidar", "range_min", static_cast(0.1f)); 192 | this->lidar_ptr_->setlidaropt(LidarPropMinRange, &float_optvalue, sizeof(float)); 193 | INFO("[Open] dylidar->range_min = %f", float_optvalue); 194 | 195 | this->frequency_ = 196 | toml::find_or(params_toml, "dylidar", "frequency", static_cast(10.f)); 197 | this->lidar_ptr_->setlidaropt(LidarPropScanFrequency, &this->frequency_, sizeof(float)); 198 | INFO("[Open] dylidar->frequency = %f", this->frequency_); 199 | 200 | this->sensor_state_ = SwitchState::open; 201 | INFO("Ydlidar %s ok", this->state_msg_[this->sensor_state_].c_str()); 202 | return code_->GetKeyCode(SYS::KeyCode::kOK); 203 | } 204 | 205 | int32_t cyberdog::sensor::YdlidarCarpo::Start_() 206 | { 207 | INFO("Start ydlidar ..."); 208 | if (this->lidar_ptr_ == nullptr) { 209 | ERROR("Start ydlidar failed (Now Ydlidar is not yet opened)."); 210 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 211 | } 212 | if (this->sensor_state_ == SwitchState::start) { 213 | INFO("Start ydlidar ok (consistent with the current state)"); 214 | return code_->GetKeyCode(SYS::KeyCode::kOK); 215 | } 216 | if (!this->lidar_ptr_->initialize()) { 217 | ERROR("Start ydlidar failed (%s).", this->lidar_ptr_->DescribeError()); 218 | this->lidar_ptr_->disconnecting(); 219 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 220 | } 221 | if (!this->lidar_ptr_->turnOn()) { 222 | ERROR("Start ydlidar (turnOn) failed (%s)", this->lidar_ptr_->DescribeError()); 223 | this->lidar_ptr_->disconnecting(); 224 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 225 | } 226 | 227 | if (this->update_data_thread_ptr_ == nullptr) { 228 | this->update_data_thread_ptr_ = std::make_shared( 229 | std::bind(&cyberdog::sensor::YdlidarCarpo::UpdateData, this)); 230 | this->update_data_thread_ptr_->detach(); 231 | } 232 | 233 | this->sensor_state_ = SwitchState::start; 234 | INFO("Ydlidar %s ok", this->state_msg_[this->sensor_state_].c_str()); 235 | return code_->GetKeyCode(SYS::KeyCode::kOK); 236 | } 237 | 238 | int32_t cyberdog::sensor::YdlidarCarpo::Stop_() 239 | { 240 | INFO("Stop ydlidar ..."); 241 | if (this->lidar_ptr_ == nullptr) { 242 | ERROR("Stop ydlidar failed (Now Ydlidar is not yet opened)."); 243 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 244 | } 245 | if (this->sensor_state_ == SwitchState::stop) { 246 | INFO("Stop ydlidar ok (consistent with the current state)"); 247 | return code_->GetKeyCode(SYS::KeyCode::kOK); 248 | } 249 | if (!this->lidar_ptr_->turnOff()) { 250 | ERROR("Stop ydlidar (turnOff) failed (%s)", this->lidar_ptr_->DescribeError()); 251 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 252 | } 253 | this->lidar_ptr_->disconnecting(); 254 | this->sensor_state_ = SwitchState::stop; 255 | INFO("Ydlidar %s ok", this->state_msg_[this->sensor_state_].c_str()); 256 | return code_->GetKeyCode(SYS::KeyCode::kOK); 257 | } 258 | 259 | int32_t cyberdog::sensor::YdlidarCarpo::Close_() 260 | { 261 | INFO("Close ydlidar ..."); 262 | if (this->sensor_state_ == SwitchState::close) { 263 | INFO("Close ydlidar ok (consistent with the current state)"); 264 | return code_->GetKeyCode(SYS::KeyCode::kOK); 265 | } 266 | if (this->lidar_ptr_ != nullptr) { 267 | this->Stop_(); 268 | } 269 | this->lidar_ptr_ = nullptr; 270 | this->sensor_state_ = SwitchState::close; 271 | INFO("Ydlidar %s ok", this->state_msg_[this->sensor_state_].c_str()); 272 | return code_->GetKeyCode(SYS::KeyCode::kOK); 273 | } 274 | 275 | void cyberdog::sensor::YdlidarCarpo::UpdateData() 276 | { 277 | INFO("UpdateData ydlidar ..."); 278 | int sleep_time = static_cast(1000 / this->frequency_); 279 | bool warn_is_print = false; 280 | while (true) { 281 | if (!rclcpp::ok()) { 282 | ERROR("Ydlidar update data failed (!rclcpp::ok())"); 283 | break; 284 | } 285 | std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time)); 286 | if (this->sensor_state_ != SwitchState::start) { 287 | if (!warn_is_print) { 288 | warn_is_print = true; 289 | WARN("Ydlidar update data continue (!start)"); 290 | } 291 | continue; 292 | } 293 | if (this->lidar_ptr_ == nullptr) { 294 | if (!warn_is_print) { 295 | warn_is_print = true; 296 | WARN("Ydlidar update data continue (lidar_ptr == nullptr)"); 297 | } 298 | continue; 299 | } 300 | if (warn_is_print) { 301 | warn_is_print = false; 302 | INFO("Ydlidar update data ..."); 303 | } 304 | if (this->lidar_ptr_->doProcessSimple(this->scan_sdk)) { 305 | this->raw_scan_.header.stamp.sec = RCL_NS_TO_S(this->scan_sdk.stamp); 306 | this->raw_scan_.header.stamp.nanosec = this->scan_sdk.stamp - 307 | RCL_S_TO_NS(this->raw_scan_.header.stamp.sec); 308 | this->raw_scan_.angle_min = this->scan_sdk.config.min_angle; 309 | this->raw_scan_.angle_max = this->scan_sdk.config.max_angle; 310 | this->raw_scan_.angle_increment = this->scan_sdk.config.angle_increment; 311 | this->raw_scan_.scan_time = this->scan_sdk.config.scan_time; 312 | this->raw_scan_.time_increment = this->scan_sdk.config.time_increment; 313 | this->raw_scan_.range_min = this->scan_sdk.config.min_range; 314 | this->raw_scan_.range_max = this->scan_sdk.config.max_range; 315 | int size = (this->scan_sdk.config.max_angle - this->scan_sdk.config.min_angle) / 316 | this->scan_sdk.config.angle_increment + 1; 317 | this->raw_scan_.ranges.resize(size); 318 | this->raw_scan_.intensities.resize(size); 319 | std::vector scan_updata; 320 | scan_updata.resize(size); 321 | for (size_t i = 0; i < this->scan_sdk.points.size(); i++) { 322 | int index = std::ceil( 323 | (this->scan_sdk.points[i].angle - this->scan_sdk.config.min_angle) / 324 | this->scan_sdk.config.angle_increment); 325 | if ((index >= 0) && (index < size) && (!scan_updata[index])) { 326 | scan_updata[index] = true; 327 | this->raw_scan_.ranges[index] = this->scan_sdk.points[i].range; 328 | this->raw_scan_.intensities[index] = this->scan_sdk.points[i].intensity; 329 | } 330 | } 331 | if (this->filter_ && this->filter_ptr_->update(this->raw_scan_, this->filter_scan_)) { 332 | this->payload_callback_(std::make_shared(this->filter_scan_)); 333 | } else { 334 | this->payload_callback_(std::make_shared(this->raw_scan_)); 335 | } 336 | } 337 | } 338 | INFO("Ydlidar update data ok"); 339 | } 340 | 341 | void cyberdog::sensor::YdlidarCarpo::UpdateSimulationData() 342 | { 343 | int sleep_time = static_cast(1000 / this->frequency_); 344 | std::shared_ptr sim_scan_ptr = std::make_shared(this->raw_scan_); 345 | while (true) { 346 | if (!rclcpp::ok()) { 347 | WARN("[cyberdog_lidar]: !rclcpp::ok()"); 348 | break; 349 | } 350 | std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time)); 351 | if ((this->sensor_state_ != SwitchState::start) || (this->lidar_ptr_ == nullptr)) { 352 | continue; 353 | } 354 | this->raw_scan_.header.stamp = rclcpp::Clock(RCL_SYSTEM_TIME).now(); 355 | this->payload_callback_(sim_scan_ptr); 356 | } 357 | } 358 | 359 | int32_t cyberdog::sensor::YdlidarCarpo::SelfCheck() 360 | { 361 | INFO("SelfCheck ydlidar ..."); 362 | // auto self_check = [&]() -> int32_t { 363 | // switch (this->sensor_state_) 364 | // { 365 | // case SwitchState::open: 366 | // return this->Start() + this->Stop() + this->Close() + 367 | // this->Open() + this->Start() + this->Stop() + this->Start() + this->Stop() + 368 | // this->Close() + this->Open(); 369 | // case SwitchState::start: 370 | // return this->Stop() + this->Close() + 371 | // this->Open() + this->Start() + this->Stop() + this->Start() + this->Stop() + 372 | // this->Close() + this->Open() + this->Start(); 373 | 374 | // case SwitchState::stop: 375 | // return this->Close() + 376 | // this->Open() + this->Start() + this->Stop() + this->Start() + this->Stop() + 377 | // this->Close() + this->Open() + this->Start() + this->Stop(); 378 | 379 | // case SwitchState::close: 380 | // return this->Open() + this->Start() + this->Stop() + this->Start() + this->Stop() + 381 | // this->Close() + this->Open() + this->Start() + this->Stop() + this->Close(); 382 | 383 | // default: 384 | // return code_->GetKeyCode(SYS::KeyCode::kOK); 385 | // break; 386 | // } 387 | // }; 388 | auto fast_self_check = [&]() -> int32_t { 389 | switch (this->sensor_state_) { 390 | case SwitchState::open: 391 | return this->Start() + this->Stop() + this->Close() + this->Open(); 392 | case SwitchState::start: 393 | return this->Stop() + this->Start() + this->Stop() + this->Start(); 394 | case SwitchState::stop: 395 | return this->Close() + this->Open() + this->Start() + this->Stop(); 396 | case SwitchState::close: 397 | return this->Open() + this->Start() + this->Stop() + this->Close(); 398 | default: 399 | return code_->GetKeyCode(SYS::KeyCode::kOK); 400 | break; 401 | } 402 | }; 403 | if (fast_self_check()) { 404 | ERROR("SelfCheck ydlidar failed."); 405 | return code_->GetKeyCode(SYS::KeyCode::kSelfCheckFailed); 406 | } 407 | INFO("Ydlidar self check ok"); 408 | return code_->GetKeyCode(SYS::KeyCode::kOK); 409 | } 410 | 411 | int32_t cyberdog::sensor::YdlidarCarpo::LowPowerOn() 412 | { 413 | INFO("LowPowerOn ydlidar ..."); 414 | if (this->Stop()) { 415 | ERROR("LowPowerOn ydlidar failed."); 416 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 417 | } 418 | INFO("Ydlidar low power on ok"); 419 | return code_->GetKeyCode(SYS::KeyCode::kOK); 420 | } 421 | 422 | int32_t cyberdog::sensor::YdlidarCarpo::LowPowerOff() 423 | { 424 | INFO("LowPowerOff ydlidar ..."); 425 | if (this->Start()) { 426 | ERROR("LowPowerOff ydlidar failed."); 427 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 428 | } 429 | INFO("Ydlidar low power off ok"); 430 | return code_->GetKeyCode(SYS::KeyCode::kOK); 431 | } 432 | 433 | #include "pluginlib/class_list_macros.hpp" 434 | PLUGINLIB_EXPORT_CLASS(cyberdog::sensor::YdlidarCarpo, cyberdog::sensor::LidarBase) 435 | -------------------------------------------------------------------------------- /cyberdog_tof/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_tof) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_ros REQUIRED) 11 | find_package(pluginlib REQUIRED) 12 | find_package(params REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(cyberdog_common REQUIRED) 15 | find_package(protocol REQUIRED) 16 | find_package(ament_index_cpp REQUIRED) 17 | find_package(cyberdog_embed_protocol REQUIRED) 18 | find_package(cyberdog_system REQUIRED) 19 | 20 | 21 | 22 | add_library(${PROJECT_NAME} SHARED 23 | src/tof_plugin.cpp 24 | ) 25 | target_include_directories(${PROJECT_NAME} PUBLIC 26 | $ 27 | $) 28 | 29 | target_link_libraries(${PROJECT_NAME} 30 | ${cyberdog_log_LIBRARIES} 31 | ) 32 | # define project name to get relative share path 33 | target_compile_definitions(${PROJECT_NAME} PRIVATE "PACKAGE_NAME=\"${PROJECT_NAME}\"") 34 | target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 35 | 36 | ament_target_dependencies( 37 | ${PROJECT_NAME} 38 | pluginlib 39 | params 40 | rclcpp 41 | cyberdog_common 42 | ament_index_cpp 43 | protocol 44 | cyberdog_embed_protocol 45 | ) 46 | 47 | pluginlib_export_plugin_description_file(${PROJECT_NAME} plugin.xml) 48 | 49 | install( 50 | DIRECTORY include/ 51 | DESTINATION include/ 52 | ) 53 | 54 | 55 | install( 56 | TARGETS ${PROJECT_NAME} 57 | EXPORT export_${PROJECT_NAME} 58 | ARCHIVE DESTINATION lib 59 | LIBRARY DESTINATION lib 60 | RUNTIME DESTINATION bin 61 | ) 62 | 63 | if(BUILD_TESTING) 64 | find_package(ament_lint_auto REQUIRED) 65 | ament_lint_auto_find_test_dependencies() 66 | find_package(ament_cmake_gtest REQUIRED) 67 | endif() 68 | 69 | ament_export_include_directories(include) 70 | ament_export_libraries(${PROJECT_NAME}) 71 | ament_export_targets(export_${PROJECT_NAME}) 72 | ament_package() 73 | -------------------------------------------------------------------------------- /cyberdog_tof/README.md: -------------------------------------------------------------------------------- 1 | # bcm_gps 2 | 3 | 该模块主要提供GPS的驱动封装 4 | 5 | - 外部参数通过`cyberdog_ception_bridge`模块中`params/bcmgps_config.toml`载入 6 | - GPS模块自身日志保存在`cyberdog_ception_bridge`模块中`logs`路径下 7 | 8 | ### 主要接口部分如下： 9 | 10 | ```cpp 11 | namespace bcm_gps 12 | { 13 | typedef LD2BRM_PvtPvtPolledPayload GPS_Payload; 14 | using NMEA_callback = std::function; 15 | using PAYLOAD_callback = std::function payload)>; 16 | 17 | class GPS 18 | { 19 | public: 20 | explicit GPS(PAYLOAD_callback PAYLOAD_cb = nullptr, NMEA_callback NMEA_cb = nullptr); 21 | ~GPS(); 22 | bool Open(); 23 | void Start(); 24 | void Stop(); 25 | void Close(); 26 | bool Ready(); 27 | void SetCallback(NMEA_callback NMEA_cb); 28 | void SetCallback(PAYLOAD_callback PAYLOAD_cb); 29 | 30 | void SetL5Bias(uint32_t biasCm); 31 | void SetLteFilterEn(bool enable = true); 32 | }; // class GPS 33 | } // namespace bcm_gps 34 | ``` 35 | 36 | #### public函数： 37 | 38 | 构造函数：初始化GPS对象 39 | 40 | ```cpp 41 | explicit GPS(PAYLOAD_callback PAYLOAD_cb = nullptr, NMEA_callback NMEA_cb = nullptr); 42 | ``` 43 | 44 | - `PAYLOAD_cb`：payload形式回调函数 45 | - `NMEA_cb`：NMEA标准格式字符串回调函数 46 | 47 | 48 | 49 | 开启设备：返回是否开启成功（构造函数会自动开启设备，主要用于手动调用Close()后重新开启） 50 | 51 | ```cpp 52 | bool Open(); 53 | ``` 54 | 55 | 56 | 57 | 打开定位 58 | 59 | ```cpp 60 | void Start(); 61 | ``` 62 | 63 | 64 | 65 | 关闭定位 66 | 67 | ```cpp 68 | void Stop(); 69 | ``` 70 | 71 | 72 | 73 | 关闭设备（关闭后会清空注册的回调函数，重新打开需要Open()后重新注册回调函数获取数据） 74 | 75 | ```cpp 76 | void Close(); 77 | ``` 78 | 79 | 80 | 81 | 获取硬件设备准备状态：返回准备是否成功 82 | 83 | ```cpp 84 | bool Ready(); 85 | ``` 86 | 87 | 88 | 89 | 注册回调函数 90 | 91 | ```cpp 92 | void SetCallback(NMEA_callback NMEA_cb); 93 | void SetCallback(PAYLOAD_callback PAYLOAD_cb); 94 | ``` 95 | 96 | - `PAYLOAD_cb`：payload形式回调函数 97 | - `NMEA_cb`：NMEA标准格式字符串回调函数 98 | 99 | 100 | 101 | Input L5 bias in centimeter 102 | 103 | ```cpp 104 | void SetL5Bias(uint32_t biasCm); 105 | ``` 106 | 107 | - biasCm：L5 bias in centimeter 108 | 109 | 110 | 111 | Enable/Disable LTE Filter 112 | 113 | ```cpp 114 | void SetLteFilterEn(bool enable = true); 115 | ``` 116 | 117 | - enable：Enable/Disable 118 | 119 | 120 | 121 | #### GPS_Payload消息结构 122 | 123 | ```cpp 124 | /** @brief BRM-NAV-PVT (0x01 0x07) 125 | * 126 | */ 127 | PACK( 128 | typedef struct 129 | { 130 | U4 iTOW; 131 | U2 year; 132 | U1 month; 133 | U1 day; 134 | U1 hour; 135 | U1 min; 136 | U1 sec; 137 | X1 valid; 138 | U4 tAcc; 139 | I4 nano; 140 | U1 fixType; 141 | X1 flags; 142 | X1 flags2; 143 | U1 numSV; 144 | I4 lon; 145 | I4 lat; 146 | I4 height; 147 | I4 hMSL; 148 | U4 hAcc; 149 | U4 vAcc; 150 | I4 velN; 151 | I4 velE; 152 | I4 velD; 153 | I4 gSpeed; 154 | I4 headMot; 155 | U4 sAcc; 156 | U4 headAcc; 157 | U2 pDOP; 158 | I1 leapS; 159 | U1 reserved1[5]; 160 | I4 headVeh; 161 | I2 magDec; 162 | U2 magAcc; 163 | }) LD2BRM_PvtPvtPolledPayload; 164 | ``` 165 | 166 | 167 | 168 | ### 外部参数 169 | 170 | 外部参数通过`cyberdog_ception_bridge`模块中`params/bcmgps_config.toml`载入 171 | 172 | ```toml 173 | skip_download = false # need in first line 174 | onlyfirst_download = true 175 | tty = "/dev/ttyTHS0" 176 | patch_path = "/usr/sbin/bream.patch" 177 | 178 | #uint8_t infMsgMask[6] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F}; 179 | #full logging (GLLIO, GLLAPI, RAWDATA, DEVKF, USR2, ) 180 | infMsgMask = [31, 31, 31, 31, 31, 31] 181 | 182 | #1(L1 Best) 2(L1 Auto) 3(L1 ULP), 4(L1L5 Best) 5(L1L5 Auto) 183 | PowerModePreset = 4 184 | 185 | MsgRate = [ 186 | 1, # Report GPGGA rate 187 | 1, # Report GPRMC rate 188 | 1, # Report GPGSV rate 189 | 1, # Report NAVEOE rate 190 | 1, # Report NAVPVT rate 191 | 1, # Report PGLOR SPEED rate 192 | 1, # Report PGLOR FIX rate 193 | 1, # Report PGLOR SAT rate 194 | 1, # Report PGLOR LSQ rate 195 | 1, # Report PGLOR PWR rate 196 | 1, # Report PGLOR STA rate 197 | 1, # Report NAVODO rate 198 | 1, # Report NAVSAT rate 199 | 1, # Report NAVDOP rate 200 | 1, # Report CBEE status rate 201 | 1, # Report ASC SUBFRAMES rate 202 | 1, # Report ASC MEAS rate 203 | 1 # Report ASC AGC rate 204 | ] 205 | 206 | AckAiding = true 207 | ``` 208 | 209 | - `skip_download`：是否跳过下载patch（请置于首行） 210 | - `onlyfirst_download`：仅编译后首次下载（该选项为`true`将会在下载后将`skip_download`置为`true`） 211 | - `tty`：设备号 212 | - `patch_path`：下载patch路径 213 | - `infMsgMask`：GPS模块日志配置 214 | - `PowerModePreset`：GPS频段选择 215 | - 1：(L1 Best) 216 | - 2：(L1 Auto) 217 | - 3：(L1 ULP) 218 | - 4：(L1L5 Best) (默认) 219 | - 5：(L1L5 Auto) 220 | - `MsgRate`：消息频率设置（0为关闭） 221 | - `AckAiding`：true/false 222 | 223 | 224 | 225 | ### Example 226 | 227 | ```cpp 228 | class Example 229 | { 230 | public: 231 | std::shared_ptr gps_; 232 | std::shared_ptr payload_; 233 | 234 | Example() 235 | { 236 | gps_ = std::make_shared(std::bind( 237 | &Example::payload_callback, this, std::placeholders::_1)); 238 | } 239 | 240 | void Func() 241 | { 242 | gps_->Start(); 243 | gps_->Stop(); 244 | gps_->Close(); 245 | } 246 | void payload_callback(std::shared_ptr payload) 247 | { 248 | payload_ = payload; 249 | } 250 | }; // class example 251 | ``` 252 | 253 | -------------------------------------------------------------------------------- /cyberdog_tof/include/tof_base/tof_base.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #ifndef TOF_BASE__TOF_BASE_HPP_ 16 | #define TOF_BASE__TOF_BASE_HPP_ 17 | 18 | #include 19 | #include 20 | #include "protocol/msg/single_tof_payload.hpp" 21 | 22 | namespace cyberdog 23 | { 24 | 25 | namespace sensor 26 | { 27 | class TofBase 28 | { 29 | public: 30 | virtual int32_t Init(bool simulator = false) = 0; 31 | std::function Open, Start, Stop, Close; 32 | virtual int32_t Open_() = 0; 33 | virtual int32_t Start_() = 0; 34 | virtual int32_t Stop_() = 0; 35 | virtual int32_t Close_() = 0; 36 | virtual int32_t SelfCheck() = 0; 37 | virtual int32_t LowPowerOn() = 0; 38 | virtual int32_t LowPowerOff() = 0; 39 | virtual ~TofBase() {} 40 | void SetSinglePayloadCallback( 41 | std::function payload)> cb) 43 | { 44 | single_payload_callback_ = cb; 45 | } 46 | 47 | protected: 48 | std::function payload)> 49 | single_payload_callback_; 50 | 51 | TofBase() {} 52 | }; // class TofBase 53 | } // namespace sensor 54 | } // namespace cyberdog 55 | 56 | #endif // TOF_BASE__TOF_BASE_HPP_ 57 | -------------------------------------------------------------------------------- /cyberdog_tof/include/tof_plugin/tof_plugin.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #ifndef TOF_PLUGIN__TOF_PLUGIN_HPP_ 16 | #define TOF_PLUGIN__TOF_PLUGIN_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "tof_base/tof_base.hpp" 24 | #include "embed_protocol/embed_protocol.hpp" 25 | #include "cyberdog_common/cyberdog_log.hpp" 26 | #include "cyberdog_common/cyberdog_semaphore.hpp" 27 | #include "cyberdog_system/robot_code.hpp" 28 | #include "cyberdog_common/cyberdog_toml.hpp" 29 | #include "rclcpp/rclcpp.hpp" 30 | 31 | namespace cyberdog 32 | { 33 | namespace sensor 34 | { 35 | namespace EP = cyberdog::embed; 36 | namespace SYS = cyberdog::system; 37 | 38 | class TofCarpo : public cyberdog::sensor::TofBase 39 | { 40 | using SwitchState = enum {open = 0, start, stop, close, }; // [类型]切换状态 41 | using Signal = cyberdog::common::Semaphore; 42 | using Clock = std::chrono::system_clock; 43 | using TomlParse = common::CyberdogToml; 44 | 45 | public: 46 | int32_t Init(bool simulator = false) override; 47 | int32_t Open_() override; 48 | int32_t Start_() override; 49 | int32_t Stop_() override; 50 | int32_t Close_() override; 51 | int32_t SelfCheck() override; 52 | int32_t LowPowerOn() override; 53 | int32_t LowPowerOff() override; 54 | 55 | public: 56 | // 结构 57 | typedef struct 58 | { 59 | union { 60 | uint8_t data[136]; 61 | struct 62 | { 63 | uint8_t data_array[64]; 64 | uint64_t data_clock; 65 | uint8_t intensity_array[64]; 66 | }; 67 | }; 68 | uint8_t enable_on_ack; 69 | uint8_t enable_off_ack; 70 | Signal enable_on_signal; 71 | Signal enable_off_signal; 72 | Signal data_signal; 73 | std::atomic data_received; 74 | std::atomic waiting_data; 75 | uint32_t rx_cnt; 76 | uint32_t rx_error_cnt; 77 | Clock::time_point time_start; 78 | } TofMsg; 79 | 80 | enum class TofCode : int32_t 81 | { 82 | kDemoError1 = 21 83 | }; 84 | 85 | private: 86 | // 变量 87 | bool simulator_; 88 | std::thread simulator_thread_; 89 | std::map>> tof_map_; 90 | std::map> tof_data_map_; 91 | std::atomic is_working_ = false; 92 | std::atomic is_low_power_ = false; 93 | std::shared_ptr> code_{nullptr}; 94 | std::atomic sensor_state_ {SwitchState::close}; // node 状态 95 | 96 | private: 97 | // 方法 98 | void SimulationThread(); // 更新模拟数据 99 | bool IsSingleStarted(const std::string & name); 100 | bool IsSingleClosed(const std::string & name); 101 | void TofMsgCallback(EP::DataLabel & label, std::shared_ptr data); 102 | LOGGER_MINOR_INSTANCE("cyberdog_tof"); 103 | }; // class TofCarpo 104 | } // namespace sensor 105 | } // namespace cyberdog 106 | 107 | #endif // TOF_PLUGIN__TOF_PLUGIN_HPP_ 108 | -------------------------------------------------------------------------------- /cyberdog_tof/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_tof 5 | 1.0.0 6 | GPS pluginlib plugins 7 | ZhengJunyuan 8 | Apache License, Version 2.0 9 | 10 | ament_cmake_ros 11 | 12 | pluginlib 13 | rclcpp 14 | params 15 | cyberdog_common 16 | cyberdog_system 17 | ament_index_cpp 18 | protocol 19 | cyberdog_embed_protocol 20 | 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | -------------------------------------------------------------------------------- /cyberdog_tof/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is a Tof plugin. 4 | 5 | -------------------------------------------------------------------------------- /cyberdog_tof/src/tof_plugin.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "tof_plugin/tof_plugin.hpp" 23 | #include "pluginlib/class_list_macros.hpp" 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "ament_index_cpp/get_package_share_directory.hpp" 26 | 27 | 28 | const int kTofOffset = 50; 29 | const char * kDefaultPath = "/toml_config/sensors/"; 30 | const char * kConfigFile = "/toml_config/sensors/tof_config.toml"; 31 | const int kMsgCheckInterval = 60000; // ms 32 | 33 | int32_t cyberdog::sensor::TofCarpo::Init(bool simulator) 34 | { 35 | simulator_ = simulator; 36 | const SYS::ModuleCode kModuleCode = SYS::ModuleCode::kToF; 37 | code_ = std::make_shared>(kModuleCode); 38 | this->Open = std::bind(&cyberdog::sensor::TofCarpo::Open_, this); 39 | this->Start = std::bind(&cyberdog::sensor::TofCarpo::Start_, this); 40 | this->Stop = std::bind(&cyberdog::sensor::TofCarpo::Stop_, this); 41 | this->Close = std::bind(&cyberdog::sensor::TofCarpo::Close_, this); 42 | 43 | std::vector tof_cfg_files; 44 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 45 | 46 | auto GetCfgFile = [&]() { 47 | toml::value tof_config; 48 | auto config_file = local_share_dir + kConfigFile; 49 | toml::value config; 50 | if (!TomlParse::ParseFile(config_file, config)) { 51 | ERROR("toml file[%s] is invalid!", config_file.c_str()); 52 | return false; 53 | } 54 | if (!TomlParse::Get(config, "tof_config", tof_config)) { 55 | ERROR("toml file[%s] get [tof_config] failed!", config_file.c_str()); 56 | return false; 57 | } else { 58 | if (!TomlParse::Get(tof_config, "config_files", tof_cfg_files)) { 59 | ERROR("toml file[%s] get [config_files] failed!", config_file.c_str()); 60 | return false; 61 | } 62 | } 63 | return true; 64 | }; 65 | 66 | if (!simulator_) { 67 | if (!GetCfgFile()) { 68 | ERROR("Init failed!"); 69 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 70 | } 71 | for (auto & file : tof_cfg_files) { 72 | auto path = local_share_dir + kDefaultPath + file; 73 | std::shared_ptr> tof_msg = std::make_shared>( 74 | path, false); 75 | 76 | std::shared_ptr tof_data = 77 | std::make_shared(); 78 | if (tof_map_.find(tof_msg->GetName()) == tof_map_.end()) { 79 | tof_map_.insert(std::make_pair(tof_msg->GetName(), tof_msg)); 80 | tof_data_map_.insert(std::make_pair(tof_msg->GetName(), tof_data)); 81 | tof_msg->GetData()->data_received = false; 82 | tof_msg->LINK_VAR(tof_msg->GetData()->data); 83 | tof_msg->LINK_VAR(tof_msg->GetData()->enable_off_ack); 84 | tof_msg->LINK_VAR(tof_msg->GetData()->enable_on_ack); 85 | tof_msg->SetDataCallback( 86 | std::bind( 87 | &cyberdog::sensor::TofCarpo:: 88 | TofMsgCallback, this, std::placeholders::_1, std::placeholders::_2)); 89 | } 90 | } 91 | } else { 92 | for (auto & file : tof_cfg_files) { 93 | auto path = local_share_dir + kDefaultPath + file; 94 | { 95 | toml::value single_config; 96 | std::string name; 97 | 98 | if (!TomlParse::ParseFile(path, single_config)) { 99 | ERROR("Init failed, toml file[%s] is invalid!", path.c_str()); 100 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 101 | } 102 | if (!TomlParse::Get(single_config, "name", name)) { 103 | ERROR("Init failed, toml file[%s] get param [name] failed!", path.c_str()); 104 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 105 | } 106 | 107 | tof_map_.insert(std::make_pair(name, nullptr)); 108 | } 109 | } 110 | simulator_thread_ = std::thread(std::bind(&cyberdog::sensor::TofCarpo::SimulationThread, this)); 111 | simulator_thread_.detach(); 112 | } 113 | return code_->GetKeyCode(SYS::KeyCode::kOK); 114 | } 115 | 116 | 117 | int32_t cyberdog::sensor::TofCarpo::Open_() 118 | { 119 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 120 | bool status_ok = true; 121 | 122 | if (!simulator_) { 123 | for (auto & tof : tof_map_) { 124 | tof.second->GetData()->time_start = Clock::now(); 125 | tof.second->GetData()->rx_cnt = 0; 126 | tof.second->GetData()->rx_error_cnt = 0; 127 | if (tof.second->GetData()->data_received) { 128 | INFO("[%s] opened successfully", tof.first.c_str()); 129 | continue; 130 | } 131 | int retry = 0; 132 | bool single_status_ok = true; 133 | while (retry++ < 3) { 134 | tof.second->Operate("enable_on", std::vector{}); 135 | if (tof.second->GetData()->enable_on_signal.WaitFor(1000)) { 136 | if (!tof.second->GetData()->data_received) { 137 | ERROR( 138 | "[%s] opened failed,can not receive enable on ack ,time[%d]", 139 | tof.first.c_str(), retry); 140 | single_status_ok = false; 141 | } else { 142 | single_status_ok = true; 143 | INFO("[%s] opened successfully", tof.first.c_str()); 144 | break; 145 | } 146 | } else { 147 | if (tof.second->GetData()->enable_on_ack == 0) { 148 | single_status_ok = true; 149 | INFO("[%s] opened successfully", tof.first.c_str()); 150 | break; 151 | } else { 152 | single_status_ok = false; 153 | ERROR( 154 | "[%s] opened failed, get ack 0x%x!", tof.first.c_str(), 155 | tof.second->GetData()->enable_on_ack); 156 | } 157 | } 158 | } 159 | if (!single_status_ok) {status_ok = false;} 160 | } 161 | } 162 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 163 | return return_code; 164 | } 165 | 166 | int32_t cyberdog::sensor::TofCarpo::Start_() 167 | { 168 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 169 | bool status_ok = true; 170 | if (!simulator_) { 171 | for (auto & tof : tof_map_) { 172 | if (tof.second->GetData()->data_received) { 173 | INFO("[%s] started successfully", tof.first.c_str()); 174 | continue; 175 | } 176 | if (!IsSingleStarted(tof.first)) { 177 | ERROR("[%s] started failed,can not receive data ", tof.first.c_str()); 178 | status_ok = false; 179 | } else { 180 | INFO("[%s] started successfully", tof.first.c_str()); 181 | } 182 | } 183 | } 184 | is_working_ = status_ok; 185 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 186 | return return_code; 187 | } 188 | 189 | int32_t cyberdog::sensor::TofCarpo::Stop_() 190 | { 191 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 192 | bool status_ok = true; 193 | if (!simulator_) { 194 | for (auto & tof : tof_map_) { 195 | int retry = 0; 196 | bool single_status_ok = true; 197 | while (retry++ < 3) { 198 | tof.second->Operate("enable_off", std::vector{}); 199 | if (tof.second->GetData()->enable_off_signal.WaitFor(1000)) { 200 | if (IsSingleClosed(tof.first)) { 201 | INFO("[%s] stoped successfully", tof.first.c_str()); 202 | single_status_ok = true; 203 | break; 204 | } 205 | ERROR( 206 | "[%s] stoped failed,can not receive enable off ack,time[%d]", 207 | tof.first.c_str(), retry); 208 | single_status_ok = false; 209 | } else { 210 | if (tof.second->GetData()->enable_off_ack == 0) { 211 | // TODO(jyy) check stoped; 212 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 213 | if (IsSingleClosed(tof.first)) { 214 | INFO("[%s] stoped successfully", tof.first.c_str()); 215 | single_status_ok = true; 216 | break; 217 | } else { 218 | ERROR( 219 | "[%s] stoped failed,get ack but data is receving,time[%d]", 220 | tof.first.c_str(), retry); 221 | single_status_ok = false; 222 | } 223 | } else { 224 | single_status_ok = false; 225 | ERROR( 226 | "[%s] stoped failed, get ack 0x%x!", tof.first.c_str(), 227 | tof.second->GetData()->enable_off_ack); 228 | } 229 | } 230 | } 231 | if (!single_status_ok) {status_ok = false;} 232 | } 233 | } 234 | is_working_ = (status_ok ? false : true); 235 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 236 | return return_code; 237 | } 238 | 239 | int32_t cyberdog::sensor::TofCarpo::Close_() 240 | { 241 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 242 | bool status_ok = true; 243 | if (!simulator_) { 244 | for (auto & tof : tof_map_) { 245 | if (IsSingleClosed(tof.first)) { 246 | INFO("[%s] closed successfully", tof.first.c_str()); 247 | } else { 248 | status_ok = false; 249 | INFO("[%s] closed failed", tof.first.c_str()); 250 | } 251 | } 252 | } 253 | is_working_ = (status_ok ? false : true); 254 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 255 | return return_code; 256 | } 257 | 258 | int32_t cyberdog::sensor::TofCarpo::SelfCheck() 259 | { 260 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 261 | if (Start() != return_code) { 262 | return_code = code_->GetKeyCode(SYS::KeyCode::kSelfCheckFailed); 263 | } 264 | return return_code; 265 | } 266 | 267 | int32_t cyberdog::sensor::TofCarpo::LowPowerOn() 268 | { 269 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 270 | if (Stop() != return_code) { 271 | return_code = code_->GetKeyCode(SYS::KeyCode::kFailed); 272 | } 273 | return return_code; 274 | } 275 | int32_t cyberdog::sensor::TofCarpo::LowPowerOff() 276 | { 277 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 278 | if (Open() != return_code) { 279 | return_code = code_->GetKeyCode(SYS::KeyCode::kFailed); 280 | } 281 | return return_code; 282 | } 283 | void cyberdog::sensor::TofCarpo::SimulationThread() 284 | { 285 | while (1) { 286 | if (!rclcpp::ok()) { 287 | WARN("[cyberdog_tof]: !rclcpp::ok()"); 288 | break; 289 | } 290 | if (is_working_) { 291 | INFO("[cyberdog_tof]: publish cyberdog_tof payload succeed"); 292 | std::this_thread::sleep_for(std::chrono::microseconds(200000)); 293 | const int datanum = protocol::msg::SingleTofPayload::TOF_DATA_NUM; 294 | std::vector obj; 295 | for (size_t i = 0; i < datanum; i++) { 296 | obj.push_back(1.0f * protocol::msg::SingleTofPayload::SCALE_FACTOR); 297 | } 298 | auto tof_payload = std::make_shared(); 299 | struct timespec time_stu; 300 | clock_gettime(CLOCK_REALTIME, &time_stu); 301 | tof_payload->header.stamp.nanosec = time_stu.tv_nsec; 302 | tof_payload->header.stamp.sec = time_stu.tv_sec; 303 | tof_payload->data = obj; 304 | tof_payload->data_available = false; 305 | 306 | for (auto & tof : tof_map_) { 307 | tof_payload->header.frame_id = tof.first; 308 | if (single_payload_callback_ != nullptr) { 309 | single_payload_callback_(tof_payload); 310 | } 311 | } 312 | INFO("[cyberdog_tof]: publish cyberdog_tof payload succeed"); 313 | } 314 | } 315 | } 316 | bool cyberdog::sensor::TofCarpo::IsSingleStarted(const std::string & name) 317 | { 318 | if (tof_map_.find(name) == tof_map_.end()) { 319 | INFO("tof map not find [%s]", name.c_str()); 320 | return false; 321 | } 322 | tof_map_.at(name)->GetData()->waiting_data = true; 323 | bool is_started = tof_map_.at(name)->GetData()->data_signal.WaitFor(1000) ? false : true; 324 | tof_map_.at(name)->GetData()->waiting_data = false; 325 | return is_started; 326 | } 327 | 328 | bool cyberdog::sensor::TofCarpo::IsSingleClosed(const std::string & name) 329 | { 330 | if (tof_map_.find(name) == tof_map_.end()) { 331 | INFO("tof map not find [%s]", name.c_str()); 332 | return false; 333 | } 334 | tof_map_.at(name)->GetData()->waiting_data = true; 335 | bool is_closed = tof_map_.at(name)->GetData()->data_signal.WaitFor(500) ? true : false; 336 | tof_map_.at(name)->GetData()->waiting_data = false; 337 | tof_map_.at(name)->GetData()->data_received = false; 338 | return is_closed; 339 | } 340 | 341 | void cyberdog::sensor::TofCarpo::TofMsgCallback( 342 | EP::DataLabel & label, 343 | std::shared_ptr data) 344 | { 345 | if (tof_map_.find(label.group_name) != tof_map_.end()) { 346 | if (label.name == "enable_on_ack") { 347 | if (data->enable_on_ack != 0) { 348 | ERROR("%s,enable_on ack err 0x:%x", label.group_name.c_str(), data->enable_on_ack); 349 | } 350 | data->enable_on_signal.Give(); 351 | } else if (label.name == "enable_off_ack") { 352 | if (data->enable_off_ack != 0) { 353 | ERROR("%s,enable_off ack err 0x:%x", label.group_name.c_str(), data->enable_off_ack); 354 | } 355 | data->enable_off_signal.Give(); 356 | } else if (label.name == "data") { 357 | if (data->waiting_data) { 358 | data->data_signal.Give(); 359 | } 360 | if (!data->data_received) { 361 | data->data_received = true; 362 | } 363 | const int datanum = protocol::msg::SingleTofPayload::TOF_DATA_NUM; 364 | std::vector obj_data; 365 | std::vector obj_intensity; 366 | for (size_t i = 0; i < datanum; i++) { 367 | obj_data.push_back( 368 | (data->data_array[i] * 2.0f + kTofOffset) * 369 | protocol::msg::SingleTofPayload::SCALE_FACTOR); 370 | obj_intensity.push_back( 371 | data->intensity_array[i]); 372 | } 373 | 374 | struct timespec time_stu; 375 | clock_gettime(CLOCK_REALTIME, &time_stu); 376 | if (tof_data_map_.find(label.group_name) == tof_data_map_.end()) { 377 | ERROR("data map no msg name %s", label.group_name.c_str()); 378 | } else { 379 | data->rx_cnt++; 380 | if (!label.is_full) { 381 | data->rx_error_cnt++; 382 | } 383 | 384 | // msg check 385 | auto now = Clock::now(); 386 | auto duration = 387 | std::chrono::duration_cast( 388 | now - data->time_start); 389 | if (duration.count() >= kMsgCheckInterval) { 390 | data->time_start = Clock::now(); 391 | if (data->rx_error_cnt > 0) { 392 | WARN( 393 | "[%s] get error data:[%d]/[%d] ,interval[%d] !", 394 | label.group_name.c_str(), tof_map_.at( 395 | label.group_name)->GetData()->rx_error_cnt, tof_map_.at( 396 | label.group_name)->GetData()->rx_cnt, kMsgCheckInterval); 397 | data->rx_error_cnt = 0; 398 | } 399 | data->rx_cnt = 0; 400 | } 401 | 402 | tof_data_map_.at(label.group_name)->header.frame_id = label.group_name; 403 | tof_data_map_.at(label.group_name)->header.stamp.nanosec = time_stu.tv_nsec; 404 | tof_data_map_.at(label.group_name)->header.stamp.sec = time_stu.tv_sec; 405 | tof_data_map_.at(label.group_name)->data = obj_data; 406 | tof_data_map_.at(label.group_name)->intensity = obj_intensity; 407 | tof_data_map_.at(label.group_name)->data_available = label.is_full; 408 | if (single_payload_callback_ != nullptr) { 409 | single_payload_callback_(tof_data_map_.at(label.group_name)); 410 | } 411 | } 412 | } else { 413 | WARN("unknown msg name %s", label.name.c_str()); 414 | } 415 | } else { 416 | ERROR("can drive error,error name %s", label.name.c_str()); 417 | } 418 | } 419 | PLUGINLIB_EXPORT_CLASS(cyberdog::sensor::TofCarpo, cyberdog::sensor::TofBase) 420 | -------------------------------------------------------------------------------- /cyberdog_ultrasonic/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_ultrasonic) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_ros REQUIRED) 11 | find_package(pluginlib REQUIRED) 12 | find_package(params REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(cyberdog_common REQUIRED) 15 | find_package(protocol REQUIRED) 16 | find_package(ament_index_cpp REQUIRED) 17 | find_package(cyberdog_embed_protocol REQUIRED) 18 | find_package(cyberdog_system REQUIRED) 19 | 20 | 21 | add_library(${PROJECT_NAME} SHARED 22 | src/ultrasonic_plugin.cpp 23 | ) 24 | target_include_directories(${PROJECT_NAME} PUBLIC 25 | $ 26 | $) 27 | 28 | target_link_libraries(${PROJECT_NAME} 29 | ${cyberdog_log_LIBRARIES} 30 | ) 31 | 32 | # define project name to get relative share path 33 | target_compile_definitions(${PROJECT_NAME} PRIVATE "PACKAGE_NAME=\"${PROJECT_NAME}\"") 34 | target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 35 | 36 | ament_target_dependencies( 37 | ${PROJECT_NAME} 38 | pluginlib 39 | params 40 | rclcpp 41 | cyberdog_common 42 | ament_index_cpp 43 | protocol 44 | cyberdog_embed_protocol 45 | ) 46 | 47 | pluginlib_export_plugin_description_file(${PROJECT_NAME} plugin.xml) 48 | 49 | install( 50 | DIRECTORY include/ 51 | DESTINATION include/ 52 | ) 53 | 54 | 55 | install( 56 | TARGETS ${PROJECT_NAME} 57 | EXPORT export_${PROJECT_NAME} 58 | ARCHIVE DESTINATION lib 59 | LIBRARY DESTINATION lib 60 | RUNTIME DESTINATION bin 61 | ) 62 | 63 | if(BUILD_TESTING) 64 | find_package(ament_lint_auto REQUIRED) 65 | ament_lint_auto_find_test_dependencies() 66 | find_package(ament_cmake_gtest REQUIRED) 67 | endif() 68 | 69 | ament_export_include_directories(include) 70 | ament_export_libraries(${PROJECT_NAME}) 71 | ament_export_targets(export_${PROJECT_NAME}) 72 | ament_package() 73 | -------------------------------------------------------------------------------- /cyberdog_ultrasonic/README.md: -------------------------------------------------------------------------------- 1 | # bcm_gps 2 | 3 | 该模块主要提供GPS的驱动封装 4 | 5 | - 外部参数通过`cyberdog_ception_bridge`模块中`params/bcmgps_config.toml`载入 6 | - GPS模块自身日志保存在`cyberdog_ception_bridge`模块中`logs`路径下 7 | 8 | ### 主要接口部分如下： 9 | 10 | ```cpp 11 | namespace bcm_gps 12 | { 13 | typedef LD2BRM_PvtPvtPolledPayload GPS_Payload; 14 | using NMEA_callback = std::function; 15 | using PAYLOAD_callback = std::function payload)>; 16 | 17 | class GPS 18 | { 19 | public: 20 | explicit GPS(PAYLOAD_callback PAYLOAD_cb = nullptr, NMEA_callback NMEA_cb = nullptr); 21 | ~GPS(); 22 | bool Open(); 23 | void Start(); 24 | void Stop(); 25 | void Close(); 26 | bool Ready(); 27 | void SetCallback(NMEA_callback NMEA_cb); 28 | void SetCallback(PAYLOAD_callback PAYLOAD_cb); 29 | 30 | void SetL5Bias(uint32_t biasCm); 31 | void SetLteFilterEn(bool enable = true); 32 | }; // class GPS 33 | } // namespace bcm_gps 34 | ``` 35 | 36 | #### public函数： 37 | 38 | 构造函数：初始化GPS对象 39 | 40 | ```cpp 41 | explicit GPS(PAYLOAD_callback PAYLOAD_cb = nullptr, NMEA_callback NMEA_cb = nullptr); 42 | ``` 43 | 44 | - `PAYLOAD_cb`：payload形式回调函数 45 | - `NMEA_cb`：NMEA标准格式字符串回调函数 46 | 47 | 48 | 49 | 开启设备：返回是否开启成功（构造函数会自动开启设备，主要用于手动调用Close()后重新开启） 50 | 51 | ```cpp 52 | bool Open(); 53 | ``` 54 | 55 | 56 | 57 | 打开定位 58 | 59 | ```cpp 60 | void Start(); 61 | ``` 62 | 63 | 64 | 65 | 关闭定位 66 | 67 | ```cpp 68 | void Stop(); 69 | ``` 70 | 71 | 72 | 73 | 关闭设备（关闭后会清空注册的回调函数，重新打开需要Open()后重新注册回调函数获取数据） 74 | 75 | ```cpp 76 | void Close(); 77 | ``` 78 | 79 | 80 | 81 | 获取硬件设备准备状态：返回准备是否成功 82 | 83 | ```cpp 84 | bool Ready(); 85 | ``` 86 | 87 | 88 | 89 | 注册回调函数 90 | 91 | ```cpp 92 | void SetCallback(NMEA_callback NMEA_cb); 93 | void SetCallback(PAYLOAD_callback PAYLOAD_cb); 94 | ``` 95 | 96 | - `PAYLOAD_cb`：payload形式回调函数 97 | - `NMEA_cb`：NMEA标准格式字符串回调函数 98 | 99 | 100 | 101 | Input L5 bias in centimeter 102 | 103 | ```cpp 104 | void SetL5Bias(uint32_t biasCm); 105 | ``` 106 | 107 | - biasCm：L5 bias in centimeter 108 | 109 | 110 | 111 | Enable/Disable LTE Filter 112 | 113 | ```cpp 114 | void SetLteFilterEn(bool enable = true); 115 | ``` 116 | 117 | - enable：Enable/Disable 118 | 119 | 120 | 121 | #### GPS_Payload消息结构 122 | 123 | ```cpp 124 | /** @brief BRM-NAV-PVT (0x01 0x07) 125 | * 126 | */ 127 | PACK( 128 | typedef struct 129 | { 130 | U4 iTOW; 131 | U2 year; 132 | U1 month; 133 | U1 day; 134 | U1 hour; 135 | U1 min; 136 | U1 sec; 137 | X1 valid; 138 | U4 tAcc; 139 | I4 nano; 140 | U1 fixType; 141 | X1 flags; 142 | X1 flags2; 143 | U1 numSV; 144 | I4 lon; 145 | I4 lat; 146 | I4 height; 147 | I4 hMSL; 148 | U4 hAcc; 149 | U4 vAcc; 150 | I4 velN; 151 | I4 velE; 152 | I4 velD; 153 | I4 gSpeed; 154 | I4 headMot; 155 | U4 sAcc; 156 | U4 headAcc; 157 | U2 pDOP; 158 | I1 leapS; 159 | U1 reserved1[5]; 160 | I4 headVeh; 161 | I2 magDec; 162 | U2 magAcc; 163 | }) LD2BRM_PvtPvtPolledPayload; 164 | ``` 165 | 166 | 167 | 168 | ### 外部参数 169 | 170 | 外部参数通过`cyberdog_ception_bridge`模块中`params/bcmgps_config.toml`载入 171 | 172 | ```toml 173 | skip_download = false # need in first line 174 | onlyfirst_download = true 175 | tty = "/dev/ttyTHS0" 176 | patch_path = "/usr/sbin/bream.patch" 177 | 178 | #uint8_t infMsgMask[6] = {0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F}; 179 | #full logging (GLLIO, GLLAPI, RAWDATA, DEVKF, USR2, ) 180 | infMsgMask = [31, 31, 31, 31, 31, 31] 181 | 182 | #1(L1 Best) 2(L1 Auto) 3(L1 ULP), 4(L1L5 Best) 5(L1L5 Auto) 183 | PowerModePreset = 4 184 | 185 | MsgRate = [ 186 | 1, # Report GPGGA rate 187 | 1, # Report GPRMC rate 188 | 1, # Report GPGSV rate 189 | 1, # Report NAVEOE rate 190 | 1, # Report NAVPVT rate 191 | 1, # Report PGLOR SPEED rate 192 | 1, # Report PGLOR FIX rate 193 | 1, # Report PGLOR SAT rate 194 | 1, # Report PGLOR LSQ rate 195 | 1, # Report PGLOR PWR rate 196 | 1, # Report PGLOR STA rate 197 | 1, # Report NAVODO rate 198 | 1, # Report NAVSAT rate 199 | 1, # Report NAVDOP rate 200 | 1, # Report CBEE status rate 201 | 1, # Report ASC SUBFRAMES rate 202 | 1, # Report ASC MEAS rate 203 | 1 # Report ASC AGC rate 204 | ] 205 | 206 | AckAiding = true 207 | ``` 208 | 209 | - `skip_download`：是否跳过下载patch（请置于首行） 210 | - `onlyfirst_download`：仅编译后首次下载（该选项为`true`将会在下载后将`skip_download`置为`true`） 211 | - `tty`：设备号 212 | - `patch_path`：下载patch路径 213 | - `infMsgMask`：GPS模块日志配置 214 | - `PowerModePreset`：GPS频段选择 215 | - 1：(L1 Best) 216 | - 2：(L1 Auto) 217 | - 3：(L1 ULP) 218 | - 4：(L1L5 Best) (默认) 219 | - 5：(L1L5 Auto) 220 | - `MsgRate`：消息频率设置（0为关闭） 221 | - `AckAiding`：true/false 222 | 223 | 224 | 225 | ### Example 226 | 227 | ```cpp 228 | class Example 229 | { 230 | public: 231 | std::shared_ptr gps_; 232 | std::shared_ptr payload_; 233 | 234 | Example() 235 | { 236 | gps_ = std::make_shared(std::bind( 237 | &Example::payload_callback, this, std::placeholders::_1)); 238 | } 239 | 240 | void Func() 241 | { 242 | gps_->Start(); 243 | gps_->Stop(); 244 | gps_->Close(); 245 | } 246 | void payload_callback(std::shared_ptr payload) 247 | { 248 | payload_ = payload; 249 | } 250 | }; // class example 251 | ``` 252 | 253 | -------------------------------------------------------------------------------- /cyberdog_ultrasonic/include/ultrasonic_base/ultrasonic_base.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | 16 | #ifndef ULTRASONIC_BASE__ULTRASONIC_BASE_HPP_ 17 | #define ULTRASONIC_BASE__ULTRASONIC_BASE_HPP_ 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace cyberdog 24 | { 25 | 26 | namespace sensor 27 | { 28 | class UltrasonicBase 29 | { 30 | public: 31 | virtual int32_t Init(bool simulator = false) = 0; 32 | std::function Open, Start, Stop, Close; 33 | virtual int32_t Open_() = 0; 34 | virtual int32_t Start_() = 0; 35 | virtual int32_t Stop_() = 0; 36 | virtual int32_t Close_() = 0; 37 | virtual int32_t SelfCheck() = 0; 38 | virtual int32_t LowPowerOn() = 0; 39 | virtual int32_t LowPowerOff() = 0; 40 | virtual ~UltrasonicBase() {} 41 | void SetSinglePayloadCallback( 42 | std::function payload)> cb) 44 | { 45 | single_payload_callback_ = cb; 46 | } 47 | 48 | protected: 49 | std::function payload)> single_payload_callback_; 50 | // protocol::msg::UltrasonicPayload 51 | UltrasonicBase() {} 52 | }; // class UltrasonicBase 53 | } // namespace sensor 54 | } // namespace cyberdog 55 | 56 | #endif // ULTRASONIC_BASE__ULTRASONIC_BASE_HPP_ 57 | -------------------------------------------------------------------------------- /cyberdog_ultrasonic/include/ultrasonic_plugin/ultrasonic_plugin.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | 16 | #ifndef ULTRASONIC_PLUGIN__ULTRASONIC_PLUGIN_HPP_ 17 | #define ULTRASONIC_PLUGIN__ULTRASONIC_PLUGIN_HPP_ 18 | 19 | 20 | #include 21 | #include 22 | #include 23 | #include "ultrasonic_base/ultrasonic_base.hpp" 24 | #include "embed_protocol/embed_protocol.hpp" 25 | #include "cyberdog_common/cyberdog_log.hpp" 26 | #include "cyberdog_common/cyberdog_semaphore.hpp" 27 | #include "cyberdog_common/cyberdog_toml.hpp" 28 | #include "cyberdog_system/robot_code.hpp" 29 | 30 | namespace cyberdog 31 | { 32 | namespace sensor 33 | { 34 | namespace EP = cyberdog::embed; 35 | namespace SYS = cyberdog::system; 36 | 37 | class UltrasonicCarpo : public cyberdog::sensor::UltrasonicBase 38 | { 39 | using SwitchState = enum {open = 0, start, stop, close, }; // [类型]切换状态 40 | using Signal = cyberdog::common::Semaphore; 41 | using TomlParse = common::CyberdogToml; 42 | 43 | public: 44 | int32_t Init(bool simulator = false) override; 45 | int32_t Open_() override; 46 | int32_t Start_() override; 47 | int32_t Stop_() override; 48 | int32_t Close_() override; 49 | int32_t SelfCheck() override; 50 | int32_t LowPowerOn() override; 51 | int32_t LowPowerOff() override; 52 | 53 | public: 54 | typedef struct 55 | { 56 | union { 57 | uint8_t data[8]; 58 | struct 59 | { 60 | uint16_t ultrasonic_data; 61 | uint16_t ultrasonic_data_intensity; 62 | uint32_t ultrasonic_data_clock; 63 | }; 64 | }; 65 | uint8_t enable_on_ack; 66 | uint8_t enable_off_ack; 67 | Signal enable_on_signal; 68 | Signal enable_off_signal; 69 | Signal data_signal; 70 | std::atomic data_received; 71 | std::atomic waiting_data; 72 | } UltrasonicMsg; 73 | 74 | enum class UltrasonicCode : int32_t 75 | { 76 | kDemoError1 = 21 77 | }; 78 | 79 | private: 80 | bool simulator_; 81 | std::thread simulator_thread_; 82 | void SimulationThread(); // 更新模拟数据 83 | std::shared_ptr> code_{nullptr}; 84 | std::map>> ultrasonic_map_; 85 | std::map> ultrasonic_data_map_; 86 | std::atomic is_working_; 87 | 88 | private: 89 | int32_t Reset(const std::string & name); 90 | bool IsSingleStarted(const std::string & name); 91 | bool IsSingleClosed(const std::string & name); 92 | void UltrasonicMsgCallback(EP::DataLabel & label, std::shared_ptr data); 93 | 94 | LOGGER_MINOR_INSTANCE("cyberdog_ultrasonic"); 95 | }; // class UltrasonicCarpo 96 | } // namespace sensor 97 | } // namespace cyberdog 98 | 99 | #endif // ULTRASONIC_PLUGIN__ULTRASONIC_PLUGIN_HPP_ 100 | -------------------------------------------------------------------------------- /cyberdog_ultrasonic/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_ultrasonic 5 | 1.0.0 6 | GPS pluginlib plugins 7 | ZhengJunyuan 8 | Apache License, Version 2.0 9 | 10 | ament_cmake_ros 11 | 12 | pluginlib 13 | rclcpp 14 | params 15 | cyberdog_common 16 | cyberdog_system 17 | ament_index_cpp 18 | protocol 19 | cyberdog_embed_protocol 20 | 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | -------------------------------------------------------------------------------- /cyberdog_ultrasonic/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is a Ultrasonic plugin. 4 | 5 | -------------------------------------------------------------------------------- /cyberdog_ultrasonic/src/ultrasonic_plugin.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "ultrasonic_plugin/ultrasonic_plugin.hpp" 24 | #include "pluginlib/class_list_macros.hpp" 25 | #include "rclcpp/rclcpp.hpp" 26 | #include "ament_index_cpp/get_package_share_directory.hpp" 27 | #include "cyberdog_common/cyberdog_log.hpp" 28 | 29 | const char * kDefaultPath = "/toml_config/sensors/"; 30 | const char * kConfigFile = "/toml_config/sensors/ultrasonic_config.toml"; 31 | 32 | int32_t cyberdog::sensor::UltrasonicCarpo::Init(bool simulator) 33 | { 34 | simulator_ = simulator; 35 | const SYS::ModuleCode kModuleCode = SYS::ModuleCode::kUltrasonic; 36 | code_ = std::make_shared>(kModuleCode); 37 | this->Open = std::bind(&cyberdog::sensor::UltrasonicCarpo::Open_, this); 38 | this->Start = std::bind(&cyberdog::sensor::UltrasonicCarpo::Start_, this); 39 | this->Stop = std::bind(&cyberdog::sensor::UltrasonicCarpo::Stop_, this); 40 | this->Close = std::bind(&cyberdog::sensor::UltrasonicCarpo::Close_, this); 41 | 42 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 43 | std::vector ultrasonic_cfg_files; 44 | 45 | auto GetCfgFile = [&]() { 46 | toml::value ultrasonic_config; 47 | auto config_file = local_share_dir + kConfigFile; 48 | toml::value config; 49 | if (!TomlParse::ParseFile(config_file, config)) { 50 | ERROR("toml file[%s] is invalid!", config_file.c_str()); 51 | return false; 52 | } 53 | if (!TomlParse::Get(config, "ultrasonic_config", ultrasonic_config)) { 54 | ERROR("toml file[%s] get param [ultrasonic_config] failed!", config_file.c_str()); 55 | return false; 56 | } else { 57 | if (!TomlParse::Get(ultrasonic_config, "config_files", ultrasonic_cfg_files)) { 58 | ERROR("toml file[%s] get param [config_files] failed!", config_file.c_str()); 59 | return false; 60 | } 61 | } 62 | return true; 63 | }; 64 | 65 | if (!simulator) { 66 | if (!GetCfgFile()) { 67 | ERROR("Init failed!"); 68 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 69 | } 70 | for (auto & file : ultrasonic_cfg_files) { 71 | auto path = local_share_dir + kDefaultPath + file; 72 | std::shared_ptr> ultrasonic_msg = 73 | std::make_shared>( 74 | path, false); 75 | std::shared_ptr ultrasonic_data = 76 | std::make_shared(); 77 | 78 | if (ultrasonic_map_.find(ultrasonic_msg->GetName()) == ultrasonic_map_.end()) { 79 | ultrasonic_map_.insert(std::make_pair(ultrasonic_msg->GetName(), ultrasonic_msg)); 80 | ultrasonic_data_map_.insert(std::make_pair(ultrasonic_msg->GetName(), ultrasonic_data)); 81 | ultrasonic_msg->GetData()->data_received = false; 82 | ultrasonic_msg->LINK_VAR(ultrasonic_msg->GetData()->data); 83 | ultrasonic_msg->LINK_VAR(ultrasonic_msg->GetData()->enable_off_ack); 84 | ultrasonic_msg->LINK_VAR(ultrasonic_msg->GetData()->enable_on_ack); 85 | ultrasonic_msg->SetDataCallback( 86 | std::bind( 87 | &cyberdog::sensor::UltrasonicCarpo:: 88 | UltrasonicMsgCallback, this, std::placeholders::_1, std::placeholders::_2)); 89 | } 90 | } 91 | } else { 92 | for (auto & file : ultrasonic_cfg_files) { 93 | auto path = local_share_dir + kDefaultPath + file; 94 | { 95 | toml::value single_config; 96 | std::string name; 97 | 98 | if (!common::CyberdogToml::ParseFile(path, single_config)) { 99 | ERROR("Init failed, toml file[%s] is invalid!", path.c_str()); 100 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 101 | } 102 | if (!common::CyberdogToml::Get(single_config, "name", name)) { 103 | ERROR("Init failed, toml file[%s] get param [name] failed!", path.c_str()); 104 | return code_->GetKeyCode(SYS::KeyCode::kFailed); 105 | } 106 | 107 | ultrasonic_map_.insert(std::make_pair(name, nullptr)); 108 | } 109 | } 110 | simulator_thread_ = 111 | std::thread(std::bind(&cyberdog::sensor::UltrasonicCarpo::SimulationThread, this)); 112 | simulator_thread_.detach(); 113 | } 114 | return code_->GetKeyCode(SYS::KeyCode::kOK); 115 | } 116 | 117 | int32_t cyberdog::sensor::UltrasonicCarpo::Open_() 118 | { 119 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 120 | bool status_ok = true; 121 | 122 | if (!simulator_) { 123 | for (auto & ultrasonic : ultrasonic_map_) { 124 | if (ultrasonic.second->GetData()->data_received) { 125 | INFO("[%s] opened successfully", ultrasonic.first.c_str()); 126 | continue; 127 | } 128 | int retry = 0; 129 | bool single_status_ok = true; 130 | while (retry++ < 3) { 131 | ultrasonic.second->Operate("enable_on", std::vector{}); 132 | if (ultrasonic.second->GetData()->enable_on_signal.WaitFor(1000)) { 133 | if (!ultrasonic.second->GetData()->data_received) { 134 | ERROR( 135 | "[%s] opened failed,can not receive enable on ack ,time[%d]", 136 | ultrasonic.first.c_str(), retry); 137 | single_status_ok = false; 138 | } else { 139 | single_status_ok = true; 140 | INFO("[%s] opened successfully", ultrasonic.first.c_str()); 141 | break; 142 | } 143 | } else { 144 | if (ultrasonic.second->GetData()->enable_on_ack == 0) { 145 | if (IsSingleStarted(ultrasonic.first)) { 146 | INFO("[%s] opened successfully", ultrasonic.first.c_str()); 147 | single_status_ok = true; 148 | break; 149 | } else { 150 | single_status_ok = false; 151 | WARN("[%s]opened but no data received!", ultrasonic.first.c_str()); 152 | Reset(ultrasonic.first); 153 | } 154 | } else { 155 | single_status_ok = false; 156 | ERROR( 157 | "[%s] opened failed, get ack 0x%x!", 158 | ultrasonic.first.c_str(), ultrasonic.second->GetData()->enable_on_ack); 159 | } 160 | } 161 | } 162 | if (!single_status_ok) {status_ok = false;} 163 | } 164 | } 165 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 166 | return return_code; 167 | } 168 | 169 | int32_t cyberdog::sensor::UltrasonicCarpo::Start_() 170 | { 171 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 172 | bool status_ok = true; 173 | if (!simulator_) { 174 | for (auto & ultrasonic : ultrasonic_map_) { 175 | if (ultrasonic.second->GetData()->data_received) { 176 | INFO("[%s] started successfully", ultrasonic.first.c_str()); 177 | continue; 178 | } 179 | if (!IsSingleStarted(ultrasonic.first)) { 180 | ERROR("[%s] started failed,can not receive data ", ultrasonic.first.c_str()); 181 | status_ok = false; 182 | } else { 183 | INFO("[%s] started successfully", ultrasonic.first.c_str()); 184 | } 185 | } 186 | } 187 | is_working_ = status_ok; 188 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 189 | return return_code; 190 | } 191 | 192 | int32_t cyberdog::sensor::UltrasonicCarpo::Stop_() 193 | { 194 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 195 | bool status_ok = true; 196 | if (!simulator_) { 197 | for (auto & ultrasonic : ultrasonic_map_) { 198 | int retry = 0; 199 | bool single_status_ok = true; 200 | while (retry++ < 3) { 201 | ultrasonic.second->Operate("enable_off", std::vector{}); 202 | if (ultrasonic.second->GetData()->enable_off_signal.WaitFor(1000)) { 203 | if (IsSingleClosed(ultrasonic.first)) { 204 | INFO("[%s] stoped successfully", ultrasonic.first.c_str()); 205 | single_status_ok = true; 206 | break; 207 | } 208 | ERROR( 209 | "[%s] stoped failed,can not receive enable off ack,time[%d]", 210 | ultrasonic.first.c_str(), retry); 211 | single_status_ok = false; 212 | } else { 213 | if (ultrasonic.second->GetData()->enable_off_ack == 0) { 214 | // TODO(jyy) check stoped; 215 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 216 | if (IsSingleClosed(ultrasonic.first)) { 217 | INFO("[%s] stoped successfully", ultrasonic.first.c_str()); 218 | single_status_ok = true; 219 | break; 220 | } else { 221 | ERROR( 222 | "[%s] stoped failed,get ack but data is receving,time[%d]", 223 | ultrasonic.first.c_str(), retry); 224 | single_status_ok = false; 225 | } 226 | } else { 227 | single_status_ok = false; 228 | ERROR( 229 | "[%s] stoped failed, get ack 0x%x!", 230 | ultrasonic.first.c_str(), ultrasonic.second->GetData()->enable_off_ack); 231 | } 232 | } 233 | } 234 | if (!single_status_ok) {status_ok = false;} 235 | } 236 | } 237 | is_working_ = (status_ok ? false : true); 238 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 239 | return return_code; 240 | } 241 | 242 | int32_t cyberdog::sensor::UltrasonicCarpo::Close_() 243 | { 244 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 245 | bool status_ok = true; 246 | if (!simulator_) { 247 | for (auto & ultrasonic : ultrasonic_map_) { 248 | if (IsSingleClosed(ultrasonic.first)) { 249 | INFO("[%s] closed successfully", ultrasonic.first.c_str()); 250 | } else { 251 | status_ok = false; 252 | INFO("[%s] closed failed", ultrasonic.first.c_str()); 253 | } 254 | } 255 | } 256 | is_working_ = (status_ok ? false : true); 257 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 258 | return return_code; 259 | } 260 | int32_t cyberdog::sensor::UltrasonicCarpo::Reset(const std::string & name) 261 | { 262 | if (ultrasonic_map_.find(name) == ultrasonic_map_.end()) { 263 | INFO("%s:ultrasonic map not find [%s]", __func__, name.c_str()); 264 | return false; 265 | } 266 | INFO("%s[%s]", __func__, name.c_str()); 267 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 268 | bool status_ok = true; 269 | 270 | if (!simulator_) { 271 | int retry = 0; 272 | bool single_status_ok = true; 273 | while (retry++ < 3) { 274 | ultrasonic_map_.at(name)->Operate("enable_off", std::vector{}); 275 | if (!ultrasonic_map_.at(name)->GetData()->enable_off_signal.WaitFor(500)) { 276 | if (ultrasonic_map_.at(name)->GetData()->enable_off_ack == 0) { 277 | single_status_ok = true; 278 | std::this_thread::sleep_for(std::chrono::milliseconds(200)); 279 | } else { 280 | single_status_ok = false; 281 | ERROR( 282 | "[%s] reset failed, get ack 0x%x!", 283 | name.c_str(), ultrasonic_map_.at(name)->GetData()->enable_off_ack); 284 | } 285 | } else { 286 | ERROR("[%s] reset failed, no ack![%d]", name.c_str(), retry); 287 | } 288 | } 289 | if (!single_status_ok) {status_ok = false;} 290 | } 291 | if (!status_ok) {return_code = code_->GetKeyCode(SYS::KeyCode::kFailed);} 292 | return return_code; 293 | } 294 | int32_t cyberdog::sensor::UltrasonicCarpo::SelfCheck() 295 | { 296 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 297 | if (Start() != return_code) { 298 | return_code = code_->GetKeyCode(SYS::KeyCode::kSelfCheckFailed); 299 | } 300 | return return_code; 301 | } 302 | 303 | int32_t cyberdog::sensor::UltrasonicCarpo::LowPowerOn() 304 | { 305 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 306 | if (Stop() != return_code) { 307 | return_code = code_->GetKeyCode(SYS::KeyCode::kFailed); 308 | } 309 | return return_code; 310 | } 311 | 312 | int32_t cyberdog::sensor::UltrasonicCarpo::LowPowerOff() 313 | { 314 | int32_t return_code = code_->GetKeyCode(SYS::KeyCode::kOK); 315 | if (Open() != return_code) { 316 | return_code = code_->GetKeyCode(SYS::KeyCode::kFailed); 317 | } 318 | return return_code; 319 | } 320 | 321 | bool cyberdog::sensor::UltrasonicCarpo::IsSingleStarted(const std::string & name) 322 | { 323 | if (ultrasonic_map_.find(name) == ultrasonic_map_.end()) { 324 | INFO("ultrasonic map not find [%s]", name.c_str()); 325 | return false; 326 | } 327 | ultrasonic_map_.at(name)->GetData()->waiting_data = true; 328 | bool is_started = ultrasonic_map_.at(name)->GetData()->data_signal.WaitFor(1000) ? false : true; 329 | ultrasonic_map_.at(name)->GetData()->waiting_data = false; 330 | return is_started; 331 | } 332 | 333 | bool cyberdog::sensor::UltrasonicCarpo::IsSingleClosed(const std::string & name) 334 | { 335 | if (ultrasonic_map_.find(name) == ultrasonic_map_.end()) { 336 | INFO("ultrasonic map not find [%s]", name.c_str()); 337 | return false; 338 | } 339 | ultrasonic_map_.at(name)->GetData()->waiting_data = true; 340 | bool is_closed = ultrasonic_map_.at(name)->GetData()->data_signal.WaitFor(1000) ? true : false; 341 | ultrasonic_map_.at(name)->GetData()->waiting_data = false; 342 | ultrasonic_map_.at(name)->GetData()->data_received = false; 343 | return is_closed; 344 | } 345 | 346 | void cyberdog::sensor::UltrasonicCarpo::UltrasonicMsgCallback( 347 | EP::DataLabel & label, 348 | std::shared_ptr data) 349 | { 350 | if (ultrasonic_map_.find(label.group_name) != ultrasonic_map_.end()) { 351 | if (label.name == "enable_on_ack") { 352 | if (data->enable_on_ack != 0) { 353 | ERROR("%s,enable_on ack err 0x:%x", label.group_name.c_str(), data->enable_on_ack); 354 | } 355 | data->enable_on_signal.Give(); 356 | } else if (label.name == "enable_off_ack") { 357 | if (data->enable_off_ack != 0) { 358 | ERROR("%s,enable_off ack err 0x:%x", label.group_name.c_str(), data->enable_off_ack); 359 | } 360 | data->enable_off_signal.Give(); 361 | } else if (label.name == "data") { 362 | if (data->waiting_data) { 363 | data->data_signal.Give(); 364 | } 365 | if (!data->data_received) { 366 | data->data_received = true; 367 | } 368 | struct timespec time_stu; 369 | clock_gettime(CLOCK_REALTIME, &time_stu); 370 | if (ultrasonic_data_map_.find(label.group_name) == ultrasonic_data_map_.end()) { 371 | ERROR("data map no msg name %s", label.group_name.c_str()); 372 | } else { 373 | ultrasonic_data_map_.at(label.group_name)->header.frame_id = label.group_name; 374 | ultrasonic_data_map_.at(label.group_name)->header.stamp.nanosec = time_stu.tv_nsec; 375 | ultrasonic_data_map_.at(label.group_name)->header.stamp.sec = time_stu.tv_sec; 376 | ultrasonic_data_map_.at(label.group_name)->radiation_type = 377 | sensor_msgs::msg::Range::ULTRASOUND; 378 | ultrasonic_data_map_.at(label.group_name)->min_range = 0.1f; 379 | ultrasonic_data_map_.at(label.group_name)->max_range = 1.0f; 380 | ultrasonic_data_map_.at(label.group_name)->field_of_view = 15.0f; 381 | ultrasonic_data_map_.at(label.group_name)->range = data->ultrasonic_data * 0.001f; 382 | if (single_payload_callback_ != nullptr) { 383 | single_payload_callback_(ultrasonic_data_map_.at(label.group_name)); 384 | } 385 | } 386 | } else { 387 | WARN("unknown msg name %s", label.name.c_str()); 388 | } 389 | } else { 390 | ERROR("can drive error,error name %s", label.name.c_str()); 391 | } 392 | } 393 | 394 | void cyberdog::sensor::UltrasonicCarpo::SimulationThread() 395 | { 396 | while (1) { 397 | if (!rclcpp::ok()) { 398 | WARN("[cyberdog_ultrasonic]: !rclcpp::ok()"); 399 | break; 400 | } 401 | if (is_working_) { 402 | std::this_thread::sleep_for(std::chrono::microseconds(200000)); 403 | auto ultrasonic_payload = std::make_shared(); 404 | struct timespec time_stu; 405 | clock_gettime(CLOCK_REALTIME, &time_stu); 406 | ultrasonic_payload->header.stamp.nanosec = time_stu.tv_nsec; 407 | ultrasonic_payload->header.stamp.sec = time_stu.tv_sec; 408 | ultrasonic_payload->radiation_type = sensor_msgs::msg::Range::ULTRASOUND; 409 | ultrasonic_payload->min_range = 0.1f; 410 | ultrasonic_payload->max_range = 1.0f; 411 | ultrasonic_payload->field_of_view = 15.0f; 412 | ultrasonic_payload->range = 0.001f; 413 | 414 | for (auto & ultrasonic : ultrasonic_map_) { 415 | ultrasonic_payload->header.frame_id = ultrasonic.first; 416 | if (single_payload_callback_ != nullptr) { 417 | single_payload_callback_(ultrasonic_payload); 418 | } 419 | } 420 | } 421 | } 422 | } 423 | PLUGINLIB_EXPORT_CLASS(cyberdog::sensor::UltrasonicCarpo, cyberdog::sensor::UltrasonicBase) 424 | -------------------------------------------------------------------------------- /sensor_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(sensor_manager) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | if(NOT CMAKE_C_STANDARD) 9 | set(CMAKE_C_STANDARD 99) 10 | endif() 11 | 12 | if(NOT CMAKE_CXX_STANDARD) 13 | set(CMAKE_CXX_STANDARD 17) 14 | endif() 15 | 16 | 17 | link_directories(/opt/ros2/galactic/lib) 18 | 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(manager_base REQUIRED) 22 | find_package(protocol REQUIRED) 23 | find_package(pluginlib REQUIRED) 24 | find_package(cyberdog_system REQUIRED) 25 | find_package(cyberdog_gps REQUIRED) 26 | find_package(cyberdog_lidar REQUIRED) 27 | find_package(cyberdog_ultrasonic REQUIRED) 28 | find_package(cyberdog_common REQUIRED) 29 | find_package(cyberdog_tof REQUIRED) 30 | find_package(cyberdog_machine REQUIRED) 31 | find_package(ament_index_cpp REQUIRED) 32 | 33 | 34 | 35 | include_directories(include) 36 | 37 | add_executable(${PROJECT_NAME} 38 | src/sensor_manager.cpp 39 | src/main.cpp) 40 | 41 | 42 | target_link_libraries(${PROJECT_NAME} 43 | ${cyberdog_log_LIBRARIES} 44 | ) 45 | 46 | target_include_directories(${PROJECT_NAME} PUBLIC 47 | $ 48 | $ 49 | ) 50 | 51 | 52 | 53 | ament_target_dependencies(${PROJECT_NAME} 54 | rclcpp 55 | protocol 56 | manager_base 57 | cyberdog_system 58 | pluginlib 59 | cyberdog_gps 60 | cyberdog_lidar 61 | cyberdog_ultrasonic 62 | cyberdog_tof 63 | cyberdog_machine 64 | ament_index_cpp) 65 | 66 | install( TARGETS 67 | ${PROJECT_NAME} 68 | DESTINATION lib/${PROJECT_NAME}) 69 | 70 | if(BUILD_TESTING) 71 | find_package(ament_lint_auto REQUIRED) 72 | ament_lint_auto_find_test_dependencies() 73 | endif() 74 | 75 | ament_package() 76 | -------------------------------------------------------------------------------- /sensor_manager/include/sensor_manager/self_check.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | #ifndef SENSOR_MANAGER__SELF_CHECK_HPP_ 15 | #define SENSOR_MANAGER__SELF_CHECK_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include "ament_index_cpp/get_package_share_directory.hpp" 21 | #include "cyberdog_common/cyberdog_toml.hpp" 22 | #include "cyberdog_common/cyberdog_log.hpp" 23 | 24 | namespace cyberdog 25 | { 26 | namespace sensor 27 | { 28 | class SensorSelfCheck final 29 | { 30 | public: 31 | SensorSelfCheck() 32 | { 33 | std::vector target_vec; 34 | std::vector critical_moudle_vec; 35 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 36 | auto path = local_share_dir + std::string("/toml_config/manager/selfcheck_config.toml"); 37 | toml::value config; 38 | if (common::CyberdogToml::ParseFile(path, config)) { 39 | INFO("Parse Self Check config file started, toml file is valid"); 40 | toml::value sensors_sec; 41 | if (common::CyberdogToml::Get(config, "sensors", sensors_sec)) { 42 | INFO("Self Check init started, parse sensors config succeed"); 43 | if (common::CyberdogToml::Get(sensors_sec, "jump", target_vec)) { 44 | INFO("Self Check init started, parse jump array succeed"); 45 | } 46 | if (common::CyberdogToml::Get(sensors_sec, "critical", critical_moudle_vec)) { 47 | INFO("Self Check init started, parse critical array succeed"); 48 | } 49 | } 50 | } 51 | if (target_vec.size() > 0) { 52 | for (auto & elem : target_vec) { 53 | if (self_check_map_.find(elem) != self_check_map_.end()) { 54 | self_check_map_[elem] = true; 55 | } 56 | } 57 | } else { 58 | INFO("jump sensors is empty"); 59 | } 60 | if (critical_moudle_vec.size() > 0) { 61 | for (auto & elem : critical_moudle_vec) { 62 | if (critical_moudle_map_.find(elem) != critical_moudle_map_.end()) { 63 | critical_moudle_map_[elem] = true; 64 | } 65 | } 66 | } else { 67 | INFO("critical sensors is empty"); 68 | } 69 | } 70 | 71 | bool IsJump(const std::string name) 72 | { 73 | if (self_check_map_.find(name) != self_check_map_.end()) { 74 | return self_check_map_[name]; 75 | } 76 | return false; 77 | } 78 | 79 | bool IsCritical(const std::string & name) 80 | { 81 | if (critical_moudle_map_.find(name) != critical_moudle_map_.end()) { 82 | return critical_moudle_map_[name]; 83 | } 84 | return false; 85 | } 86 | 87 | private: 88 | std::map self_check_map_ = { 89 | {"lidar", false}, 90 | {"gps", false}, 91 | {"ultrasonic", false}, 92 | {"tof", false} 93 | }; 94 | std::map critical_moudle_map_ = { 95 | {"lidar", false}, 96 | {"gps", false}, 97 | {"ultrasonic", false}, 98 | {"tof", false} 99 | }; 100 | }; 101 | } // namespace sensor 102 | } // namespace cyberdog 103 | 104 | #endif // SENSOR_MANAGER__SELF_CHECK_HPP_ 105 | -------------------------------------------------------------------------------- /sensor_manager/include/sensor_manager/sensor_manager.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | #ifndef SENSOR_MANAGER__SENSOR_MANAGER_HPP_ 15 | #define SENSOR_MANAGER__SENSOR_MANAGER_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include "pluginlib/class_loader.hpp" 26 | #include "bcmgps_base/bcmgps_base.hpp" 27 | #include "protocol/msg/gps_payload.hpp" 28 | #include "lidar_base/lidar_base.hpp" 29 | #include "sensor_msgs/msg/laser_scan.hpp" 30 | #include "manager_base/manager_base.hpp" 31 | #include "ultrasonic_base/ultrasonic_base.hpp" 32 | #include "tof_base/tof_base.hpp" 33 | #include "protocol/msg/single_tof_payload.hpp" 34 | #include "protocol/msg/head_tof_payload.hpp" 35 | #include "protocol/msg/rear_tof_payload.hpp" 36 | #include "rclcpp/rclcpp.hpp" 37 | #include "protocol/srv/sensor_operation.hpp" 38 | #include "cyberdog_machine/cyberdog_fs_machine.hpp" 39 | #include "cyberdog_machine/cyberdog_heartbeats.hpp" 40 | #include "sensor_manager/self_check.hpp" 41 | 42 | namespace cyberdog 43 | { 44 | namespace sensor 45 | { 46 | namespace SYS = cyberdog::system; 47 | 48 | enum class SensorErrorCode : int32_t 49 | { 50 | kDemoError1 = 21, 51 | kDemoError2 = 22, 52 | kDemoError3 = 23 53 | }; 54 | 55 | class SensorManager final : public cyberdog::machine::MachineActuator 56 | { 57 | using ScanMsg = sensor_msgs::msg::LaserScan; // [topic 类型]激光数据 58 | 59 | public: 60 | explicit SensorManager(const std::string & name); 61 | ~SensorManager(); 62 | 63 | void Config(); 64 | bool Init(); 65 | void Run(); 66 | int32_t SelfCheck(); 67 | 68 | public: 69 | int32_t OnError(); 70 | int32_t OnLowPower(); 71 | int32_t OnSuspend(); 72 | int32_t OnProtected(); 73 | int32_t OnActive(); 74 | int32_t OnDeActive(); 75 | int32_t OnSetUp(); 76 | int32_t ONTearDown(); 77 | int32_t OnOTA(); 78 | 79 | private: 80 | bool IsStateValid(); 81 | 82 | template 83 | bool SensorOperation(T elem, uint8_t oper_id); 84 | 85 | private: 86 | std::string name_; 87 | std::vector simulator_; 88 | rclcpp::Node::SharedPtr node_ptr_ {nullptr}; 89 | std::unique_ptr heart_beats_ptr_ {nullptr}; 90 | std::shared_ptr> code_ptr_ {nullptr}; 91 | std::unique_ptr sensor_self_check_ptr {nullptr}; 92 | rclcpp::executors::MultiThreadedExecutor executor; 93 | rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; 94 | 95 | private: 96 | std::shared_ptr gps_; 97 | rclcpp::Publisher::SharedPtr gps_publisher_; 98 | void gps_payload_callback(std::shared_ptr msg); 99 | 100 | std::shared_ptr lidar_; 101 | rclcpp::Publisher::SharedPtr lidar_publisher_; 102 | void lidar_payload_callback(std::shared_ptr msg); 103 | 104 | std::shared_ptr ultrasonic_; 105 | rclcpp::Publisher::SharedPtr ultrasonic_publisher_; 106 | void ultrasonic_payload_callback(std::shared_ptr msg); 107 | std::mutex ultrasonic_lock_; 108 | 109 | std::shared_ptr tof_; 110 | rclcpp::Publisher::SharedPtr head_tof_publisher_; 111 | rclcpp::Publisher::SharedPtr rear_tof_publisher_; 112 | std::shared_ptr head_tof_payload; 113 | std::shared_ptr rear_tof_payload; 114 | std::unordered_map> head_tof_; 115 | std::unordered_map> rear_tof_; 116 | std::mutex head_tof_lock_; 117 | std::mutex rear_tof_lock_; 118 | void SingleTofPayloadCallback(std::shared_ptr msg); 119 | 120 | rclcpp::Service::SharedPtr sensor_operation_srv_; 121 | void sensor_operation( 122 | const protocol::srv::SensorOperation::Request::SharedPtr request, 123 | protocol::srv::SensorOperation::Response::SharedPtr response); 124 | 125 | private: 126 | const std::string Uninitialized_V = std::string("Uninitialized"); 127 | const std::string SetUp_V = std::string("SetUp"); 128 | const std::string TearDown_V = std::string("TearDown"); 129 | const std::string SelfCheck_V = std::string("SelfCheck"); 130 | const std::string Active_V = std::string("Active"); 131 | const std::string DeActive_V = std::string("DeActive"); 132 | const std::string Protected_V = std::string("Protected"); 133 | const std::string LowPower_V = std::string("LowPower"); 134 | const std::string OTA_V = std::string("OTA"); 135 | const std::string Error_V = std::string("Error"); 136 | }; // class SensorManager 137 | } // namespace sensor 138 | } // namespace cyberdog 139 | 140 | #endif // SENSOR_MANAGER__SENSOR_MANAGER_HPP_ 141 | -------------------------------------------------------------------------------- /sensor_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | sensor_manager 5 | 0.0.0 6 | TODO: Package description 7 | dukun 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rclcpp 12 | cyberdog_system 13 | cyberdog_common 14 | manager_base 15 | pluginlib 16 | cyberdog_gps 17 | cyberdog_lidar 18 | cyberdog_ultrasonic 19 | cyberdog_tof 20 | cyberdog_machine 21 | ament_index_cpp 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /sensor_manager/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #include 16 | #include 17 | #include "sensor_manager/sensor_manager.hpp" 18 | #include "cyberdog_common/cyberdog_log.hpp" 19 | 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | rclcpp::init(argc, argv); 24 | LOGGER_MAIN_INSTANCE("sensor_manager"); 25 | INFO("sensor manager begin"); 26 | auto sensor_manager = 27 | std::make_shared(std::string("sensor_manager")); 28 | sensor_manager->Config(); 29 | INFO("sensor manager Config succeed"); 30 | if (!sensor_manager->Init()) { 31 | INFO("sensor manager init failed!"); 32 | return -1; 33 | } 34 | sensor_manager->Run(); 35 | // sensor_manager->OnActive(); 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /sensor_manager/src/sensor_manager.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 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 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "sensor_manager/sensor_manager.hpp" 21 | #include "cyberdog_common/cyberdog_log.hpp" 22 | 23 | #define IS_OK(code) ((code & 0xFF) ? false : true) 24 | 25 | cyberdog::sensor::SensorManager::SensorManager(const std::string & name) 26 | : cyberdog::machine::MachineActuator(name), 27 | name_(name) 28 | { 29 | node_ptr_ = rclcpp::Node::make_shared(name_); 30 | executor.add_node(node_ptr_); 31 | heart_beats_ptr_ = std::make_unique("sensor"); 32 | code_ptr_ = std::make_shared>( 33 | SYS::ModuleCode::kSensorManager); 34 | sensor_self_check_ptr = std::make_unique(); 35 | } 36 | 37 | cyberdog::sensor::SensorManager::~SensorManager() 38 | { 39 | } 40 | 41 | void cyberdog::sensor::SensorManager::Config() 42 | { 43 | INFO("sensor manager Configuring begin"); 44 | 45 | // gps 46 | INFO("gps Configuring beginning"); 47 | gps_publisher_ = node_ptr_->create_publisher( 48 | "gps_payload", 49 | rclcpp::SystemDefaultsQoS()); 50 | std::shared_ptr> gps_classloader; 51 | gps_classloader = std::make_shared>( 52 | "cyberdog_gps", "cyberdog::sensor::GpsBase"); 53 | gps_ = gps_classloader->createSharedInstance("cyberdog::sensor::GpsCarpo"); 54 | gps_->SetPayloadCallback( 55 | std::bind( 56 | &SensorManager::gps_payload_callback, this, 57 | std::placeholders::_1)); 58 | 59 | // lidar 60 | INFO("sensor manager Configuring begin"); 61 | INFO("lidar Configuring beginning"); 62 | lidar_publisher_ = node_ptr_->create_publisher( 63 | "scan", 64 | rclcpp::SystemDefaultsQoS()); 65 | std::shared_ptr> lidar_classloader; 66 | lidar_classloader = std::make_shared>( 67 | "cyberdog_lidar", "cyberdog::sensor::LidarBase"); 68 | lidar_ = lidar_classloader->createSharedInstance("cyberdog::sensor::YdlidarCarpo"); 69 | lidar_->SetPayloadCallback( 70 | std::bind( 71 | &SensorManager::lidar_payload_callback, this, 72 | std::placeholders::_1)); 73 | 74 | // ultrasonic 75 | INFO("ultrasonic Configuring beginning"); 76 | ultrasonic_publisher_ = node_ptr_->create_publisher( 77 | "ultrasonic_payload", rclcpp::SystemDefaultsQoS()); 78 | std::shared_ptr> ultrasonic_classloader; 79 | 80 | ultrasonic_classloader = 81 | std::make_shared>( 82 | "cyberdog_ultrasonic", "cyberdog::sensor::UltrasonicBase"); 83 | ultrasonic_ = ultrasonic_classloader->createSharedInstance("cyberdog::sensor::UltrasonicCarpo"); 84 | ultrasonic_->SetSinglePayloadCallback( 85 | std::bind( 86 | &SensorManager::ultrasonic_payload_callback, this, 87 | std::placeholders::_1)); 88 | 89 | // tof 90 | INFO("tof Configuring beginning"); 91 | head_tof_publisher_ = node_ptr_->create_publisher( 92 | "head_tof_payload", rclcpp::SystemDefaultsQoS()); 93 | rear_tof_publisher_ = node_ptr_->create_publisher( 94 | "rear_tof_payload", rclcpp::SystemDefaultsQoS()); 95 | std::shared_ptr> tof_classloader; 96 | tof_classloader = std::make_shared>( 97 | "cyberdog_tof", "cyberdog::sensor::TofBase"); 98 | tof_ = tof_classloader->createSharedInstance("cyberdog::sensor::TofCarpo"); 99 | head_tof_["left_head_tof"] = false; 100 | head_tof_["right_head_tof"] = false; 101 | rear_tof_["left_rear_tof"] = false; 102 | rear_tof_["right_rear_tof"] = false; 103 | head_tof_payload = std::make_shared(); 104 | rear_tof_payload = std::make_shared(); 105 | tof_->SetSinglePayloadCallback( 106 | std::bind( 107 | &SensorManager::SingleTofPayloadCallback, this, 108 | std::placeholders::_1)); 109 | 110 | callback_group_ = 111 | node_ptr_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 112 | 113 | sensor_operation_srv_ = node_ptr_->create_service( 114 | "sensor_operation", 115 | std::bind( 116 | &SensorManager::sensor_operation, this, std::placeholders::_1, 117 | std::placeholders::_2), 118 | rmw_qos_profile_services_default, callback_group_); 119 | 120 | INFO("sensor_manager Configuring,success"); 121 | } 122 | 123 | bool cyberdog::sensor::SensorManager::Init() 124 | { 125 | INFO("SensorManager Initing begin"); 126 | 127 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 128 | auto path = local_share_dir + std::string("/toml_config/manager/state_machine_config.toml"); 129 | if (!this->MachineActuatorInit( 130 | path, 131 | node_ptr_)) 132 | { 133 | ERROR("Init failed, actuator init error."); 134 | return false; 135 | } 136 | this->RegisterStateCallback(SetUp_V, std::bind(&SensorManager::OnSetUp, this)); 137 | this->RegisterStateCallback(TearDown_V, std::bind(&SensorManager::ONTearDown, this)); 138 | this->RegisterStateCallback(SelfCheck_V, std::bind(&SensorManager::SelfCheck, this)); 139 | this->RegisterStateCallback(Active_V, std::bind(&SensorManager::OnActive, this)); 140 | this->RegisterStateCallback(DeActive_V, std::bind(&SensorManager::OnDeActive, this)); 141 | this->RegisterStateCallback(Protected_V, std::bind(&SensorManager::OnProtected, this)); 142 | this->RegisterStateCallback(LowPower_V, std::bind(&SensorManager::OnLowPower, this)); 143 | this->RegisterStateCallback(OTA_V, std::bind(&SensorManager::OnOTA, this)); 144 | this->RegisterStateCallback(Error_V, std::bind(&SensorManager::OnError, this)); 145 | heart_beats_ptr_->HeartBeatRun(); 146 | INFO(">>>init:heart run and state actuator start"); 147 | return this->ActuatorStart(); 148 | } 149 | 150 | void cyberdog::sensor::SensorManager::Run() 151 | { 152 | executor.spin(); 153 | rclcpp::shutdown(); 154 | } 155 | 156 | int32_t cyberdog::sensor::SensorManager::SelfCheck() 157 | { 158 | int32_t error_code = code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 159 | int32_t return_code = code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 160 | try { 161 | // check all sensors from config 162 | INFO("SensorManager SelfCheck begin"); 163 | if (!sensor_self_check_ptr->IsJump("lidar")) { 164 | return_code = this->lidar_->SelfCheck(); 165 | if (!IS_OK(return_code)) { 166 | ERROR("Lidar selfcheck fail."); 167 | if (sensor_self_check_ptr->IsCritical("lidar")) { 168 | return return_code; 169 | } 170 | error_code += return_code; 171 | } else { 172 | INFO("Lidar selfcheck success."); 173 | } 174 | } 175 | 176 | if (!sensor_self_check_ptr->IsJump("tof")) { 177 | return_code = this->tof_->SelfCheck(); 178 | if (!IS_OK(return_code)) { 179 | ERROR("Tof selfcheck fail."); 180 | if (sensor_self_check_ptr->IsCritical("tof")) { 181 | return return_code; 182 | } 183 | error_code += return_code; 184 | } else { 185 | INFO("Tof selfcheck success."); 186 | } 187 | } 188 | 189 | if (!sensor_self_check_ptr->IsJump("ultrasonic")) { 190 | return_code = this->ultrasonic_->SelfCheck(); 191 | if (!IS_OK(return_code)) { 192 | ERROR("Ultrasonic selfcheck fail."); 193 | if (sensor_self_check_ptr->IsCritical("ultrasonic")) { 194 | return return_code; 195 | } 196 | error_code += return_code; 197 | } else { 198 | INFO("Ultrasonic selfcheck success."); 199 | } 200 | } 201 | 202 | if (!sensor_self_check_ptr->IsJump("gps")) { 203 | return_code = this->gps_->SelfCheck(); 204 | if (!IS_OK(return_code)) { 205 | ERROR("Gps selfcheck fail."); 206 | if (sensor_self_check_ptr->IsCritical("gps")) { 207 | return return_code; 208 | } 209 | error_code += return_code; 210 | } else { 211 | INFO("Lidar selfcheck success."); 212 | } 213 | } 214 | return error_code; 215 | } catch (const std::bad_function_call & e) { 216 | ERROR("bad function:%s", e.what()); 217 | return code_ptr_->GetKeyCode(SYS::KeyCode::kSelfCheckFailed); 218 | } catch (const std::exception & e) { 219 | ERROR("exception:%s", e.what()); 220 | return code_ptr_->GetKeyCode(SYS::KeyCode::kSelfCheckFailed); 221 | } catch (...) { 222 | ERROR("self check unkown exception!"); 223 | return code_ptr_->GetKeyCode(SYS::KeyCode::kSelfCheckFailed); 224 | } 225 | return return_code; 226 | } 227 | 228 | bool cyberdog::sensor::SensorManager::IsStateValid() 229 | { 230 | // check state from behavior tree 231 | return true; 232 | } 233 | 234 | template 235 | bool cyberdog::sensor::SensorManager::SensorOperation( 236 | T elem, uint8_t oper_id) 237 | { 238 | bool result = false; 239 | int32_t code; 240 | switch (oper_id) { 241 | case protocol::srv::SensorOperation::Request::OPR_OPEN: 242 | { 243 | code = elem->Open(); 244 | result = (IS_OK(code) > 0) ? false : true; 245 | break; 246 | } 247 | case protocol::srv::SensorOperation::Request::OPR_START: 248 | { 249 | code = elem->Start(); 250 | result = (IS_OK(code) > 0) ? false : true; 251 | break; 252 | } 253 | case protocol::srv::SensorOperation::Request::OPR_STOP: 254 | { 255 | code = elem->Stop(); 256 | result = (IS_OK(code) > 0) ? false : true; 257 | break; 258 | } 259 | case protocol::srv::SensorOperation::Request::OPR_CLOSE: 260 | { 261 | code = elem->Close(); 262 | result = (IS_OK(code) > 0) ? false : true; 263 | break; 264 | } 265 | default: 266 | break; 267 | } 268 | return result; 269 | } 270 | 271 | int32_t cyberdog::sensor::SensorManager::OnError() 272 | { 273 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 274 | } 275 | 276 | int32_t cyberdog::sensor::SensorManager::OnLowPower() 277 | { 278 | int32_t return_code = code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 279 | 280 | return_code = this->lidar_->LowPowerOn(); 281 | if (!IS_OK(return_code)) { 282 | ERROR("Lidar set low power fail."); 283 | return return_code; 284 | } else { 285 | INFO("Lidar set low power success."); 286 | } 287 | 288 | return_code = this->ultrasonic_->LowPowerOn(); 289 | if (!IS_OK(return_code)) { 290 | ERROR("Ultrasonic set low power fail."); 291 | return return_code; 292 | } else { 293 | INFO("Ultrasonic set low power success."); 294 | } 295 | 296 | return_code = this->tof_->LowPowerOn(); 297 | if (!IS_OK(return_code)) { 298 | ERROR("Tof set low power fail."); 299 | return return_code; 300 | } else { 301 | INFO("Tof set low power success."); 302 | } 303 | 304 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 305 | } 306 | 307 | int32_t cyberdog::sensor::SensorManager::OnSuspend() 308 | { 309 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 310 | } 311 | 312 | int32_t cyberdog::sensor::SensorManager::OnProtected() 313 | { 314 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 315 | } 316 | 317 | int32_t cyberdog::sensor::SensorManager::OnActive() 318 | { 319 | int32_t return_code = code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 320 | 321 | INFO("SensorManager Running begin"); 322 | auto lidar_on = [&]() 323 | { 324 | return this->lidar_->Start(); 325 | }; 326 | auto ultrasonic_on = [&]() 327 | { 328 | return this->ultrasonic_->LowPowerOff(); 329 | }; 330 | auto tof_on = [&]() 331 | { 332 | return this->tof_->LowPowerOff(); 333 | }; 334 | std::future lidar_future = std::async(std::launch::async, lidar_on); 335 | std::future ultrasonic_future = std::async(std::launch::async, ultrasonic_on); 336 | std::future tof_future = std::async(std::launch::async, tof_on); 337 | 338 | return_code = lidar_future.get(); 339 | if (!IS_OK(return_code)) { 340 | ERROR("Lidar start fail."); 341 | return return_code; 342 | } else { 343 | INFO("Lidar start success."); 344 | } 345 | return_code = ultrasonic_future.get(); 346 | if (!IS_OK(return_code)) { 347 | ERROR("Ultrasonic start fail."); 348 | return return_code; 349 | } else { 350 | INFO("Ultrasonic start success."); 351 | } 352 | 353 | return_code = tof_future.get(); 354 | if (!IS_OK(return_code)) { 355 | ERROR("Tof start fail."); 356 | return return_code; 357 | } else { 358 | INFO("Tof start success."); 359 | } 360 | 361 | // GPS TODO 362 | // if (!this->gps_->Start()) { 363 | // ERROR("Gps start fail."); 364 | // return code_ptr_->GetKeyCode(SYS::KeyCode::kFailed); 365 | // } 366 | // INFO("Gps start success."); 367 | 368 | INFO("Sensor manager start success."); 369 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 370 | } 371 | 372 | int32_t cyberdog::sensor::SensorManager::OnDeActive() 373 | { 374 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 375 | } 376 | 377 | int32_t cyberdog::sensor::SensorManager::OnSetUp() 378 | { 379 | int32_t return_code; 380 | INFO("sensor on setup"); 381 | this->node_ptr_->declare_parameter("simulator", std::vector{}); 382 | this->node_ptr_->get_parameter("simulator", this->simulator_); 383 | auto is_simulator = [this](std::string sensor_name) -> bool 384 | { 385 | return static_cast(std::find( 386 | this->simulator_.begin(), this->simulator_.end(), 387 | sensor_name) != this->simulator_.end()); 388 | }; 389 | INFO("gps is_simulator %d", is_simulator("gps")); 390 | INFO("ultrasonic_ is_simulator %d", is_simulator("ultrasonic")); 391 | INFO("tof_ is_simulator %d", is_simulator("tof")); 392 | INFO("lidar_ is_simulator %d", is_simulator("lidar")); 393 | auto IsJump = [&](std::string name, std::string step) 394 | { 395 | if (sensor_self_check_ptr->IsJump(name)) { 396 | INFO("Jump %s at %s error.", name.c_str(), step.c_str()); 397 | return true; 398 | } else { 399 | return false; 400 | } 401 | }; 402 | 403 | return_code = ultrasonic_->Init(is_simulator("ultrasonic")); 404 | if (IS_OK(return_code)) { 405 | return_code = ultrasonic_->Open(); 406 | if (!IS_OK(return_code)) { 407 | if (!IsJump("ultrasonic", "open")) { 408 | return return_code; 409 | } 410 | } 411 | } else { 412 | if (!IsJump("ultrasonic", "init")) { 413 | return return_code; 414 | } 415 | } 416 | 417 | return_code = tof_->Init(is_simulator("tof")); 418 | if (IS_OK(return_code)) { 419 | return_code = tof_->Open(); 420 | if (!IS_OK(return_code)) { 421 | if (!IsJump("tof", "open")) { 422 | return return_code; 423 | } 424 | } 425 | } else { 426 | if (!IsJump("tof", "init")) { 427 | return return_code; 428 | } 429 | } 430 | 431 | return_code = lidar_->Init(is_simulator("lidar")); 432 | if (IS_OK(return_code)) { 433 | return_code = lidar_->Open(); 434 | if (!IS_OK(return_code)) { 435 | if (!IsJump("lidar", "open")) { 436 | return return_code; 437 | } 438 | } 439 | } else { 440 | if (!IsJump("lidar", "init")) { 441 | return return_code; 442 | } 443 | } 444 | 445 | return_code = gps_->Init(is_simulator("gps")); 446 | if (IS_OK(return_code)) { 447 | return_code = gps_->Open(); 448 | if (!IS_OK(return_code)) { 449 | if (!IsJump("gps", "open")) { 450 | return return_code; 451 | } 452 | } 453 | } else { 454 | if (!IsJump("gps", "init")) { 455 | return return_code; 456 | } 457 | } 458 | 459 | INFO("SensorManager Running begin"); 460 | 461 | return_code = this->lidar_->Start(); 462 | if (!IS_OK(return_code)) { 463 | ERROR("Lidar start fail."); 464 | if (!IsJump("lidar", "start")) { 465 | return return_code; 466 | } 467 | } else { 468 | INFO("Lidar start success."); 469 | } 470 | 471 | return_code = this->gps_->Start(); 472 | if (!IS_OK(return_code)) { 473 | ERROR("Gps start fail."); 474 | if (!IsJump("gps", "start")) { 475 | return return_code; 476 | } 477 | } else { 478 | INFO("Gps start success."); 479 | } 480 | 481 | return_code = this->ultrasonic_->Start(); 482 | if (!IS_OK(return_code)) { 483 | ERROR("Ultrasonic start fail."); 484 | if (!IsJump("ultrasonic", "start")) { 485 | return return_code; 486 | } 487 | } else { 488 | INFO("Ultrasonic start success."); 489 | } 490 | 491 | return_code = this->tof_->Start(); 492 | if (!IS_OK(return_code)) { 493 | ERROR("Tof start fail."); 494 | if (!IsJump("tof", "start")) { 495 | return return_code; 496 | } 497 | } else { 498 | INFO("Tof start success."); 499 | } 500 | 501 | INFO("Sensor manager start success."); 502 | INFO("sensor setup success"); 503 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 504 | } 505 | 506 | int32_t cyberdog::sensor::SensorManager::ONTearDown() 507 | { 508 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 509 | } 510 | 511 | int32_t cyberdog::sensor::SensorManager::OnOTA() 512 | { 513 | return code_ptr_->GetKeyCode(SYS::KeyCode::kOK); 514 | } 515 | 516 | void cyberdog::sensor::SensorManager::gps_payload_callback( 517 | std::shared_ptr msg) 518 | { 519 | gps_publisher_->publish(*msg); 520 | } 521 | 522 | void cyberdog::sensor::SensorManager::lidar_payload_callback( 523 | std::shared_ptr msg_ptr) 524 | { 525 | msg_ptr->header.stamp = this->node_ptr_->now(); 526 | lidar_publisher_->publish(*msg_ptr); 527 | } 528 | 529 | void cyberdog::sensor::SensorManager::ultrasonic_payload_callback( 530 | std::shared_ptr msg) 531 | { 532 | std::unique_lock lock(ultrasonic_lock_); 533 | ultrasonic_publisher_->publish(*msg); 534 | } 535 | 536 | void cyberdog::sensor::SensorManager::SingleTofPayloadCallback( 537 | std::shared_ptr msg) 538 | { 539 | if (msg->header.frame_id == "left_rear_tof" || msg->header.frame_id == "right_rear_tof") { 540 | if (rear_tof_.find(msg->header.frame_id) == head_tof_.end()) { 541 | WARN("rear tof map no name [%s] tof msg", msg->header.frame_id.c_str()); 542 | return; 543 | } 544 | std::unique_lock lock(rear_tof_lock_); 545 | if (msg->header.frame_id == "left_rear_tof") { 546 | rear_tof_.at(msg->header.frame_id) = true; 547 | rear_tof_payload->left_rear = *msg; 548 | rear_tof_payload->left_rear.header.frame_id = "left_rear"; 549 | rear_tof_payload->left_rear.tof_position = protocol::msg::SingleTofPayload::LEFT_REAR; 550 | } else { 551 | rear_tof_.at(msg->header.frame_id) = true; 552 | rear_tof_payload->right_rear = *msg; 553 | rear_tof_payload->right_rear.header.frame_id = "right_rear"; 554 | rear_tof_payload->right_rear.tof_position = protocol::msg::SingleTofPayload::RIGHT_REAR; 555 | } 556 | if (rear_tof_.at("left_rear_tof") && rear_tof_.at("right_rear_tof")) { 557 | rear_tof_publisher_->publish(*rear_tof_payload); 558 | rear_tof_.at("left_rear_tof") = false; 559 | rear_tof_.at("right_rear_tof") = false; 560 | } 561 | } else if (msg->header.frame_id == "left_head_tof" || msg->header.frame_id == "right_head_tof") { 562 | if (head_tof_.find(msg->header.frame_id) == head_tof_.end()) { 563 | WARN("head tof map no name [%s] tof msg", msg->header.frame_id.c_str()); 564 | return; 565 | } 566 | std::unique_lock lock(rear_tof_lock_); 567 | if (msg->header.frame_id == "left_head_tof") { 568 | head_tof_.at(msg->header.frame_id) = true; 569 | head_tof_payload->left_head = *msg; 570 | head_tof_payload->left_head.header.frame_id = "left_head"; 571 | head_tof_payload->left_head.tof_position = protocol::msg::SingleTofPayload::LEFT_HEAD; 572 | } else { 573 | head_tof_.at(msg->header.frame_id) = true; 574 | head_tof_payload->right_head = *msg; 575 | head_tof_payload->right_head.header.frame_id = "right_head"; 576 | head_tof_payload->right_head.tof_position = protocol::msg::SingleTofPayload::RIGHT_HEAD; 577 | } 578 | if (head_tof_.at("left_head_tof") && head_tof_.at("right_head_tof")) { 579 | head_tof_publisher_->publish(*head_tof_payload); 580 | head_tof_.at("left_head_tof") = false; 581 | head_tof_.at("right_head_tof") = false; 582 | } 583 | } 584 | } 585 | void cyberdog::sensor::SensorManager::sensor_operation( 586 | const protocol::srv::SensorOperation::Request::SharedPtr request, 587 | protocol::srv::SensorOperation::Response::SharedPtr response) 588 | { 589 | auto sensor_tuple = std::make_tuple(lidar_, ultrasonic_, tof_, gps_); 590 | response->success = true; 591 | switch (request->sensor_id) { 592 | case protocol::srv::SensorOperation::Request::ID_ALL: 593 | { 594 | response->success &= 595 | SensorOperation(sensor_tuple))>( 597 | std::get(sensor_tuple), 599 | request->operation); 600 | response->success &= 601 | SensorOperation(sensor_tuple))>( 603 | std::get(sensor_tuple), 605 | request->operation); 606 | response->success &= 607 | SensorOperation(sensor_tuple))>( 609 | std::get(sensor_tuple), 611 | request->operation); 612 | response->success &= 613 | SensorOperation(sensor_tuple))>( 615 | std::get(sensor_tuple), 617 | request->operation); 618 | } 619 | break; 620 | 621 | case protocol::srv::SensorOperation::Request::ID_LIDAR: 622 | { 623 | response->success = 624 | SensorOperation(sensor_tuple))>( 626 | std::get(sensor_tuple), 628 | request->operation); 629 | } 630 | break; 631 | 632 | case protocol::srv::SensorOperation::Request::ID_ULTRA: 633 | { 634 | response->success = 635 | SensorOperation(sensor_tuple))>( 637 | std::get(sensor_tuple), 639 | request->operation); 640 | } 641 | break; 642 | 643 | case protocol::srv::SensorOperation::Request::ID_TOF: 644 | { 645 | response->success = 646 | SensorOperation(sensor_tuple))>( 648 | std::get(sensor_tuple), 650 | request->operation); 651 | } 652 | break; 653 | 654 | case protocol::srv::SensorOperation::Request::ID_GPS: 655 | { 656 | response->success = 657 | SensorOperation(sensor_tuple))>( 659 | std::get(sensor_tuple), 661 | request->operation); 662 | } 663 | break; 664 | 665 | default: 666 | { 667 | response->success = false; 668 | } 669 | break; 670 | } 671 | } 672 | --------------------------------------------------------------------------------