├── README.md ├── low_power_consumption ├── lib │ └── libpm_client.so ├── package.xml ├── src │ ├── main.cpp │ └── power_consumption_manager.cpp ├── CMakeLists.txt └── include │ └── low_power_consumption │ ├── pm_if.h │ ├── power_consumption_manager.hpp │ └── low_power_consumption.hpp ├── .github ├── ISSUE_TEMPLATE │ ├── custom.md │ ├── feature_request.md │ └── bug_report.md └── workflows │ └── workflow.yml ├── .gitignore ├── manager_base ├── package.xml ├── CMakeLists.txt └── include │ └── manager_base │ └── manager_base.hpp ├── black_box ├── package.xml ├── src │ └── test_black_box.cpp ├── CMakeLists.txt └── include │ └── black_box │ └── black_box.hpp ├── user_info_manager ├── package.xml ├── CMakeLists.txt ├── src │ └── test_inteface_node.cpp ├── include │ └── user_info_manager │ │ └── UserAccountManager.hpp └── test │ └── test.cpp ├── cyberdog_permissions ├── package.xml ├── src │ ├── main.cpp │ └── cyberdog_permission.cpp ├── CMakeLists.txt └── include │ └── cyberdog_permission │ ├── cyberdog_permission.hpp │ └── board_info.hpp ├── cyberdog_manager ├── package.xml ├── src │ ├── main.cpp │ └── cyberdog_manager.cpp ├── CMakeLists.txt └── include │ └── cyberdog_manager │ ├── cyberdog_manager.hpp │ ├── env_contex.hpp │ ├── error_context.hpp │ ├── audio_info.hpp │ ├── touch_info.hpp │ ├── battery_capacity_info.hpp │ ├── heart_context.hpp │ ├── power_brd_info.hpp │ ├── led_info.hpp │ ├── power_consumption_info.hpp │ ├── account_info.hpp │ └── ready_info.hpp └── LICENSE.txt /README.md: -------------------------------------------------------------------------------- 1 | # cyberdog_manager 2 | 3 | -------------------------------------------------------------------------------- /low_power_consumption/lib/libpm_client.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/manager/HEAD/low_power_consumption/lib/libpm_client.so -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /manager_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | manager_base 5 | 0.0.0 6 | TODO: Package description 7 | dukun 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | protocol 14 | cyberdog_system 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /black_box/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | black_box 5 | 0.0.0 6 | TODO: Package description 7 | dukun 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | rclcpp 12 | protocol 13 | cyberdog_common 14 | params 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /user_info_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | user_info_manager 5 | 0.0.0 6 | TODO: Package description 7 | yp 8 | Apache License, Version 2.0 9 | ament_cmake 10 | rclcpp 11 | rapidjson 12 | cyberdog_common 13 | black_box 14 | ament_lint_auto 15 | ament_lint_common 16 | 17 | ament_cmake 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /low_power_consumption/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | low_power_consumption 5 | 0.0.0 6 | TODO: Package description 7 | dukun 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rclcpp 12 | cyberdog_common 13 | std_srvs 14 | protocol 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /cyberdog_permissions/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_permission 5 | 0.0.0 6 | TODO: Package description 7 | liukai 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rclcpp 12 | std_msgs 13 | cyberdog_common 14 | std_srvs 15 | ament_index_cpp 16 | 17 | ament_lint_auto 18 | ament_lint_common 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /cyberdog_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_manager 5 | 0.0.0 6 | TODO: Package description 7 | dukun 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rclcpp 12 | rclcpp_action 13 | cyberdog_system 14 | manager_base 15 | black_box 16 | cyberdog_common 17 | user_info_manager 18 | cyberdog_machine 19 | std_srvs 20 | protocol 21 | low_power_consumption 22 | ament_index_cpp 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /manager_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(manager_base) 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 | # find dependencies 17 | find_package(ament_cmake REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(protocol REQUIRED) 20 | find_package(cyberdog_system REQUIRED) 21 | 22 | 23 | install(DIRECTORY include/ 24 | DESTINATION include/) 25 | 26 | if(BUILD_TESTING) 27 | find_package(ament_lint_auto REQUIRED) 28 | # the following line skips the linter which checks for copyrights 29 | # uncomment the line when a copyright and license is not present in all source files 30 | #set(ament_cmake_copyright_FOUND TRUE) 31 | # the following line skips cpplint (only works in a git repo) 32 | # uncomment the line when this package is not in a git repo 33 | #set(ament_cmake_cpplint_FOUND TRUE) 34 | ament_lint_auto_find_test_dependencies() 35 | endif() 36 | 37 | ament_export_include_directories(include) 38 | 39 | ament_package() 40 | -------------------------------------------------------------------------------- /low_power_consumption/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 "low_power_consumption/power_consumption_manager.hpp" 17 | 18 | int main(int argc, char ** argv) 19 | { 20 | rclcpp::init(argc, argv); 21 | LOGGER_MAIN_INSTANCE("power_consumption_manager"); 22 | std::shared_ptr power_manger = 23 | std::make_shared("cyberdog_poewr_manager"); 24 | if (!power_manger->Init()) { 25 | ERROR("init error"); 26 | } 27 | power_manger->Run(); 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /.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 cyberdog_manager manager_base black_box user_info_manager \ 28 | && colcon test --event-handlers console_cohesion+ --return-code-on-test-failure --packages-select cyberdog_manager manager_base black_box user_info_manager" 29 | # colcon build test 30 | -------------------------------------------------------------------------------- /cyberdog_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 | #include 15 | #include "cyberdog_manager/cyberdog_manager.hpp" 16 | #include "manager_base/manager_base.hpp" 17 | #include "cyberdog_common/cyberdog_log.hpp" 18 | 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | LOGGER_MAIN_INSTANCE("cyberdog_manager"); 24 | 25 | std::shared_ptr manager = 26 | std::make_shared("cyberdog_manager"); 27 | std::thread t([&]() { 28 | if (!manager->Init()) { 29 | ERROR("init error"); 30 | } 31 | }); 32 | t.detach(); 33 | manager->Run(); 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /cyberdog_permissions/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 | #include 15 | #include "cyberdog_permission/cyberdog_permission.hpp" 16 | #include "cyberdog_common/cyberdog_log.hpp" 17 | 18 | 19 | int main(int argc, char ** argv) 20 | { 21 | LOGGER_MAIN_INSTANCE(NODE_NAME); 22 | rclcpp::init(argc, argv); 23 | auto permission_node = 24 | std::make_shared(); 25 | try { 26 | rclcpp::spin(permission_node); 27 | } catch (rclcpp::exceptions::RCLError & e) { 28 | ERROR( 29 | "node spin rcl error exception:(line:%d,file:%d,messgae:%s[%s])", 30 | e.line, e.file, e.message, e.formatted_message); 31 | } catch (const std::exception & e) { 32 | std::cerr << e.what() << '\n'; 33 | } catch (...) { 34 | ERROR("node spin unkown exception"); 35 | } 36 | rclcpp::shutdown(); 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /cyberdog_permissions/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_permission) 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 | find_package(ament_cmake REQUIRED) 17 | find_package(rclcpp REQUIRED) 18 | find_package(std_msgs REQUIRED) 19 | find_package(std_srvs REQUIRED) 20 | find_package(cyberdog_common REQUIRED) 21 | find_package(ament_index_cpp REQUIRED) 22 | 23 | include_directories(include) 24 | 25 | add_executable(${PROJECT_NAME} src/cyberdog_permission.cpp src/main.cpp) 26 | 27 | ament_target_dependencies( ${PROJECT_NAME} 28 | rclcpp 29 | std_msgs 30 | std_srvs 31 | cyberdog_common 32 | ament_index_cpp) 33 | 34 | install(TARGETS ${PROJECT_NAME} 35 | DESTINATION lib/${PROJECT_NAME}) 36 | 37 | if(BUILD_TESTING) 38 | find_package(ament_lint_auto REQUIRED) 39 | # the following line skips the linter which checks for copyrights 40 | # uncomment the line when a copyright and license is not present in all source files 41 | #set(ament_cmake_copyright_FOUND TRUE) 42 | # the following line skips cpplint (only works in a git repo) 43 | # uncomment the line when this package is not in a git repo 44 | #set(ament_cmake_cpplint_FOUND TRUE) 45 | ament_lint_auto_find_test_dependencies() 46 | endif() 47 | 48 | 49 | ament_package() 50 | -------------------------------------------------------------------------------- /user_info_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(user_info_manager) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic -Wno-class-memaccess) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(rapidjson REQUIRED) 12 | find_package(cyberdog_common REQUIRED) 13 | find_package(black_box REQUIRED) 14 | 15 | set(dependencies 16 | cyberdog_common 17 | rapidjson 18 | rclcpp 19 | black_box 20 | ) 21 | 22 | if(BUILD_TESTING) 23 | find_package(ament_lint_auto REQUIRED) 24 | ament_lint_auto_find_test_dependencies() 25 | endif() 26 | 27 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) 28 | include_directories(include) 29 | 30 | add_executable(test_node src/test_inteface_node.cpp) 31 | add_executable(test_node2 test/test.cpp) 32 | 33 | #target_compile_definitions(test_node PRIVATE BenchmarkPath="${CMAKE_INSTALL_PREFIX}/lib/cyberdog_common") 34 | ament_target_dependencies(test_node rclcpp rapidjson cyberdog_common black_box) 35 | ament_target_dependencies(test_node2 rclcpp rapidjson cyberdog_common black_box) 36 | 37 | target_link_libraries(test_node ${cyberdog_log_LIBRARIES}) 38 | install( 39 | TARGETS test_node test_node2 40 | DESTINATION lib/${PROJECT_NAME} 41 | ) 42 | install( 43 | DIRECTORY include/ 44 | DESTINATION include/ 45 | ) 46 | ament_export_include_directories(include) 47 | ament_export_dependencies(${dependencies}) 48 | ament_package() 49 | 50 | -------------------------------------------------------------------------------- /low_power_consumption/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(low_power_consumption) 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 | # find dependencies 17 | find_package(ament_cmake REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(protocol REQUIRED) 20 | find_package(std_srvs REQUIRED) 21 | find_package(cyberdog_common REQUIRED) 22 | 23 | include_directories(include) 24 | 25 | install( 26 | DIRECTORY include/ 27 | DESTINATION include/ 28 | ) 29 | 30 | # install( 31 | # DIRECTORY lib/ 32 | # DESTINATION lib/ 33 | # ) 34 | add_executable(${PROJECT_NAME} src/power_consumption_manager.cpp src/main.cpp) 35 | ament_target_dependencies( ${PROJECT_NAME} 36 | rclcpp 37 | protocol 38 | cyberdog_common 39 | std_srvs) 40 | 41 | install(TARGETS ${PROJECT_NAME} 42 | DESTINATION lib/${PROJECT_NAME}) 43 | 44 | if(BUILD_TESTING) 45 | find_package(ament_lint_auto REQUIRED) 46 | # the following line skips the linter which checks for copyrights 47 | # uncomment the line when a copyright and license is not present in all source files 48 | #set(ament_cmake_copyright_FOUND TRUE) 49 | # the following line skips cpplint (only works in a git repo) 50 | # uncomment the line when this package is not in a git repo 51 | #set(ament_cmake_cpplint_FOUND TRUE) 52 | ament_lint_auto_find_test_dependencies() 53 | endif() 54 | 55 | 56 | ament_package() 57 | -------------------------------------------------------------------------------- /black_box/src/test_black_box.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 | int main(int argc, char * argv[]) 22 | { 23 | rclcpp::init(argc, argv); 24 | LOGGER_MAIN_INSTANCE("Global_Name"); 25 | auto node = rclcpp::Node::make_shared("test"); 26 | cyberdog::manager::BlackBox test_data; 27 | test_data.AddUser("xiaomi"); 28 | test_data.AddUser("xiaoxiao"); 29 | test_data.ModifyUserName("xiaomi", "mimi"); 30 | int filed_number; 31 | test_data.GetDataBaseList(filed_number); 32 | INFO("the filed number is %d \n", filed_number); 33 | auto black_box_ptr = std::make_shared(node); 34 | auto result = black_box_ptr->Init(); 35 | INFO("init: %d", result); 36 | rclcpp::executors::SingleThreadedExecutor exec; 37 | exec.add_node(node); 38 | exec.spin(); 39 | rclcpp::shutdown(); 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /user_info_manager/src/test_inteface_node.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 "rclcpp/rclcpp.hpp" 18 | #include "cyberdog_common/cyberdog_log.hpp" 19 | #include "user_info_manager/UserAccountManager.hpp" 20 | #include "vector" 21 | class Test_node : public rclcpp::Node 22 | { 23 | public: 24 | explicit Test_node(std::string name) 25 | : Node(name) 26 | { 27 | RCLCPP_INFO(this->get_logger(), "node name is %s.", name.c_str()); 28 | INFO("enter test!!!!"); 29 | cyberdog::common::CyberdogAccountManager account_manager; 30 | // account_manager.AddMember("ding"); 31 | // account_manager.ModifyUserInformation("fff",100,0); 32 | // int result[2]; 33 | // account_manager.SearchUser("ding",result); 34 | std::vector vectorUser; 35 | account_manager.SearAllUser(vectorUser); 36 | // account_manager.DeleteVoice("sss"); 37 | // account_manager.DeleteFace("ding"); 38 | // account_manager.DeleteUserInformation("eee"); 39 | } 40 | }; 41 | 42 | int main(int argc, char ** argv) 43 | { 44 | rclcpp::init(argc, argv); 45 | auto node = std::make_shared("test_node"); 46 | rclcpp::spin(node); 47 | rclcpp::shutdown(); 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /low_power_consumption/include/low_power_consumption/pm_if.h: -------------------------------------------------------------------------------- 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 LOW_POWER_CONSUMPTION__PM_IF_H_ 15 | #define LOW_POWER_CONSUMPTION__PM_IF_H_ 16 | 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | 24 | enum PM_DEV 25 | { 26 | // bit 0-7 camera 27 | PM_CAM_REALSNS = 1 << 0, // 深度相机 28 | PM_CAM_AI = 1 << 1, // AI相机组件 29 | PM_CAM_FISH = 1<<2, // 鱼眼相机组件 30 | PM_CAM_ALL = 0Xff, 31 | 32 | // bit 8-11 motion 33 | PM_MOTION = 1 << 8, // 运动组件 34 | PM_MOTOR = 1 << 9, //电机断电 35 | // other 36 | PM_GPS = 1 << 12, // GPS 组件 37 | PM_LIDAR = 1 << 13, // 激光雷达组件, 38 | PM_TOF = 1 << 14, // TOF 组件 39 | PM_ULTRA = 1 << 15, // 超声波组件 40 | PM_ALL = 0xffffffff, 41 | PM_ALL_NO_MOTION = PM_ALL & (~PM_MOTION) & (~PM_MOTOR), 42 | PM_ALL_NO_TOF = PM_ALL & (~PM_TOF) & (~PM_MOTOR), 43 | PM_NO_MOTION_TOF = PM_ALL & (~PM_MOTION) & (~PM_TOF) & (~PM_MOTOR) 44 | }; 45 | 46 | int PmRequest(unsigned int devs, unsigned int * err); 47 | int PmRelease(unsigned int devs, unsigned int * err); 48 | int PmQuery(unsigned int dev); 49 | 50 | 51 | enum PM_SYS 52 | { 53 | PM_SYS_SLEEP = 0, 54 | PM_SYS_REBOOT, 55 | PM_SYS_SHUTDOWN, 56 | }; 57 | 58 | int PmSysRequest(int op); 59 | 60 | 61 | #ifdef __cplusplus 62 | } 63 | #endif 64 | 65 | 66 | #endif // LOW_POWER_CONSUMPTION__PM_IF_H_ 67 | -------------------------------------------------------------------------------- /cyberdog_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_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 | find_package(ament_cmake REQUIRED) 17 | find_package(rclcpp REQUIRED) 18 | find_package(rclcpp_action REQUIRED) 19 | find_package(manager_base REQUIRED) 20 | find_package(protocol REQUIRED) 21 | find_package(cyberdog_system REQUIRED) 22 | find_package(cyberdog_common REQUIRED) 23 | find_package(black_box REQUIRED) 24 | find_package(cyberdog_machine REQUIRED) 25 | find_package(std_srvs REQUIRED) 26 | find_package(protocol REQUIRED) 27 | find_package(low_power_consumption REQUIRED) 28 | find_package(ament_index_cpp REQUIRED) 29 | find_package(user_info_manager REQUIRED) 30 | 31 | include_directories(include) 32 | 33 | add_executable(${PROJECT_NAME} src/cyberdog_manager.cpp src/main.cpp) 34 | 35 | # MESSAGE('--------------${CMAKE_INSTALL_PREFIX}------------------') 36 | 37 | ament_target_dependencies( ${PROJECT_NAME} 38 | rclcpp 39 | rclcpp_action 40 | protocol 41 | manager_base 42 | cyberdog_system 43 | cyberdog_common 44 | black_box 45 | cyberdog_machine 46 | std_srvs 47 | protocol 48 | low_power_consumption 49 | ament_index_cpp 50 | user_info_manager ) 51 | 52 | install(TARGETS ${PROJECT_NAME} 53 | DESTINATION lib/${PROJECT_NAME}) 54 | 55 | if(BUILD_TESTING) 56 | find_package(ament_lint_auto REQUIRED) 57 | # the following line skips the linter which checks for copyrights 58 | # uncomment the line when a copyright and license is not present in all source files 59 | #set(ament_cmake_copyright_FOUND TRUE) 60 | # the following line skips cpplint (only works in a git repo) 61 | # uncomment the line when this package is not in a git repo 62 | #set(ament_cmake_cpplint_FOUND TRUE) 63 | ament_lint_auto_find_test_dependencies() 64 | endif() 65 | 66 | 67 | ament_package() 68 | -------------------------------------------------------------------------------- /cyberdog_permissions/include/cyberdog_permission/cyberdog_permission.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 CYBERDOG_PERMISSION__CYBERDOG_PERMISSION_HPP_ 15 | #define CYBERDOG_PERMISSION__CYBERDOG_PERMISSION_HPP_ 16 | #include 17 | #include 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "cyberdog_permission/board_info.hpp" 20 | #include "std_srvs/srv/trigger.hpp" 21 | #include "std_msgs/msg/string.hpp" 22 | 23 | #define NODE_NAME "cyberdog_permission" 24 | 25 | namespace cyberdog 26 | { 27 | namespace manager 28 | { 29 | class CyberdogPermission final : public rclcpp::Node 30 | { 31 | public: 32 | CyberdogPermission(); 33 | 34 | private: 35 | void SnCallback( 36 | const std_srvs::srv::Trigger::Request::SharedPtr request, 37 | std_srvs::srv::Trigger::Response::SharedPtr response); 38 | 39 | void RealsenseRecovery( 40 | const std_srvs::srv::Trigger::Request::SharedPtr request, 41 | std_srvs::srv::Trigger::Response::SharedPtr response); 42 | 43 | private: 44 | bool Shell(const std::string & _command, int & _code, std::string & _message); 45 | 46 | private: 47 | std::string cyberdog_sn; 48 | // std::thread sn_thread; 49 | rclcpp::CallbackGroup::SharedPtr callback_group_; 50 | rclcpp::Service::SharedPtr sn_srv_; 51 | rclcpp::Service::SharedPtr realsense_recovery_srv_; 52 | rclcpp::Publisher::SharedPtr sn_pub_; 53 | }; // class CyberdogPermission 54 | } // namespace manager 55 | } // namespace cyberdog 56 | 57 | #endif // CYBERDOG_PERMISSION__CYBERDOG_PERMISSION_HPP_ 58 | -------------------------------------------------------------------------------- /black_box/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(black_box) 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 | # find dependencies 17 | find_package(Boost COMPONENTS system filesystem REQUIRED) 18 | find_package(ament_cmake REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(protocol REQUIRED) 21 | find_package(sqlite3_vendor REQUIRED) 22 | find_package(SQLite3 REQUIRED) 23 | find_package(cyberdog_common REQUIRED) 24 | find_package(params REQUIRED) 25 | # uncomment the following section in order to fill in 26 | # further dependencies manually. 27 | # find_package( REQUIRED) 28 | 29 | include_directories(include) 30 | 31 | add_library(black_box SHARED src/black_box.cpp) 32 | target_link_libraries(black_box ${SQLite3_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY}) 33 | ament_target_dependencies(black_box rclcpp protocol cyberdog_common params) 34 | ament_target_dependencies(black_box rclcpp cyberdog_common params) 35 | # target_include_directories(black_box 36 | # PUBLIC 37 | # $ 38 | # $) 39 | 40 | add_executable(test_black_box src/test_black_box.cpp) 41 | target_link_libraries(test_black_box black_box) 42 | ament_target_dependencies(test_black_box rclcpp) 43 | # target_include_directories(test_black_box 44 | # PUBLIC 45 | # $ 46 | # $) 47 | 48 | if(BUILD_TESTING) 49 | find_package(ament_lint_auto REQUIRED) 50 | ament_lint_auto_find_test_dependencies() 51 | endif() 52 | 53 | install(DIRECTORY 54 | include/ 55 | DESTINATION include) 56 | 57 | install(TARGETS black_box test_black_box 58 | LIBRARY DESTINATION lib 59 | RUNTIME DESTINATION lib/${PROJECT_NAME} 60 | ) 61 | ament_export_include_directories(include) 62 | ament_export_dependencies(sqlite3_vendor SQLite3 cyberdog_common) 63 | ament_export_libraries(black_box) 64 | ament_package() 65 | -------------------------------------------------------------------------------- /low_power_consumption/include/low_power_consumption/power_consumption_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 | 15 | #ifndef LOW_POWER_CONSUMPTION__POWER_CONSUMPTION_MANAGER_HPP_ 16 | #define LOW_POWER_CONSUMPTION__POWER_CONSUMPTION_MANAGER_HPP_ 17 | 18 | #include 19 | #include 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "protocol/msg/motion_status.hpp" 22 | #include "low_power_consumption/low_power_consumption.hpp" 23 | #include "cyberdog_common/cyberdog_log.hpp" 24 | 25 | namespace cyberdog 26 | { 27 | namespace manager 28 | { 29 | class PowerConsumptionManager 30 | { 31 | public: 32 | explicit PowerConsumptionManager(const std::string & name); 33 | ~PowerConsumptionManager(); 34 | 35 | bool low_power_consumption_set(); 36 | bool nomal_power_consumption_set(); 37 | 38 | bool Init(); 39 | void Run(); 40 | 41 | private: 42 | void sub_mostion_status_callback(const protocol::msg::MotionStatus msg); 43 | bool send_power_manager_request(const bool msg); 44 | void EnterLowPower( 45 | const std_srvs::srv::SetBool::Request::SharedPtr request, 46 | std_srvs::srv::SetBool::Response::SharedPtr response); 47 | rclcpp::Node::SharedPtr node_sub_motion_ptr_ {nullptr}; 48 | rclcpp::Node::SharedPtr node_power_consump_ptr_ {nullptr}; 49 | rclcpp::Subscription::SharedPtr motion_status_sub_ {nullptr}; 50 | rclcpp::Client::SharedPtr power_comsumption_client_ {nullptr}; 51 | rclcpp::Service::SharedPtr power_consumption_manager_srv_ {nullptr}; 52 | std::unique_ptr lpc_ptr_ {nullptr}; 53 | rclcpp::executors::MultiThreadedExecutor executor_; 54 | }; // class PowerConsumptionManager 55 | 56 | } // namespace manager 57 | } // namespace cyberdog 58 | 59 | #endif // LOW_POWER_CONSUMPTION__POWER_CONSUMPTION_MANAGER_HPP_ 60 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/cyberdog_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 CYBERDOG_MANAGER__CYBERDOG_MANAGER_HPP_ 15 | #define CYBERDOG_MANAGER__CYBERDOG_MANAGER_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "manager_base/manager_base.hpp" 24 | #include "black_box/black_box.hpp" 25 | #include "protocol/srv/audio_volume_get.hpp" 26 | #include "protocol/srv/audio_execute.hpp" 27 | #include "protocol/srv/motor_temp.hpp" 28 | #include "std_msgs/msg/string.hpp" 29 | #include "std_msgs/msg/bool.hpp" 30 | #include "std_srvs/srv/trigger.hpp" 31 | #include "std_srvs/srv/set_bool.hpp" 32 | #include "cyberdog_manager/env_contex.hpp" 33 | #include "cyberdog_manager/query_info.hpp" 34 | #include "cyberdog_manager/account_info.hpp" 35 | #include "cyberdog_manager/power_consumption_info.hpp" 36 | #include "cyberdog_manager/ready_info.hpp" 37 | #include "cyberdog_manager/battery_capacity_info.hpp" 38 | #include "cyberdog_manager/touch_info.hpp" 39 | #include "cyberdog_manager/audio_info.hpp" 40 | #include "cyberdog_manager/heart_context.hpp" 41 | #include "cyberdog_manager/error_context.hpp" 42 | #include "cyberdog_manager/machine_state_switch_context.hpp" 43 | #include "cyberdog_manager/led_info.hpp" 44 | #include "cyberdog_manager/power_brd_info.hpp" 45 | 46 | namespace cyberdog 47 | { 48 | namespace manager 49 | { 50 | 51 | class CyberdogManager /* : public ManagerBase */ 52 | { 53 | public: 54 | explicit CyberdogManager(const std::string & name); 55 | ~CyberdogManager(); 56 | 57 | void Config(); 58 | bool Init(); 59 | void Run(); 60 | 61 | private: 62 | void OnActive(); 63 | bool SetState(int8_t state, std::string json_data = "{}"); 64 | 65 | private: 66 | std::string name_; 67 | std::map selfcheck_status_; 68 | rclcpp::Node::SharedPtr node_ptr_ {nullptr}; 69 | rclcpp::executors::MultiThreadedExecutor executor_; 70 | std::unique_ptr env_node_ptr_ {nullptr}; 71 | std::unique_ptr query_node_ptr_ {nullptr}; 72 | std::unique_ptr account_node_ptr_ {nullptr}; 73 | std::shared_ptr power_consumption_node_ptr {nullptr}; 74 | std::unique_ptr ready_node_ptr {nullptr}; 75 | std::unique_ptr bcin_node_ptr {nullptr}; 76 | std::unique_ptr touch_node_ptr {nullptr}; 77 | std::shared_ptr audio_node_ptr {nullptr}; 78 | std::shared_ptr led_node_ptr {nullptr}; 79 | std::unique_ptr heart_beat_ptr_ {nullptr}; 80 | std::unique_ptr error_context_ptr_ {nullptr}; 81 | std::shared_ptr mssc_context_ptr_ {nullptr}; 82 | std::shared_ptr black_box_ptr_ {nullptr}; 83 | std::shared_ptr power_brd_node_ptr_ {nullptr}; 84 | }; // class CyberdogManager 85 | } // namespace manager 86 | } // namespace cyberdog 87 | 88 | #endif // CYBERDOG_MANAGER__CYBERDOG_MANAGER_HPP_ 89 | -------------------------------------------------------------------------------- /black_box/include/black_box/black_box.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 BLACK_BOX__BLACK_BOX_HPP_ 15 | #define BLACK_BOX__BLACK_BOX_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | namespace cyberdog 31 | { 32 | namespace manager 33 | { 34 | 35 | struct Table 36 | { 37 | std::string name; 38 | std::string key; 39 | std::vector> fields; 40 | }; 41 | 42 | struct MemberInformaion 43 | { 44 | int id; 45 | std::string name; 46 | int voiceStatus; 47 | int faceStatus; 48 | }; 49 | 50 | class BlackBox final 51 | { 52 | using TouchStatusMsg = protocol::msg::TouchStatus; 53 | 54 | public: 55 | explicit BlackBox(rclcpp::Node::SharedPtr node_ptr); 56 | BlackBox() {} 57 | BlackBox(const BlackBox &) = delete; 58 | BlackBox & operator=(const BlackBox &) = delete; 59 | ~BlackBox() {} 60 | 61 | bool Config(); 62 | bool Init(); 63 | 64 | private: 65 | /* ros implementation, node and subscribers */ 66 | rclcpp::Node::SharedPtr node_ptr_ {nullptr}; 67 | rclcpp::Subscription::SharedPtr touch_status_sub_ {nullptr}; 68 | sqlite3 * ppDb_; 69 | std::string DB_URL_; 70 | std::stringstream errmsg_; 71 | std::vector general_subs_; 72 | std::vector topic_names_; 73 | std::map tables_; 74 | std::map topics_map_; 75 | std::string CREAT_TABLES_SQL_; 76 | std::mutex query_mutex_; 77 | size_t db_size_threshold_; 78 | 79 | private: 80 | void GeneralMsgCallback( 81 | std::shared_ptr msg, 82 | const std::string topic_type); 83 | bool ConnetDB(std::string & DB_URL); 84 | bool InsertTouchStatus(const TouchStatusMsg & msg, const std::string & topic_name); 85 | void RollOverDB(); 86 | std::string GetTime(); 87 | std::string filename_ = "/opt/ros2/cyberdog/share/params/toml_config/manager"; 88 | std::string item = "cyberdog"; 89 | 90 | public: 91 | // new function 92 | bool write(const std::string & name); 93 | bool AddUser(const std::string & name); 94 | bool DeleteUser(const std::string & name); 95 | bool SearchUser(std::vector & UserVector); 96 | bool SearchSingleUser(const std::string & name, int * result); 97 | bool ModifyUser(const std::string & name, int status, int newStatus); 98 | bool ModifyUserName(const std::string name, const std::string new_name); 99 | bool HasUser(const std::string & name); 100 | bool DataBaseExit(const std::string DB_path); 101 | bool GetDataBaseList(int & filed_number); 102 | 103 | bool ModifyUnlockStatus(const std::string & details, int status); 104 | bool readUnlockStatus(int * result); 105 | bool CreateUnlockStatusDB(); 106 | }; // class BlackBox 107 | } // namespace manager 108 | } // namespace cyberdog 109 | 110 | #endif // BLACK_BOX__BLACK_BOX_HPP_ 111 | -------------------------------------------------------------------------------- /low_power_consumption/include/low_power_consumption/low_power_consumption.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 LOW_POWER_CONSUMPTION__LOW_POWER_CONSUMPTION_HPP_ 15 | #define LOW_POWER_CONSUMPTION__LOW_POWER_CONSUMPTION_HPP_ 16 | 17 | #include 18 | #include "low_power_consumption/pm_if.h" 19 | #include "cyberdog_common/cyberdog_log.hpp" 20 | 21 | namespace cyberdog 22 | { 23 | namespace manager 24 | { 25 | 26 | typedef int (* Func_PmRequest)(unsigned int devs, unsigned int * err); 27 | typedef int (* Func_PmRelease)(unsigned int devs, unsigned int * err); 28 | typedef int (* Func_PmQuery)(unsigned int dev); 29 | typedef int (* Func_PmSysRequest)(int op); 30 | 31 | class LowPowerConsumption 32 | { 33 | public: 34 | LowPowerConsumption() 35 | { 36 | Dll_Open(); 37 | } 38 | ~LowPowerConsumption() 39 | { 40 | Dll_Close(); 41 | } 42 | int LpcRequest(PM_DEV devs, unsigned int * err) 43 | { 44 | return Pm_Request(devs, err); 45 | } 46 | int LpcRelease(PM_DEV devs, unsigned int * err) 47 | { 48 | return Pm_Release(devs, err); 49 | } 50 | int LpcQuery(PM_DEV dev) 51 | { 52 | return Pm_Query(dev); 53 | } 54 | int LpcSysRequest(PM_SYS op) 55 | { 56 | return Pm_SysRequest(op); 57 | } 58 | 59 | private: 60 | bool Dll_Open() 61 | { 62 | dll_handle = dlopen(dll_path, RTLD_LAZY); 63 | if (!dll_handle) { 64 | INFO("dlopen get error: %s", dlerror() ); 65 | dll_open = false; 66 | } else { 67 | INFO("dll open."); 68 | dll_open = true; 69 | pm_request = (Func_PmRequest)dlsym(dll_handle, "PmRequest"); 70 | pm_release = (Func_PmRelease)dlsym(dll_handle, "PmRelease"); 71 | pm_query = (Func_PmQuery)dlsym(dll_handle, "PmQuery"); 72 | pm_sys_request = (Func_PmSysRequest)dlsym(dll_handle, "PmSysRequest"); 73 | if (pm_request) { 74 | INFO("pm_request get."); 75 | } 76 | if (pm_release) { 77 | INFO("pm_release get."); 78 | } 79 | if (pm_query) { 80 | INFO("pm_query get."); 81 | } 82 | if (pm_sys_request) { 83 | INFO("pm_sys_request get."); 84 | } 85 | } 86 | return dll_open; 87 | } 88 | void Dll_Close() 89 | { 90 | if (dll_open) { 91 | if (dll_handle) { 92 | dlclose(dll_handle); 93 | } 94 | } 95 | dll_open = false; 96 | } 97 | int Pm_Request(unsigned int devs, unsigned int * err) 98 | { 99 | if (pm_request) { 100 | INFO("Pm_Request enter!----"); 101 | return pm_request(devs, err); 102 | } 103 | return 255; 104 | } 105 | int Pm_Release(unsigned int devs, unsigned int * err) 106 | { 107 | if (pm_release) { 108 | INFO("Pm_Release enter!-----"); 109 | return pm_release(devs, err); 110 | } 111 | return 255; 112 | } 113 | int Pm_Query(unsigned int dev) 114 | { 115 | if (pm_query) { 116 | INFO("Pm_Query enter!"); 117 | return pm_query(dev); 118 | } 119 | return 255; 120 | } 121 | int Pm_SysRequest(int op) 122 | { 123 | if (pm_sys_request) { 124 | return pm_sys_request(op); 125 | } 126 | return 255; 127 | } 128 | 129 | private: 130 | const char * dll_path = "/usr/lib/libpm_client.so"; 131 | bool dll_open {false}; 132 | void * dll_handle {nullptr}; 133 | Func_PmRequest pm_request; 134 | Func_PmRelease pm_release; 135 | Func_PmQuery pm_query; 136 | Func_PmSysRequest pm_sys_request; 137 | }; // class LowPowerConsumption 138 | } // namespace manager 139 | } // namespace cyberdog 140 | 141 | #endif // LOW_POWER_CONSUMPTION__LOW_POWER_CONSUMPTION_HPP_ 142 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/env_contex.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 CYBERDOG_MANAGER__ENV_CONTEX_HPP_ 15 | #define CYBERDOG_MANAGER__ENV_CONTEX_HPP_ 16 | 17 | #include 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "protocol/srv/trigger.hpp" 20 | #include "std_srvs/srv/trigger.hpp" 21 | #include "cyberdog_common/cyberdog_log.hpp" 22 | #include "cyberdog_common/cyberdog_json.hpp" 23 | 24 | using cyberdog::common::CyberdogJson; 25 | using rapidjson::Document; 26 | using rapidjson::kObjectType; 27 | 28 | namespace cyberdog 29 | { 30 | namespace manager 31 | { 32 | 33 | class EnvContex final 34 | { 35 | public: 36 | explicit EnvContex(rclcpp::Node::SharedPtr node_ptr) 37 | : env_contex_node_(node_ptr), environment_contex_("pro") 38 | { 39 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 40 | auto path = local_share_dir + std::string("/toml_config/manager/env.json"); 41 | Document json_document(kObjectType); 42 | auto result = CyberdogJson::ReadJsonFromFile(path, json_document); 43 | if (result) { 44 | std::string env_str; 45 | CyberdogJson::Get(json_document, "environment", env_str); 46 | if (!env_str.empty()) { 47 | INFO("environment:%s.", env_str.c_str()); 48 | environment_contex_ = env_str; 49 | } 50 | } 51 | env_callback_group_ = 52 | env_contex_node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); 53 | 54 | switch_environment_srv_ = 55 | env_contex_node_->create_service( 56 | "set_nx_environment", 57 | std::bind( 58 | &EnvContex::SwitchEnvironmentService, this, std::placeholders::_1, 59 | std::placeholders::_2), 60 | rmw_qos_profile_services_default, env_callback_group_); 61 | get_environment_srv_ = 62 | env_contex_node_->create_service( 63 | "get_nx_environment", 64 | std::bind( 65 | &EnvContex::GetEnvironmentService, this, std::placeholders::_1, 66 | std::placeholders::_2), 67 | rmw_qos_profile_services_default, env_callback_group_); 68 | } 69 | 70 | private: 71 | void SwitchEnvironmentService( 72 | const protocol::srv::Trigger::Request::SharedPtr request, 73 | protocol::srv::Trigger::Response::SharedPtr response) 74 | { 75 | environment_contex_ = request->data; 76 | INFO("switch nx environment:%s", environment_contex_.c_str()); 77 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 78 | auto path = local_share_dir + std::string("/toml_config/manager"); 79 | auto json_file = path + "/env.json"; 80 | Document json_document(kObjectType); 81 | CyberdogJson::ReadJsonFromFile(json_file, json_document); 82 | CyberdogJson::Add(json_document, "environment", environment_contex_); 83 | CyberdogJson::WriteJsonToFile(json_file, json_document); 84 | response->success = true; 85 | } 86 | 87 | void GetEnvironmentService( 88 | const std_srvs::srv::Trigger::Request::SharedPtr, 89 | std_srvs::srv::Trigger::Response::SharedPtr response) 90 | { 91 | response->message = environment_contex_; 92 | response->success = true; 93 | } 94 | 95 | private: 96 | rclcpp::Node::SharedPtr env_contex_node_{nullptr}; 97 | std::string environment_contex_; 98 | rclcpp::CallbackGroup::SharedPtr env_callback_group_; 99 | rclcpp::Service::SharedPtr switch_environment_srv_; 100 | rclcpp::Service::SharedPtr get_environment_srv_; 101 | }; 102 | 103 | } // namespace manager 104 | } // namespace cyberdog 105 | 106 | #endif // CYBERDOG_MANAGER__ENV_CONTEX_HPP_ 107 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/error_context.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 CYBERDOG_MANAGER__ERROR_CONTEXT_HPP_ 15 | #define CYBERDOG_MANAGER__ERROR_CONTEXT_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "std_msgs/msg/string.hpp" 22 | #include "protocol/msg/node_error.hpp" 23 | #include "cyberdog_common/cyberdog_log.hpp" 24 | #include "cyberdog_common/cyberdog_json.hpp" 25 | 26 | using cyberdog::common::CyberdogJson; 27 | using rapidjson::Document; 28 | using rapidjson::kObjectType; 29 | 30 | namespace cyberdog 31 | { 32 | namespace manager 33 | { 34 | struct ErrorRecorder 35 | { 36 | std::string name; 37 | int32_t code; 38 | int32_t counter; 39 | explicit ErrorRecorder(std::string nm = "cyberdog_manager") 40 | : name(nm), 41 | code(0), counter(0) 42 | { 43 | } 44 | }; // struct ErrorRecorder 45 | class ErrorContext final 46 | { 47 | private: 48 | /* data */ 49 | 50 | public: 51 | explicit ErrorContext(std::string node_name) 52 | : name_(node_name), exit_(false) 53 | { 54 | error_report_node_ = rclcpp::Node::make_shared(name_); 55 | error_report_callback_group_ = 56 | error_report_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 57 | rclcpp::SubscriptionOptions sub_options; 58 | sub_options.callback_group = error_report_callback_group_; 59 | node_error_sub_ = 60 | error_report_node_->create_subscription( 61 | "node_errors", rclcpp::SystemDefaultsQoS(), 62 | std::bind( 63 | &ErrorContext::ErrorsCallback, this, 64 | std::placeholders::_1), 65 | sub_options); 66 | rclcpp::PublisherOptions pub_options; 67 | pub_options.callback_group = error_report_callback_group_; 68 | error_pub_ = error_report_node_->create_publisher( 69 | "manager_errors", 70 | rclcpp::SystemDefaultsQoS(), 71 | pub_options 72 | ); 73 | std::thread( 74 | [this]() { 75 | rclcpp::spin(error_report_node_); 76 | }).detach(); 77 | } 78 | ~ErrorContext() 79 | { 80 | exit_ = true; 81 | { 82 | std::lock_guard lk(er_mut_); 83 | er_cond_.notify_one(); 84 | } 85 | if (thread_.joinable()) { 86 | thread_.join(); 87 | } 88 | } 89 | void Init() 90 | { 91 | INFO("error context thread started."); 92 | thread_ = std::thread( 93 | [this]() { 94 | while (rclcpp::ok() && !exit_) { 95 | std::unique_lock lck(er_mut_); 96 | er_cond_.wait(lck, [&] {return !er_vec_.empty() || exit_;}); 97 | if (exit_) { 98 | break; 99 | } 100 | ErrorRecorder er = er_vec_.front(); 101 | er_vec_.pop(); 102 | --er.counter; 103 | if (er.counter < 0) { 104 | er.counter = 0; 105 | er_vec_.push(er); 106 | } else if (er.counter > 0) { 107 | er_vec_.push(er); 108 | } 109 | lck.unlock(); 110 | Document json_info(kObjectType); 111 | CyberdogJson::Add(json_info, "name", er.name); 112 | CyberdogJson::Add(json_info, "code", er.code); 113 | std::string info; 114 | if (!CyberdogJson::Document2String(json_info, info)) { 115 | ERROR("arror context error while encoding to json"); 116 | continue; 117 | } 118 | std_msgs::msg::String msg; 119 | msg.data = info; 120 | error_pub_->publish(msg); 121 | } 122 | }); 123 | } 124 | void NotifyError(ErrorRecorder er) 125 | { 126 | std::lock_guard lk(er_mut_); 127 | er_vec_.push(er); 128 | er_cond_.notify_one(); 129 | } 130 | void ClearError() 131 | { 132 | std::lock_guard lk(er_mut_); 133 | while (!er_vec_.empty()) { 134 | er_vec_.pop(); 135 | } 136 | } 137 | 138 | private: 139 | void ErrorsCallback(const protocol::msg::NodeError::SharedPtr msg) 140 | { 141 | ErrorRecorder er; 142 | er.name = msg->name; 143 | er.code = msg->code; 144 | er.counter = 1; 145 | NotifyError(er); 146 | } 147 | 148 | private: 149 | std::string name_; 150 | rclcpp::Node::SharedPtr error_report_node_; 151 | rclcpp::CallbackGroup::SharedPtr error_report_callback_group_; 152 | bool exit_; 153 | std::thread thread_; 154 | rclcpp::Subscription::SharedPtr node_error_sub_; 155 | rclcpp::Publisher::SharedPtr error_pub_; 156 | std::queue er_vec_; 157 | std::mutex er_mut_; 158 | std::condition_variable er_cond_; 159 | }; 160 | } // namespace manager 161 | } // namespace cyberdog 162 | 163 | #endif // CYBERDOG_MANAGER__ERROR_CONTEXT_HPP_ 164 | -------------------------------------------------------------------------------- /cyberdog_permissions/src/cyberdog_permission.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 | #include 15 | #include "cyberdog_permission/cyberdog_permission.hpp" 16 | #include "cyberdog_common/cyberdog_log.hpp" 17 | #include "cyberdog_common/cyberdog_json.hpp" 18 | #include "ament_index_cpp/get_package_share_directory.hpp" 19 | 20 | #define MAX_SN_SIZE 50 21 | #define LINE_MAX_SIZE 1024 22 | 23 | using cyberdog::common::CyberdogJson; 24 | using rapidjson::Document; 25 | using rapidjson::kObjectType; 26 | 27 | cyberdog::manager::CyberdogPermission::CyberdogPermission() 28 | : Node(NODE_NAME) 29 | { 30 | INFO("start get sn"); 31 | cyberdog_sn = BoardInfo::Get_Sn(); 32 | INFO("sn:%s", cyberdog_sn.c_str()); 33 | if (cyberdog_sn.length() > MAX_SN_SIZE) { 34 | cyberdog_sn.resize(MAX_SN_SIZE); 35 | cyberdog_sn[MAX_SN_SIZE - 1] = '\0'; 36 | } 37 | sn_pub_ = 38 | this->create_publisher( 39 | "dog_sn", 40 | rclcpp::SystemDefaultsQoS()); 41 | std::thread t([this]() { 42 | try { 43 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 44 | auto path = local_share_dir + std::string("/toml_config/manager"); 45 | if (access(path.c_str(), F_OK) != 0) { 46 | std::string cmd = "mkdir -p " + path; 47 | std::system(cmd.c_str()); 48 | cmd = "chmod 777 " + path; 49 | std::system(cmd.c_str()); 50 | } 51 | uint16_t pub_cnt = 0; 52 | while (rclcpp::ok() && (++pub_cnt < 3600)) { 53 | std_msgs::msg::String msg; 54 | msg.data = cyberdog_sn; 55 | sn_pub_->publish(msg); 56 | INFO_MILLSECONDS(3000, "publish sn:%s", cyberdog_sn.c_str()); 57 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 58 | } 59 | } catch (...) { 60 | ERROR("mkdir manager directory error."); 61 | } 62 | }); 63 | t.detach(); 64 | callback_group_ = 65 | this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 66 | sn_srv_ = 67 | this->create_service( 68 | "get_dog_sn", 69 | std::bind(&CyberdogPermission::SnCallback, this, std::placeholders::_1, std::placeholders::_2), 70 | rmw_qos_profile_services_default, 71 | callback_group_); 72 | realsense_recovery_srv_ = 73 | this->create_service( 74 | "camera/realsense_recovery", 75 | std::bind( 76 | &CyberdogPermission::RealsenseRecovery, 77 | this, std::placeholders::_1, std::placeholders::_2), 78 | rmw_qos_profile_services_default); 79 | } 80 | 81 | void cyberdog::manager::CyberdogPermission::SnCallback( 82 | const std_srvs::srv::Trigger::Request::SharedPtr, 83 | std_srvs::srv::Trigger::Response::SharedPtr response) 84 | { 85 | response->success = true; 86 | response->message = cyberdog_sn; 87 | INFO("reponse:get dog sn:--%s", response->message.c_str()); 88 | } 89 | 90 | void cyberdog::manager::CyberdogPermission::RealsenseRecovery( 91 | const std_srvs::srv::Trigger::Request::SharedPtr request, 92 | std_srvs::srv::Trigger::Response::SharedPtr response) 93 | { 94 | std::string cmd = "realsense_recovery.sh"; 95 | int32_t code; 96 | std::string message; 97 | bool ok = Shell(cmd, code, message); 98 | INFO("shell message:%s", message.c_str()); 99 | if (code == 0) { 100 | response->success = true; 101 | response->message = "ok"; 102 | } else { 103 | response->success = false; 104 | response->message = "fail"; 105 | } 106 | } 107 | 108 | bool cyberdog::manager::CyberdogPermission::Shell( 109 | const std::string & _command, int & _code, std::string & _message) 110 | { 111 | try { 112 | INFO("Shell: %s", _command.c_str()); 113 | _message = ""; 114 | if (!_command.empty()) { 115 | std::string _code_cmd = _command + "; echo $?"; // | xargs echo 116 | std::string _code_str = ""; 117 | FILE * fstream_ptr = nullptr; 118 | fstream_ptr = popen(_code_cmd.c_str(), "r"); 119 | if (fstream_ptr != nullptr) { 120 | char buffer[LINE_MAX_SIZE]; 121 | while (fgets(buffer, LINE_MAX_SIZE, fstream_ptr) != nullptr) { 122 | _code_str = buffer; 123 | memset(buffer, '\0', sizeof(buffer)); 124 | _message += _code_str; 125 | } 126 | pclose(fstream_ptr); 127 | fstream_ptr = nullptr; 128 | _code = std::atoi(_code_str.c_str()); 129 | if (_code == static_cast(0)) { 130 | return true; 131 | } else { 132 | return false; 133 | } 134 | } else { 135 | _code = -1; 136 | _message = "Canot shell command popen.\n - command : " + _command + 137 | "\n - error : " + strerror(errno); 138 | } 139 | } else { 140 | _code = -2; 141 | _message = "Shell command is empty.\n - command : " + _command; 142 | } 143 | } catch (const std::exception & e) { 144 | _code = -3; 145 | _message = "Shell command is error.\n - command : " + _command + 146 | "\n - error : " + e.what(); 147 | } 148 | ERROR("Shell: %s, %s", _command.c_str(), _message.c_str()); 149 | return false; 150 | } 151 | -------------------------------------------------------------------------------- /low_power_consumption/src/power_consumption_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 "rclcpp/rclcpp.hpp" 19 | #include "cyberdog_common/cyberdog_log.hpp" 20 | 21 | namespace cyberdog 22 | { 23 | namespace manager 24 | { 25 | PowerConsumptionManager::PowerConsumptionManager(const std::string & name) 26 | { 27 | this->node_sub_motion_ptr_ = rclcpp::Node::make_shared(name); 28 | this->node_power_consump_ptr_ = rclcpp::Node::make_shared("power_consumption"); 29 | INFO("Creating sub_node object(node)"); 30 | INFO("Creating power_consumption object(node)"); 31 | } 32 | 33 | PowerConsumptionManager::~PowerConsumptionManager() 34 | { 35 | INFO("Destroy power_consumption object(node)"); 36 | } 37 | 38 | bool PowerConsumptionManager::Init() 39 | { 40 | INFO("PowerConsumptionManager node init"); 41 | // sub motion init 42 | this->motion_status_sub_ = this->node_sub_motion_ptr_-> 43 | create_subscription( 44 | "motion_status", 10, std::bind( 45 | &PowerConsumptionManager::sub_mostion_status_callback, 46 | this, std::placeholders::_1)); 47 | 48 | // power manager srv init 49 | this->power_consumption_manager_srv_ = this->node_power_consump_ptr_-> 50 | create_service( 51 | "power_consumption_manager", std::bind( 52 | &PowerConsumptionManager::EnterLowPower, 53 | this, std::placeholders::_1, std::placeholders::_2)); 54 | 55 | this->power_comsumption_client_ = this->node_power_consump_ptr_-> 56 | create_client("power_consumption_manager"); 57 | 58 | // create subcribe 59 | this->lpc_ptr_ = std::make_unique(); 60 | return true; 61 | } 62 | 63 | bool PowerConsumptionManager::send_power_manager_request(const bool msg) 64 | { 65 | if (!power_comsumption_client_->wait_for_service()) { 66 | INFO( 67 | "call power consumption manager server not avalible"); 68 | return false; 69 | } 70 | auto req = std::make_shared(); 71 | req->data = msg; 72 | auto result_future = power_comsumption_client_->async_send_request(req); 73 | std::chrono::seconds timeout(3); 74 | std::future_status status = result_future.wait_for(timeout); 75 | 76 | if (status == std::future_status::ready) { 77 | INFO("success to call the power_comsumption server."); 78 | } else { 79 | INFO("Failed to call the power_comsumption server."); 80 | return false; 81 | } 82 | // INFO("result is: %ld", result_future.get()->success); 83 | return true; 84 | } 85 | 86 | void PowerConsumptionManager::sub_mostion_status_callback( 87 | const protocol::msg::MotionStatus msg) 88 | { 89 | // INFO("sub motion_id messages*********"); 90 | // motion_id: 趴下(101)、站立(111) 91 | static bool convert_motion_flage = true; 92 | static int lay_count = 0; 93 | 94 | static bool times_flag = true; 95 | static int count = 0; 96 | 97 | // 启动(或急停)后不进行任何操作，60s后进入低功耗 98 | if (times_flag == true && msg.motion_id == 0) { 99 | ++count; 100 | INFO("lay_count %d", count); 101 | if (count == 600) { 102 | count = 0; 103 | convert_motion_flage = false; 104 | times_flag = false; 105 | INFO("call low power consumption"); 106 | send_power_manager_request(true); 107 | } 108 | } else { 109 | count = 0; 110 | } 111 | 112 | // 运动状态转换至趴下，30s后启动低功耗 113 | if (convert_motion_flage == true && msg.motion_id == 101) { 114 | // motion_status的发布频率为10Hz，延时30s，lay_count == 300 115 | ++lay_count; 116 | INFO("lay_count = %d", lay_count); 117 | if (lay_count == 300) { 118 | INFO("call low power consumption"); 119 | send_power_manager_request(true); 120 | convert_motion_flage = false; 121 | lay_count = 0; 122 | } 123 | } else { 124 | lay_count = 0; 125 | } 126 | 127 | // 状态切换到站立，启动正常功耗 128 | if (convert_motion_flage == false && msg.motion_id == 111) { 129 | INFO("call nomal power consumption"); 130 | send_power_manager_request(false); 131 | convert_motion_flage = true; 132 | times_flag = true; 133 | } 134 | } 135 | 136 | void PowerConsumptionManager::EnterLowPower( 137 | const std_srvs::srv::SetBool::Request::SharedPtr request, 138 | std_srvs::srv::SetBool::Response::SharedPtr response) 139 | { 140 | static int r_count = 0; 141 | INFO("[%d]EnterLowPower %s:start", (r_count + 1), (request->data ? "true" : "false")); 142 | PM_DEV pd = PM_ALL_NO_MOTION; 143 | unsigned int err; 144 | int code = -1; 145 | if (request->data) { 146 | INFO("starting low power consumption"); 147 | code = lpc_ptr_->LpcRelease(pd, &err); 148 | ++r_count; 149 | INFO("LpcRelease() return is %d", code); 150 | } else { 151 | INFO("starting nomal power consumption"); 152 | code = lpc_ptr_->LpcRequest(pd, &err); 153 | ++r_count; 154 | INFO("LpcRequest() return is %d", code); 155 | } 156 | response->success = (code == 0 ? true : false); 157 | if (code != 0) { 158 | response->message = std::to_string(err); 159 | INFO("err is %d", err); 160 | } 161 | INFO("[%d]EnterLowPower %s:stop", (r_count + 1), (request->data ? "true" : "false")); 162 | } 163 | 164 | void PowerConsumptionManager::Run() 165 | { 166 | INFO("cyberdog power manager node spin"); 167 | this->executor_.add_node(node_sub_motion_ptr_); 168 | this->executor_.add_node(node_power_consump_ptr_); 169 | this->executor_.spin(); 170 | rclcpp::shutdown(); 171 | } 172 | } // namespace manager 173 | } // namespace cyberdog 174 | -------------------------------------------------------------------------------- /cyberdog_permissions/include/cyberdog_permission/board_info.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 CYBERDOG_PERMISSION__BOARD_INFO_HPP_ 15 | #define CYBERDOG_PERMISSION__BOARD_INFO_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #define INFO_FILE_NAME "/dev/disk/by-partlabel/misc" 22 | #define IOT_CRC_LEN 68 23 | #define MAC_LEN 6 24 | #define DID_LEN 8 25 | #define KEY_LEN 16 26 | 27 | namespace cyberdog 28 | { 29 | namespace manager 30 | { 31 | class BoardInfo final 32 | { 33 | public: 34 | BoardInfo() = delete; 35 | static std::string Get_Sn() 36 | { 37 | // char buf[256] = {0}; 38 | // std::ifstream infile(INFO_FILE_NAME, std::ifstream::in); 39 | // if (infile.is_open()) { 40 | // infile.seekg(10485760, std::ios::beg); 41 | // infile.get(buf, sizeof(buf)); 42 | // } 43 | // return buf; 44 | char buf[256] = {0}; 45 | FILE * stream = 46 | popen("factory-tool -f /usr/share/factory_cmd_config/system.xml -i \"SN\"", "r"); 47 | fread(buf, sizeof(char), sizeof(buf), stream); 48 | pclose(stream); 49 | for (size_t i = 0; i < strlen(buf); ++i) { 50 | if (buf[i] == '\n' || buf[i] == '\r' || buf[i] == ' ') { 51 | buf[i] = '\0'; 52 | break; 53 | } 54 | } 55 | return buf; 56 | } 57 | static bool Get_Iot(std::string & mac, std::string & did, std::string & key) 58 | { 59 | char buf[IOT_CRC_LEN + 1] = {0}; 60 | std::ifstream infile(INFO_FILE_NAME, std::ifstream::in); 61 | if (infile.is_open()) { 62 | infile.seekg(11534336, std::ios::beg); 63 | infile.get(buf, sizeof(buf)); 64 | } else { 65 | return false; 66 | } 67 | uint8_t tmp_mac[MAC_LEN + 1] = {0}; 68 | uint64_t tmp_did = 0; 69 | uint8_t tmp_key[KEY_LEN + 1] = {0}; 70 | uint32_t code = parse_otpstr(buf, tmp_mac, &tmp_did, tmp_key); 71 | // mac.assign((char*)tmp_mac, MAC_LEN); 72 | mac = hex_str(tmp_mac, MAC_LEN); 73 | did = std::to_string(tmp_did); 74 | key.assign(reinterpret_cast(tmp_key), KEY_LEN); 75 | return code == 0; 76 | } 77 | 78 | private: 79 | static std::string hex_str(const uint8_t * data, const size_t size) 80 | { 81 | if (data == NULL) { 82 | return ""; 83 | } 84 | std::string buff; 85 | const int len = size; 86 | for (int j = 0; j < len; j++) { 87 | int high = data[j] / 16, low = data[j] % 16; 88 | buff += (high < 10) ? ('0' + high) : ('a' + high - 10); 89 | buff += (low < 10) ? ('0' + low) : ('a' + low - 10); 90 | } 91 | return buff; 92 | } 93 | static int unhexify(const char * hexstr, uint8_t hexlen, uint8_t * out_buff) 94 | { 95 | unsigned char c, c2; 96 | if (hexlen > strlen(hexstr)) { 97 | hexlen = strlen(hexstr); 98 | } 99 | int len = hexlen / 2; 100 | if (0 != hexlen % 2) { /* must be even number of bytes */ 101 | return -1; 102 | } 103 | for (int i = 0; i < len; ++i) { 104 | c = *(hexstr + i * 2); 105 | if (c >= '0' && c <= '9') { 106 | c -= '0'; 107 | } else if (c >= 'a' && c <= 'f') { 108 | c -= 'a' - 10; 109 | } else if (c >= 'A' && c <= 'F') { 110 | c -= 'A' - 10; 111 | } else {return -1;} 112 | c2 = *(hexstr + i * 2 + 1); 113 | if (c2 >= '0' && c2 <= '9') { 114 | c2 -= '0'; 115 | } else if (c2 >= 'a' && c2 <= 'f') { 116 | c2 -= 'a' - 10; 117 | } else if (c2 >= 'A' && c2 <= 'F') { 118 | c2 -= 'A' - 10; 119 | } else { 120 | return -1; 121 | } 122 | *out_buff++ = ( c << 4 ) | c2; 123 | } 124 | return len; 125 | } 126 | static uint32_t crc32(const uint8_t * p_data, uint32_t size, uint32_t init_crc) 127 | { 128 | uint32_t crc = init_crc; uint32_t i, j; 129 | for (i = 0; i < size; i++) { 130 | crc = crc ^ p_data[i]; 131 | for (j = 8; j > 0; j--) { 132 | crc = (crc >> 1) ^ (0xEDB88320U & ((crc & 1) ? 0xFFFFFFFF : 0)); 133 | } 134 | } 135 | return crc; 136 | } 137 | static uint32_t parse_otpstr( 138 | const char * otphexstr, uint8_t * out_mac, uint64_t * out_did, 139 | uint8_t * out_key) 140 | { 141 | if (NULL == otphexstr || NULL == out_mac || NULL == out_did || NULL == out_key) { 142 | return -1; 143 | } 144 | if (strlen(otphexstr) != (MAC_LEN + DID_LEN + KEY_LEN + 4) * 2) { 145 | return -2; 146 | } 147 | uint8_t otp_mac[MAC_LEN] = {0}; 148 | uint8_t otp_did[DID_LEN] = {0}; 149 | uint8_t otp_key[KEY_LEN + 1] = {0}; 150 | uint8_t otp_crc[4] = {0}; 151 | char did_hex[DID_LEN * 2 + 1] = {0}; 152 | char crc_hex[9] = {0}; 153 | strncpy(did_hex, otphexstr + MAC_LEN * 2, DID_LEN * 2); 154 | did_hex[DID_LEN * 2] = '\0'; 155 | strncpy(crc_hex, otphexstr + (MAC_LEN + DID_LEN + KEY_LEN) * 2, 8); 156 | crc_hex[8] = '\0'; 157 | unhexify(otphexstr, MAC_LEN * 2, otp_mac); 158 | unhexify(did_hex, DID_LEN * 2, otp_did); 159 | unhexify(otphexstr + (MAC_LEN + DID_LEN) * 2, KEY_LEN * 2, otp_key); 160 | unhexify(crc_hex, 8, otp_crc); 161 | uint8_t tripledata[MAC_LEN + DID_LEN + KEY_LEN] = {0}; 162 | char * endptr = NULL; 163 | uint32_t otp_crc_val = 0; 164 | uint32_t sum_crc_val = 0; 165 | memcpy(tripledata, otp_mac, MAC_LEN); 166 | memcpy(tripledata + MAC_LEN, otp_did, DID_LEN); 167 | memcpy(tripledata + MAC_LEN + DID_LEN, otp_key, KEY_LEN); 168 | otp_crc_val = strtol(crc_hex, &endptr, 16); 169 | sum_crc_val = crc32(tripledata, MAC_LEN + DID_LEN + KEY_LEN, 0); 170 | if (otp_crc_val != sum_crc_val) { 171 | return -3; 172 | } 173 | memcpy(out_mac, otp_mac, MAC_LEN); 174 | *out_did = strtoul(did_hex, &endptr, 16); 175 | memcpy(out_key, otp_key, KEY_LEN); 176 | return 0; 177 | } 178 | }; 179 | } // namespace manager 180 | } // namespace cyberdog 181 | 182 | #endif // CYBERDOG_PERMISSION__BOARD_INFO_HPP_ 183 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/audio_info.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 CYBERDOG_MANAGER__AUDIO_INFO_HPP_ 15 | #define CYBERDOG_MANAGER__AUDIO_INFO_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "protocol/msg/audio_play_extend.hpp" 22 | #include "protocol/srv/audio_text_play.hpp" 23 | #include "protocol/srv/sdcard_play_id_query.hpp" 24 | #include "cyberdog_common/cyberdog_log.hpp" 25 | 26 | namespace cyberdog 27 | { 28 | namespace manager 29 | { 30 | 31 | class AudioInfoNode final 32 | { 33 | using AudioMsg = protocol::msg::AudioPlayExtend; 34 | 35 | public: 36 | explicit AudioInfoNode(rclcpp::Node::SharedPtr node_ptr) 37 | : audio_info_node_(node_ptr) 38 | { 39 | audio_callback_group_ = 40 | audio_info_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 41 | rclcpp::PublisherOptions pub_options; 42 | pub_options.callback_group = audio_callback_group_; 43 | audio_play_pub_ = 44 | audio_info_node_->create_publisher( 45 | "speech_play_extend", rclcpp::SystemDefaultsQoS(), pub_options); 46 | audio_text_play_client_ = 47 | audio_info_node_->create_client( 48 | "speech_text_play", 49 | rmw_qos_profile_services_default, audio_callback_group_); 50 | sdcard_playid_query_client_ = 51 | audio_info_node_->create_client( 52 | "SdcardPlayIdQuery", 53 | rmw_qos_profile_services_default, audio_callback_group_); 54 | } 55 | 56 | void Init() 57 | { 58 | if (!init_) { 59 | INFO("...now play sound effect ready..."); 60 | std::chrono::seconds timeout(6); 61 | auto req = std::make_shared(); 62 | req->module_name = audio_info_node_->get_name(); 63 | req->is_online = false; 64 | req->speech.play_id = protocol::msg::AudioPlay::PID_SOUND_EFFECT_READY; 65 | auto future_result = audio_text_play_client_->async_send_request(req); 66 | std::future_status status = future_result.wait_for(timeout); 67 | if (status == std::future_status::ready) { 68 | ERROR("call play sound service success."); 69 | } else { 70 | ERROR("call play sound service failed!"); 71 | } 72 | AudioMsg text_msg; 73 | text_msg.module_name = audio_info_node_->get_name(); 74 | text_msg.is_online = false; 75 | text_msg.speech.play_id = protocol::msg::AudioPlay::PID_SELF_CHECK_SUCCESS; 76 | text_msg.text = "自检完成,状态就绪!"; 77 | audio_play_pub_->publish(text_msg); 78 | init_ = true; 79 | } 80 | } 81 | 82 | void Error(std::string error_text) 83 | { 84 | AudioMsg text_msg; 85 | text_msg.module_name = audio_info_node_->get_name(); 86 | text_msg.is_online = false; 87 | text_msg.speech.play_id = protocol::msg::AudioPlay::PID_SELF_CHECK_FAILED; 88 | text_msg.text = error_text; 89 | audio_play_pub_->publish(text_msg); 90 | } 91 | 92 | void SpeechNotify(int32_t code) 93 | { 94 | auto mcpim_iter = module_code_play_id_map.find(code); 95 | if (mcpim_iter != module_code_play_id_map.end()) { 96 | uint16_t play_id = mcpim_iter->second; 97 | std::chrono::seconds timeout(6); 98 | auto req = std::make_shared(); 99 | req->module_name = audio_info_node_->get_name(); 100 | req->is_online = false; 101 | req->speech.play_id = play_id; 102 | auto future_result = audio_text_play_client_->async_send_request(req); 103 | std::future_status status = future_result.wait_for(timeout); 104 | if (status == std::future_status::ready) { 105 | ERROR("call play sound service success. play id:%d", play_id); 106 | } else { 107 | ERROR("call play sound service failed! play id:%d", play_id); 108 | } 109 | } 110 | } 111 | 112 | private: 113 | rclcpp::Node::SharedPtr audio_info_node_{nullptr}; 114 | rclcpp::CallbackGroup::SharedPtr audio_callback_group_; 115 | rclcpp::Publisher::SharedPtr audio_play_pub_ {nullptr}; 116 | rclcpp::Client::SharedPtr audio_text_play_client_ {nullptr}; 117 | rclcpp::Client::SharedPtr sdcard_playid_query_client_ {nullptr}; 118 | bool init_ {false}; 119 | const std::map module_code_play_id_map = { 120 | {2409, 31006}, // GPS自检失败 121 | {2109, 31007}, // 雷达自检失败 122 | {2209, 31008}, // TOF自检失败 123 | {2309, 31009}, // 超声自检失败 124 | {1409, 31010}, // Bms自检失败 125 | {1109, 31011}, // Led自检失败 126 | {1209, 31011}, // MiniLed自检失败 127 | {1509, 31012}, // Touch自检失败 128 | {1609, 31013}, // Uwb自检失败 129 | {5109, 31014}, // Audio自检失败 130 | {3009, 31015}, // Motion自检失败 131 | // {4409, 31056}, // Realsense自检失败 132 | {5300, 31058}, // algorithm加载完成 133 | {2403, 31016}, // GPS切换工作模式失败 134 | {2103, 31017}, // 雷达切换工作模式失败 135 | {2203, 31018}, // TOF切换工作模式失败 136 | {2303, 31019}, // 超声切换工作模式失败 137 | {1403, 31020}, // Bms切换工作模式失败 138 | {1103, 31021}, // Led切换工作模式失败 139 | {1203, 31021}, // MiniLed切换工作模式失败 140 | {1503, 31022}, // Touch切换工作模式失败 141 | {1603, 31023}, // Uwb切换工作模式失败 142 | {5103, 31024}, // Audio切换工作模式失败 143 | {3003, 31025}, // Motion切换工作模式失败 144 | {2003, 31024}, // Sensor切换工作模式失败 145 | {1003, 31025}, // Device切换工作模式失败 146 | {2401, 31016}, // GPS切换工作模式失败 147 | {2101, 31017}, // 雷达切换工作模式失败 148 | {2201, 31018}, // TOF切换工作模式失败 149 | {2301, 31019}, // 超声切换工作模式失败 150 | {1401, 31020}, // Bms切换工作模式失败 151 | {1101, 31021}, // Led切换工作模式失败 152 | {1201, 31021}, // MiniLed切换工作模式失败 153 | {1501, 31022}, // Touch切换工作模式失败 154 | {1601, 31023}, // Uwb切换工作模式失败 155 | {5101, 31024}, // Audio切换工作模式失败 156 | {3001, 31025}, // Motion切换工作模式失败 157 | {2001, 31024}, // Sensor切换工作模式失败 158 | {1001, 31025}, // Device切换工作模式失败 159 | }; 160 | }; 161 | 162 | } // namespace manager 163 | } // namespace cyberdog 164 | 165 | #endif // CYBERDOG_MANAGER__AUDIO_INFO_HPP_ 166 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/touch_info.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 CYBERDOG_MANAGER__TOUCH_INFO_HPP_ 15 | #define CYBERDOG_MANAGER__TOUCH_INFO_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "cyberdog_common/cyberdog_log.hpp" 23 | #include "std_msgs/msg/string.hpp" 24 | #include "protocol/msg/touch_status.hpp" 25 | #include "protocol/msg/bms_status.hpp" 26 | #include "protocol/msg/audio_play_extend.hpp" 27 | #include "protocol/msg/state_switch_status.hpp" 28 | #include "std_srvs/srv/trigger.hpp" 29 | 30 | namespace cyberdog 31 | { 32 | namespace manager 33 | { 34 | class TouchInfoNode final 35 | { 36 | public: 37 | explicit TouchInfoNode(rclcpp::Node::SharedPtr node_ptr) 38 | { 39 | touch_info_node_ = node_ptr; 40 | touch_callback_group_ = 41 | touch_info_node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); 42 | rclcpp::SubscriptionOptions sub_options; 43 | 44 | sub_options.callback_group = touch_callback_group_; 45 | touch_status_sub_ = touch_info_node_->create_subscription( 46 | "touch_status", rclcpp::SystemDefaultsQoS(), 47 | std::bind(&TouchInfoNode::ReportPower, this, std::placeholders::_1), sub_options); 48 | 49 | bms_status_sub_ = touch_info_node_->create_subscription( 50 | "bms_status", rclcpp::SystemDefaultsQoS(), 51 | std::bind(&TouchInfoNode::BmsStatus, this, std::placeholders::_1), sub_options); 52 | 53 | StateSwitch_sub = touch_info_node_->create_subscription( 54 | "state_switch_status", rclcpp::SystemDefaultsQoS(), 55 | std::bind(&TouchInfoNode::SelfCheckCallback, this, std::placeholders::_1), sub_options); 56 | 57 | rclcpp::PublisherOptions pub_options; 58 | pub_options.callback_group = touch_callback_group_; 59 | audio_play_extend_pub = 60 | touch_info_node_->create_publisher( 61 | "speech_play_extend", rclcpp::SystemDefaultsQoS(), pub_options); 62 | low_power_exit_client_ = touch_info_node_->create_client( 63 | "low_power_exit"); 64 | std::thread ExitLowPowerThread = std::thread(&TouchInfoNode::ExitLowPower, this); 65 | ExitLowPowerThread.detach(); 66 | } 67 | 68 | private: 69 | void ReportPower(const protocol::msg::TouchStatus::SharedPtr msg) 70 | { 71 | INFO_MILLSECONDS(30000, "[cyberdog_manager:] touch_info-->> Enter TouchStatus callback!!"); 72 | std::chrono::milliseconds report_previous_time = 73 | std::chrono::duration_cast( 74 | std::chrono::system_clock::now().time_since_epoch()); 75 | INFO("report_previous_time is %d", report_previous_time.count()); 76 | if (msg->touch_state == 3 && report_flag && 77 | abs(reporting_time.count() - report_previous_time.count()) >= 2000) 78 | { 79 | reporting_time = std::chrono::duration_cast( 80 | std::chrono::system_clock::now().time_since_epoch()); 81 | protocol::msg::AudioPlayExtend msg; 82 | msg.is_online = false; 83 | msg.module_name = touch_info_node_->get_name(); 84 | msg.speech.play_id = 1000 + battery_percent; 85 | // msg.text = "剩余电量为百分之" + std::to_string(battery_percent); 86 | // INFO("语言播报的电量为：%s", msg.text.c_str()); 87 | audio_play_extend_pub->publish(msg); 88 | if (is_low_power) { // 通知消费者 89 | lowPower_cv_.notify_all(); 90 | } 91 | } 92 | } 93 | 94 | void BmsStatus(const protocol::msg::BmsStatus::SharedPtr msg) 95 | { 96 | battery_percent = msg->batt_soc; 97 | report_flag = true; 98 | INFO_MILLSECONDS( 99 | 30000, 100 | "[cyberdog_manager:] touch_info-->> Enter BMS callback.the batt_soc is %d.", 101 | battery_percent); 102 | } 103 | void SelfCheckCallback(const protocol::msg::StateSwitchStatus::SharedPtr msg) 104 | { 105 | if (msg->state == 2) { 106 | is_low_power = true; 107 | INFO("state_switch_status state is %d", msg->state); 108 | } else { 109 | is_low_power = false; 110 | } 111 | } 112 | void ExitLowPower() 113 | { 114 | while (rclcpp::ok()) { 115 | std::unique_lock lck(exitLowPower_mtx_); 116 | while (!is_low_power) { // 消费者得到通知，跳出循环执行退出低功耗指令 117 | lowPower_cv_.wait(lck); 118 | } 119 | // 退出低功耗请求 120 | if (!low_power_exit_client_->wait_for_service(std::chrono::seconds(3))) { 121 | INFO("low_power_exit service is not avaiable"); 122 | } 123 | std::chrono::seconds timeout(13); // wait for completely exiting 124 | auto req = std::make_shared(); 125 | auto future_result = low_power_exit_client_->async_send_request(req); 126 | std::future_status status = future_result.wait_for(timeout); 127 | if (status == std::future_status::ready) { 128 | INFO("call low_power_exit success."); 129 | is_low_power = false; 130 | } else { 131 | ERROR("call low_power_exit timeout."); 132 | } 133 | } 134 | } 135 | 136 | private: 137 | rclcpp::Node::SharedPtr touch_info_node_{nullptr}; 138 | rclcpp::CallbackGroup::SharedPtr touch_callback_group_; 139 | rclcpp::Client::SharedPtr low_power_exit_client_; 140 | rclcpp::Subscription::SharedPtr touch_status_sub_; 141 | rclcpp::Subscription::SharedPtr bms_status_sub_; 142 | rclcpp::Publisher::SharedPtr audio_play_extend_pub; 143 | int battery_percent; 144 | std::chrono::milliseconds reporting_time = std::chrono::duration_cast( 145 | std::chrono::system_clock::now().time_since_epoch()); 146 | bool report_flag = false; 147 | 148 | rclcpp::Subscription::SharedPtr StateSwitch_sub; 149 | // rclcpp::CallbackGroup::SharedPtr callback_group_subscriber1_; 150 | bool low_power_status = false; 151 | std::mutex exitLowPower_mtx_; 152 | bool is_low_power{false}; 153 | std::condition_variable lowPower_cv_; 154 | }; 155 | } // namespace manager 156 | } // namespace cyberdog 157 | 158 | #endif // CYBERDOG_MANAGER__TOUCH_INFO_HPP_ 159 | -------------------------------------------------------------------------------- /user_info_manager/include/user_info_manager/UserAccountManager.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 USER_INFO_MANAGER__USERACCOUNTMANAGER_HPP_ 15 | #define USER_INFO_MANAGER__USERACCOUNTMANAGER_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "rapidjson/rapidjson.h" 25 | #include "rapidjson/document.h" 26 | #include "rapidjson/prettywriter.h" 27 | #include "rapidjson/stringbuffer.h" 28 | #include "rapidjson/filereadstream.h" 29 | #include "rapidjson/filewritestream.h" 30 | #include "rapidjson/writer.h" 31 | #include "cyberdog_common/cyberdog_log.hpp" 32 | #include "black_box/black_box.hpp" 33 | 34 | #define CYBETDOGMANAGER "CyberdogAccountManager" 35 | 36 | namespace cyberdog 37 | { 38 | namespace common 39 | { 40 | namespace json = rapidjson; 41 | class CyberdogAccountManager 42 | { 43 | public: 44 | CyberdogAccountManager() {} 45 | ~CyberdogAccountManager() {} 46 | struct UserInformation 47 | { 48 | std::string username; 49 | int voiceStatus; 50 | int faceStatus; 51 | } userinformation; 52 | std::vector vectorUser; 53 | int result[2]; 54 | 55 | public: 56 | // const std::string base_account_dir = "/opt/ros2/cyberdog/share/params/toml_config/manager"; 57 | /** 58 | * @brief add a user to database 59 | */ 60 | bool AddMember(std::string username) 61 | { 62 | INFO("[UserAccountManager]: enter ADDMember()"); 63 | cyberdog::manager::BlackBox black_box_; 64 | return black_box_.AddUser(username); 65 | } 66 | /** 67 | * @brief search all user form database 68 | */ 69 | bool SearAllUser( 70 | std::vector & vectorUser_) 71 | { 72 | INFO("[UserAccountManager]: enter serachAllUser()"); 73 | cyberdog::manager::BlackBox black_box; 74 | if (black_box.SearchUser(vectorUser_) ) { 75 | INFO("[UserAccountManager]: enter serachAllUser [if]", vectorUser_.size()); 76 | INFO("vectorUser size is :%d", vectorUser_.size()); 77 | for (int i = 0; i < vectorUser_.size(); i++) { 78 | INFO("[UserAccountManager]: enter serachAllUser [for] cycle"); 79 | INFO( 80 | "[UserAccountManager]:%d %s:%d %d", 81 | vectorUser_[i].id, vectorUser_[i].name.c_str(), 82 | vectorUser_[i].voiceStatus, vectorUser_[i].faceStatus); 83 | } 84 | return true; 85 | } else { 86 | INFO("[UserAccountManager]: search all user failed"); 87 | return false; 88 | } 89 | } 90 | /** 91 | * @brief search a user information by username 92 | * param： username (the user's name), result (the array of user's information) 93 | */ 94 | bool SearchUser(const std::string username, int * result) 95 | { 96 | INFO("[UserAccountManager]: enter SearchUser"); 97 | cyberdog::manager::BlackBox black_box; 98 | if (black_box.SearchSingleUser(username, result) ) { 99 | INFO("[UserAccountManager]: enter SearchUser [if]"); 100 | INFO( 101 | "[UserAccountManager]: search singleuser's information is : %d, %d", result[0], 102 | result[1]); 103 | return true; 104 | } else { 105 | INFO("[UserAccountManager]: search single user failed"); 106 | return false; 107 | } 108 | } 109 | /** 110 | * @brief search a user information by username 111 | * param： username (the user's name), 112 | * param: flag (flag=0 return the user's voice status; flag=1 return the user's face status) 113 | * return: the status of user's voice or face 114 | */ 115 | int SearchOneUser(std::string username, int flag) 116 | { 117 | int result[2] = {0}; 118 | if (SearchUser(username, result)) { 119 | INFO("[UserAccountManager]: search success"); 120 | return result[flag]; 121 | } else { 122 | INFO("[UserAccountManager]: the %s not exit", username.c_str()); 123 | return -1; 124 | } 125 | } 126 | /** 127 | * @brief modify the indoformation of user 128 | * param: status. the new status(voice | face) of user 129 | * param: flag. flag =0 ,status = voice; flag =1 , status = face; 130 | */ 131 | bool ModifyUserInformation(std::string username, int status, int flag) 132 | { 133 | INFO("[UserAccountManager]: enter ModifyUserInformation"); 134 | cyberdog::manager::BlackBox black_box; 135 | if (black_box.ModifyUser(username, flag, status)) { 136 | INFO("[UserAccountManager]: enter Modify user [if]"); 137 | INFO( 138 | "[UserAccountManager]: modify username:%s, flag:%d, status:%d", 139 | username.c_str(), flag, status); 140 | return true; 141 | } else { 142 | INFO("[UserAccountManager]: modify user information failed"); 143 | return false; 144 | } 145 | } 146 | /** 147 | *@brief delete the information of user 148 | */ 149 | bool DeleteUserInformation(std::string username) 150 | { 151 | INFO("[UserAccountManager]: enter DeleteUserInformation"); 152 | cyberdog::manager::BlackBox black_box; 153 | if (black_box.DeleteUser(username)) { 154 | INFO("[UserAccountManager]: delete user success"); 155 | return true; 156 | } else { 157 | INFO("[UserAccountManager]: delete user failed!!!!"); 158 | return false; 159 | } 160 | } 161 | /** 162 | * @brief delete the voice status of user 163 | */ 164 | bool DeleteVoice(std::string username) 165 | { 166 | if (ModifyUserInformation(username, 0, 0)) { 167 | INFO("[UserAccountManager]: delete %s's voice status success", username.c_str()); 168 | return true; 169 | } else { 170 | INFO("[UserAccountManager]: delete %s's voice status failed", username.c_str()); 171 | return false; 172 | } 173 | } 174 | /** 175 | * @brief delete user's information of voice,set face status to 0 176 | * param username 177 | */ 178 | bool DeleteFace(std::string username) 179 | { 180 | if (ModifyUserInformation(username, 0, 1)) { 181 | INFO("[UserAccountManager]: delete %s's voice status success", username.c_str()); 182 | return true; 183 | } else { 184 | INFO("[UserAccountManager]: delete %s's voice status failed", username.c_str()); 185 | return false; 186 | } 187 | } 188 | bool ModifyUserName(const std::string & name, const std::string & new_name) 189 | { 190 | cyberdog::manager::BlackBox black_box; 191 | if (black_box.ModifyUserName(name, new_name)) { 192 | INFO("ModifyUserName success\n"); 193 | return true; 194 | } else { 195 | INFO("ModifyUserName failed\n"); 196 | return false; 197 | } 198 | } 199 | }; 200 | } // namespace common 201 | } // namespace cyberdog 202 | #endif // USER_INFO_MANAGER__USERACCOUNTMANAGER_HPP_ 203 | -------------------------------------------------------------------------------- /manager_base/include/manager_base/manager_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 | #ifndef MANAGER_BASE__MANAGER_BASE_HPP_ 15 | #define MANAGER_BASE__MANAGER_BASE_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "rclcpp/node.hpp" 23 | #include "cyberdog_system/robot_code.hpp" 24 | #include "cyberdog_system/robot_state.hpp" 25 | #include "protocol/msg/manager_state.hpp" 26 | #include "protocol/msg/heartbeats.hpp" 27 | #include "protocol/srv/manager_init.hpp" 28 | 29 | 30 | namespace cyberdog 31 | { 32 | namespace manager 33 | { 34 | 35 | inline int64_t GetMsTime() 36 | { 37 | return (int64_t)std::chrono::duration_cast( 38 | std::chrono::system_clock::now().time_since_epoch()).count(); 39 | } 40 | 41 | /** 42 | * @brief Base class for all cyberdog managers. 43 | * 44 | */ 45 | class ManagerBase 46 | { 47 | using ManagerStateMsg = protocol::msg::ManagerState; 48 | using ManagerHeartbeatsMsg = protocol::msg::Heartbeats; 49 | using ManagerInitSrv = protocol::srv::ManagerInit; 50 | using StateFunction = std::function; 51 | 52 | public: 53 | explicit ManagerBase(const std::string & name) 54 | : name_(name) 55 | { 56 | state_ = system::ManagerState::kNotReady; 57 | BuildStateMap(); 58 | } 59 | ~ManagerBase() 60 | { 61 | if (!state_map_.empty()) { 62 | state_map_.clear(); 63 | } 64 | } 65 | 66 | virtual void Config() = 0; 67 | virtual bool Init() = 0; 68 | virtual void Run() = 0; 69 | virtual bool SelfCheck() = 0; 70 | 71 | /* state machine functions */ 72 | 73 | public: 74 | virtual void OnError() = 0; 75 | virtual void OnLowPower() = 0; 76 | virtual void OnSuspend() = 0; 77 | virtual void OnProtected() = 0; 78 | virtual void OnActive() = 0; 79 | 80 | bool RegisterStateHandler( 81 | rclcpp::Node::SharedPtr node_ptr) 82 | { 83 | if (node_ptr == nullptr) { 84 | return false; 85 | } 86 | state_sub_ = node_ptr->create_subscription( 87 | "manager_set_state", 88 | rclcpp::SystemDefaultsQoS(), 89 | std::bind(&ManagerBase::StateCheckout, this, std::placeholders::_1)); 90 | 91 | state_pub_ = node_ptr->create_publisher( 92 | "manager_set_state", 93 | rclcpp::SystemDefaultsQoS()); 94 | 95 | return true; 96 | } 97 | 98 | bool RegisterInitHandler(rclcpp::Node::SharedPtr node_ptr) 99 | { 100 | if (node_ptr == nullptr) { 101 | return false; 102 | } 103 | 104 | std::string server_name = std::string("manager_check_") + name_; 105 | init_server_ = node_ptr->create_service( 106 | server_name, 107 | std::bind( 108 | &ManagerBase::ManagerServiceCall, this, std::placeholders::_1, 109 | std::placeholders::_2)); 110 | return true; 111 | } 112 | 113 | bool RegisterHeartbeats(rclcpp::Node::SharedPtr node_ptr) 114 | { 115 | if (node_ptr == nullptr) { 116 | return false; 117 | } 118 | heartbeats_pub_ = node_ptr->create_publisher( 119 | "manager_heartbeats", 120 | rclcpp::SystemDefaultsQoS()); 121 | heartbeats_timer_ = node_ptr->create_wall_timer( 122 | std::chrono::seconds(2), 123 | std::bind(&ManagerBase::HeartbeatsFunction, this) 124 | ); 125 | return true; 126 | } 127 | 128 | bool SetState(int8_t state, std::string json_data = "{}") 129 | { 130 | if (!IsRegistered() ) { 131 | return false; 132 | } 133 | auto msg = ManagerStateMsg(); 134 | msg.state = state; 135 | msg.err_msg = json_data; 136 | this->state_pub_->publish(msg); 137 | return true; 138 | } 139 | 140 | system::ManagerState GetState() 141 | { 142 | return state_; 143 | } 144 | 145 | bool IsRegistered() 146 | { 147 | return (state_pub_ != nullptr) && (state_sub_ != nullptr); 148 | } 149 | 150 | void GetName(std::string & name) 151 | { 152 | name = name_; 153 | } 154 | 155 | private: 156 | /** 157 | * @brief State machine checkout callback. 158 | * 159 | * @param msg ros msg for trans state to checkout. 160 | */ 161 | void StateCheckout(const ManagerStateMsg::SharedPtr msg) 162 | { 163 | auto function_ptr = state_map_.find((system::ManagerState)msg->state); 164 | if (function_ptr != state_map_.end()) { 165 | state_ = (system::ManagerState)msg->state; 166 | function_ptr->second; 167 | } 168 | } 169 | 170 | /** 171 | * @brief Callback function for cyberdog_manager call all managers' selfcheck. 172 | * 173 | * @param request Unused. 174 | * @param response res_code: -1 / 0, OK / Failed. 175 | */ 176 | void ManagerServiceCall( 177 | const std::shared_ptr request, 178 | std::shared_ptr response) 179 | { 180 | (void)request; 181 | response->res_code = (int32_t)system::KeyCode::kOK; 182 | if (!SelfCheck() ) { 183 | response->res_code = (int32_t)system::KeyCode::kFailed; 184 | } 185 | } 186 | 187 | void HeartbeatsFunction() 188 | { 189 | auto msg = protocol::msg::Heartbeats(); 190 | msg.name = name_; 191 | // update msg code and params 192 | msg.state = (int8_t)state_; 193 | msg.timestamp = GetMsTime(); 194 | msg.params = std::string("OK"); 195 | heartbeats_pub_->publish(msg); 196 | } 197 | 198 | void BuildStateMap() 199 | { 200 | state_map_.insert( 201 | std::make_pair( 202 | system::ManagerState::kLowPower, 203 | std::bind(&ManagerBase::OnLowPower, this))); 204 | state_map_.insert( 205 | std::make_pair( 206 | system::ManagerState::kError, 207 | std::bind(&ManagerBase::OnError, this))); 208 | state_map_.insert( 209 | std::make_pair( 210 | system::ManagerState::kSuspend, 211 | std::bind(&ManagerBase::OnSuspend, this))); 212 | state_map_.insert( 213 | std::make_pair( 214 | system::ManagerState::kProtected, 215 | std::bind(&ManagerBase::OnProtected, this))); 216 | state_map_.insert( 217 | std::make_pair( 218 | system::ManagerState::kActive, 219 | std::bind(&ManagerBase::OnActive, this))); 220 | } 221 | 222 | private: 223 | std::string name_; 224 | system::ManagerState state_; 225 | std::map state_map_; 226 | rclcpp::TimerBase::SharedPtr heartbeats_timer_; 227 | rclcpp::Publisher::SharedPtr heartbeats_pub_ {nullptr}; 228 | // rclcpp::Node::WeakPtr node_ {nullptr}; 229 | rclcpp::Subscription::SharedPtr state_sub_ {nullptr}; 230 | rclcpp::Publisher::SharedPtr state_pub_ {nullptr}; 231 | rclcpp::Service::SharedPtr init_server_ {nullptr}; 232 | }; // class ManagerBase 233 | } // namespace manager 234 | } // namespace cyberdog 235 | 236 | #endif // MANAGER_BASE__MANAGER_BASE_HPP_ 237 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/battery_capacity_info.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 CYBERDOG_MANAGER__BATTERY_CAPACITY_INFO_HPP_ 15 | #define CYBERDOG_MANAGER__BATTERY_CAPACITY_INFO_HPP_ 16 | 17 | #include 18 | #include 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "std_msgs/msg/string.hpp" 21 | #include "protocol/msg/bms_status.hpp" 22 | #include "protocol/msg/audio_play_extend.hpp" 23 | #include "protocol/srv/audio_text_play.hpp" 24 | #include "protocol/srv/bms_info.hpp" 25 | #include "cyberdog_common/cyberdog_log.hpp" 26 | 27 | namespace cyberdog 28 | { 29 | namespace manager 30 | { 31 | // enum class BatteryMachineState : uint8_t 32 | // { 33 | // BMS_NORMAL = 0, // 正常 34 | // BMS_PROTECT = 1, // 保护 35 | // BMS_LOWPOWER = 2, // 低功耗 36 | // BMS_UNKOWN = 255, // 未知 37 | // }; 38 | 39 | class BatteryCapacityInfoNode final 40 | { 41 | using BCIN_CALLBACK = std::function; 42 | using BCINSOC_CALLBACK = std::function; 43 | using BCINBMS_CALLBACK = std::function; 44 | 45 | public: 46 | explicit BatteryCapacityInfoNode( 47 | rclcpp::Node::SharedPtr node_ptr, 48 | BCINSOC_CALLBACK bcin_soc, 49 | BCINBMS_CALLBACK bcin_bms) 50 | : battery_capacity_info_node_(node_ptr), 51 | batsoc_notify_handler(bcin_soc), 52 | bms_notify_handler(bcin_bms), 53 | is_soc_zero_(false), is_soc_five_(false), is_soc_twenty_(false), is_soc_thirty_(false) 54 | { 55 | } 56 | 57 | public: 58 | void Init() 59 | { 60 | bc_callback_group_ = 61 | battery_capacity_info_node_->create_callback_group( 62 | rclcpp::CallbackGroupType::Reentrant); 63 | rclcpp::SubscriptionOptions sub_options; 64 | sub_options.callback_group = bc_callback_group_; 65 | bms_status_sub_ = battery_capacity_info_node_->create_subscription( 66 | "bms_status", rclcpp::SystemDefaultsQoS(), 67 | std::bind(&BatteryCapacityInfoNode::BmsStatus, this, std::placeholders::_1), 68 | sub_options); 69 | bms_info_srv_ = battery_capacity_info_node_->create_service( 70 | "bms_info", std::bind( 71 | &BatteryCapacityInfoNode::BmsInfo, this, std::placeholders::_1, 72 | std::placeholders::_2), rmw_qos_profile_services_default, bc_callback_group_); 73 | rclcpp::PublisherOptions pub_options; 74 | pub_options.callback_group = bc_callback_group_; 75 | audio_play_extend_pub = 76 | battery_capacity_info_node_->create_publisher( 77 | "speech_play_extend", 78 | rclcpp::SystemDefaultsQoS(), pub_options); 79 | audio_play_client_ = battery_capacity_info_node_->create_client( 80 | "speech_text_play", 81 | rmw_qos_profile_services_default, bc_callback_group_); 82 | } 83 | 84 | private: 85 | void BmsStatus(const protocol::msg::BmsStatus::SharedPtr msg) 86 | { 87 | bms_status_ = *msg; 88 | bms_notify_handler(msg); 89 | INFO_MILLSECONDS( 90 | 30000, "Battery Capacity Info:%d", 91 | bms_status_.batt_soc); 92 | if (!bms_status_.power_wired_charging) { 93 | if (bms_status_.batt_soc <= 0) { 94 | if (!is_soc_zero_) { 95 | is_soc_zero_ = true; 96 | // is_soc_five_ = false; 97 | PlayAudioService(protocol::msg::AudioPlay::PID_PERCENT_0); 98 | } 99 | } else if (bms_status_.batt_soc < 5) { 100 | if (!is_soc_five_) { 101 | is_soc_five_ = true; 102 | is_soc_twenty_ = false; 103 | PlayAudio(protocol::msg::AudioPlay::PID_PERCENT_5); 104 | } 105 | } else if (bms_status_.batt_soc < 20) { 106 | if (!is_soc_twenty_) { 107 | is_soc_twenty_ = true; 108 | is_soc_five_ = false; 109 | is_soc_thirty_ = false; 110 | PlayAudio(protocol::msg::AudioPlay::PID_PERCENT_20); 111 | } 112 | } else if (bms_status_.batt_soc < 30) { 113 | if (!is_soc_thirty_) { 114 | is_soc_thirty_ = true; 115 | is_soc_twenty_ = false; 116 | PlayAudio(protocol::msg::AudioPlay::PID_PERCENT_30); 117 | } 118 | } 119 | } else { 120 | is_soc_twenty_ = false; 121 | is_soc_thirty_ = false; 122 | is_soc_five_ = false; 123 | } 124 | 125 | // 状态机切换过程中，不再重复请求切换 126 | if (!ms_switch_mutex_.try_lock()) { 127 | WARN_MILLSECONDS( 128 | 5000, "The machine_state is switching..." 129 | ); 130 | return; 131 | } 132 | batsoc_notify_handler(bms_status_.batt_soc, bms_status_.power_wired_charging); 133 | ms_switch_mutex_.unlock(); 134 | } 135 | 136 | void PlayAudio(const uint16_t play_id) 137 | { 138 | protocol::msg::AudioPlayExtend msg; 139 | msg.is_online = false; 140 | msg.module_name = battery_capacity_info_node_->get_name(); 141 | msg.speech.play_id = play_id; 142 | audio_play_extend_pub->publish(msg); 143 | } 144 | 145 | void PlayAudioService(const uint16_t play_id) 146 | { 147 | auto request_audio = std::make_shared(); 148 | request_audio->module_name = battery_capacity_info_node_->get_name(); 149 | request_audio->is_online = false; 150 | request_audio->speech.play_id = play_id; 151 | auto callback = [](rclcpp::Client::SharedFuture future) { 152 | INFO("Audio play result: %s", future.get()->status == 0 ? "success" : "failed"); 153 | }; 154 | auto future_result_audio = audio_play_client_->async_send_request(request_audio, callback); 155 | if (future_result_audio.wait_for(std::chrono::milliseconds(3000)) == 156 | std::future_status::timeout) 157 | { 158 | ERROR("Cannot get response from AudioPlay"); 159 | } 160 | } 161 | 162 | void BmsInfo( 163 | const protocol::srv::BmsInfo::Request::SharedPtr, 164 | protocol::srv::BmsInfo::Response::SharedPtr response) 165 | { 166 | response->msg = bms_status_; 167 | response->code = 0; 168 | } 169 | 170 | private: 171 | rclcpp::Node::SharedPtr battery_capacity_info_node_ {nullptr}; 172 | rclcpp::CallbackGroup::SharedPtr bc_callback_group_; 173 | rclcpp::Subscription::SharedPtr bms_status_sub_; 174 | rclcpp::Publisher::SharedPtr audio_play_extend_pub; 175 | rclcpp::Service::SharedPtr bms_info_srv_; 176 | rclcpp::Client::SharedPtr audio_play_client_ {nullptr}; 177 | protocol::msg::BmsStatus bms_status_; 178 | BCINSOC_CALLBACK batsoc_notify_handler; 179 | BCINBMS_CALLBACK bms_notify_handler; 180 | std::mutex ms_switch_mutex_; 181 | bool is_soc_zero_; 182 | bool is_soc_five_; 183 | bool is_soc_twenty_; 184 | bool is_soc_thirty_; 185 | }; 186 | } // namespace manager 187 | } // namespace cyberdog 188 | 189 | #endif // CYBERDOG_MANAGER__BATTERY_CAPACITY_INFO_HPP_ 190 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/heart_context.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 CYBERDOG_MANAGER__HEART_CONTEXT_HPP_ 15 | #define CYBERDOG_MANAGER__HEART_CONTEXT_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "protocol/msg/heartbeats.hpp" 23 | #include "manager_base/manager_base.hpp" 24 | #include "cyberdog_machine/cyberdog_heartbeats.hpp" 25 | 26 | using cyberdog::common::CyberdogJson; 27 | using rapidjson::Document; 28 | using rapidjson::kObjectType; 29 | 30 | namespace cyberdog 31 | { 32 | namespace manager 33 | { 34 | struct HeartbeatsRecorder 35 | { 36 | // std::string name; 37 | int64_t timestamp; 38 | // int counter = 0; 39 | bool lost = false; 40 | }; // struct HeartbeatsRecorder 41 | 42 | class HeartContext final 43 | { 44 | using ManagerHeartbeatsMsg = protocol::msg::Heartbeats; 45 | 46 | public: 47 | explicit HeartContext( 48 | rclcpp::Node::SharedPtr node_ptr, 49 | std::function func) 50 | : heart_beats_node_(node_ptr), 51 | set_state_func_(func) 52 | { 53 | heart_beats_callback_group_ = 54 | heart_beats_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 55 | // manager_vec_.emplace_back("device"); 56 | // manager_vec_.emplace_back("sensor"); 57 | // manager_vec_.emplace_back("motion"); 58 | // manager_vec_.emplace_back("perception"); 59 | // manager_vec_.emplace_back("audio"); 60 | HeartbeatsNodeInit(); 61 | heart_beats_ptr_ = std::make_unique(500, 5); 62 | } 63 | ~HeartContext() = default; 64 | void Init() 65 | { 66 | std::for_each( 67 | manager_vec_.cbegin(), manager_vec_.cend(), 68 | [this](const std::string & name) { 69 | HeartbeatsRecorder heartbeats_recorder; 70 | heartbeats_recorder.timestamp = GetMsTime(); 71 | // heartbeats_recorder.counter = 0; 72 | heartbeats_recorder.lost = false; 73 | this->heartbeats_map_.insert( 74 | std::make_pair(name, heartbeats_recorder) 75 | ); 76 | } 77 | ); 78 | heart_beats_ptr_->HeartConfig( 79 | manager_vec_, 80 | std::bind(&cyberdog::manager::HeartContext::HeartbeatsStateNotify, this), 6); 81 | heart_beats_ptr_->RegisterLostCallback( 82 | std::bind( 83 | &cyberdog::manager::HeartContext::NodeStateConfirm, this, 84 | std::placeholders::_1, std::placeholders::_2) 85 | ); 86 | rclcpp::SubscriptionOptions options; 87 | options.callback_group = heart_beats_callback_group_; 88 | heartbeats_sub_ = heart_beats_node_->create_subscription( 89 | "manager_heartbeats", 90 | rclcpp::SystemDefaultsQoS(), 91 | std::bind(&HeartContext::HeartbeatsCallback, this, std::placeholders::_1), 92 | options); 93 | // start heartbeats check work 94 | // std::chrono::seconds sec = 95 | // std::chrono::duration_cast( 96 | // std::chrono::system_clock::now().time_since_epoch()); 97 | // INFO("heart beats run time:%ld", sec.count()); 98 | INFO("heart beats start running."); 99 | heart_beats_ptr_->HeartRun(); 100 | set_state_func_((int8_t)system::ManagerState::kActive, "{}"); 101 | } 102 | 103 | private: 104 | void HeartbeatsStateNotify() 105 | { 106 | Document json_document(kObjectType); 107 | for (auto & elem : heartbeats_map_) { 108 | Document node_document(kObjectType); 109 | CyberdogJson::Add(node_document, "alive", !elem.second.lost); 110 | // std::chrono::milliseconds mill_sec = 111 | // std::chrono::duration_cast( 112 | // std::chrono::system_clock::now().time_since_epoch()); 113 | // uint64_t mill_count = mill_sec.count(); 114 | // CyberdogJson::Add(node_document, "timepoint", mill_count); 115 | CyberdogJson::Add(json_document, elem.first, node_document); 116 | } 117 | std::string json_string("{}"); 118 | if (!CyberdogJson::Document2String(json_document, json_string)) { 119 | ERROR("error while encoding to json of heart beats state notify"); 120 | } 121 | has_error_ == true ? 122 | set_state_func_((int8_t)system::ManagerState::kError, std::move(json_string)) : 123 | set_state_func_((int8_t)system::ManagerState::kOK, std::move(json_string)); 124 | has_error_ = false; 125 | } 126 | 127 | void NodeStateConfirm(const std::string & name, bool lost) 128 | { 129 | auto iter = heartbeats_map_.find(name); 130 | if (iter == heartbeats_map_.end()) { 131 | return; 132 | } 133 | has_error_ = has_error_ | lost; 134 | if (has_error_ && lost) { 135 | // std::chrono::seconds sec = 136 | // std::chrono::duration_cast( 137 | // std::chrono::system_clock::now().time_since_epoch()); 138 | // uint64_t sec_count = sec.count(); 139 | // ERROR("%s node lost, time point:%ld", iter->first.c_str(), sec_count); 140 | ERROR("%s node heart beat lost", iter->first.c_str()); 141 | } 142 | iter->second.lost = lost; 143 | iter->second.timestamp = GetMsTime(); 144 | } 145 | 146 | void HeartbeatsCallback(const ManagerHeartbeatsMsg::SharedPtr msg) 147 | { 148 | auto iter = heartbeats_map_.find(msg->name); 149 | if (iter == heartbeats_map_.end()) { 150 | return; 151 | } 152 | heart_beats_ptr_->HeartUpdate(iter->first); 153 | // iter->second.timestamp = msg->timestamp; 154 | // // iter->second.counter = 0; 155 | } 156 | 157 | bool HeartbeatsNodeInit() 158 | { 159 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 160 | auto path = local_share_dir + std::string("/toml_config/manager/heart_beat_config.toml"); 161 | toml::value config; 162 | if (!common::CyberdogToml::ParseFile(path, config)) { 163 | ERROR("Parse Heart Beat config file failed, toml file is invalid!"); 164 | return false; 165 | } 166 | toml::value heartbeat; 167 | if (!common::CyberdogToml::Get(config, "heartbeat", heartbeat)) { 168 | ERROR("Heart Beat init failed, parse heartbeat config failed!"); 169 | return false; 170 | } 171 | // toml::array actuator_array; 172 | if (!common::CyberdogToml::Get(heartbeat, "node", manager_vec_)) { 173 | ERROR("Heart Beat init failed, parse node array failed!"); 174 | return false; 175 | } 176 | return true; 177 | } 178 | 179 | private: 180 | bool has_error_ {false}; 181 | rclcpp::Node::SharedPtr heart_beats_node_ {nullptr}; 182 | std::function set_state_func_; 183 | rclcpp::CallbackGroup::SharedPtr heart_beats_callback_group_; 184 | std::vector manager_vec_; 185 | std::map heartbeats_map_; 186 | std::unique_ptr heart_beats_ptr_ {nullptr}; 187 | rclcpp::Subscription::SharedPtr heartbeats_sub_{nullptr}; 188 | }; 189 | } // namespace manager 190 | } // namespace cyberdog 191 | 192 | #endif // CYBERDOG_MANAGER__HEART_CONTEXT_HPP_ 193 | -------------------------------------------------------------------------------- /cyberdog_manager/src/cyberdog_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 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "rclcpp/rclcpp.hpp" 23 | #include "rclcpp/node.hpp" 24 | #include "std_srvs/srv/trigger.hpp" 25 | #include "protocol/srv/ota_server_cmd.hpp" 26 | #include "cyberdog_manager/cyberdog_manager.hpp" 27 | #include "cyberdog_common/cyberdog_log.hpp" 28 | #include "cyberdog_common/cyberdog_json.hpp" 29 | #include "ament_index_cpp/get_package_share_directory.hpp" 30 | #include "low_power_consumption/pm_if.h" 31 | 32 | using cyberdog::common::CyberdogJson; 33 | using rapidjson::Document; 34 | using rapidjson::kObjectType; 35 | 36 | cyberdog::manager::CyberdogManager::CyberdogManager(const std::string & name) 37 | : name_(name) 38 | { 39 | node_ptr_ = rclcpp::Node::make_shared(name_); 40 | env_node_ptr_ = std::make_unique(node_ptr_); 41 | query_node_ptr_ = std::make_unique(node_ptr_); 42 | account_node_ptr_ = std::make_unique(node_ptr_); 43 | mssc_context_ptr_ = std::make_shared(node_ptr_); 44 | power_consumption_node_ptr = std::make_shared( 45 | node_ptr_, 46 | std::bind(&MachineStateSwitchContext::KeepDownOverTime, mssc_context_ptr_)); 47 | // ready_node_ptr = std::make_unique(name_ + "_ready"); 48 | ready_node_ptr = std::make_unique(node_ptr_); 49 | heart_beat_ptr_ = std::make_unique( 50 | node_ptr_, 51 | std::bind(&CyberdogManager::SetState, this, std::placeholders::_1, std::placeholders::_2)); 52 | error_context_ptr_ = std::make_unique(name_ + "_error"); 53 | touch_node_ptr = std::make_unique(node_ptr_); 54 | audio_node_ptr = std::make_shared( 55 | node_ptr_); 56 | led_node_ptr = std::make_shared(node_ptr_); 57 | bcin_node_ptr = std::make_unique( 58 | node_ptr_, 59 | std::bind( 60 | &MachineStateSwitchContext::BatteryChargeUpdate, mssc_context_ptr_, 61 | std::placeholders::_1, std::placeholders::_2), 62 | std::bind(&LedInfoNode::BmsStatus, led_node_ptr, std::placeholders::_1)); 63 | power_brd_node_ptr_ = std::make_unique(node_ptr_); 64 | executor_.add_node(node_ptr_); 65 | } 66 | 67 | cyberdog::manager::CyberdogManager::~CyberdogManager() 68 | { 69 | node_ptr_ = nullptr; 70 | } 71 | 72 | void cyberdog::manager::CyberdogManager::Config() 73 | { 74 | INFO("config state handler"); 75 | } 76 | 77 | bool cyberdog::manager::CyberdogManager::Init() 78 | { 79 | bool exit_lowpower = false; 80 | int exit_times = 0; 81 | while (!exit_lowpower && exit_times < 3) { 82 | if (power_consumption_node_ptr->QueryLowPower()) { 83 | exit_lowpower = power_consumption_node_ptr->EnterLowPower(false); 84 | INFO("exit lowpower:%s", (exit_lowpower ? "true" : "false")); 85 | std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 86 | } else { 87 | exit_lowpower = true; 88 | } 89 | ++exit_times; 90 | } 91 | query_node_ptr_->Report(true); 92 | query_node_ptr_->Init(); 93 | // 开始自检 94 | ready_node_ptr->SelfCheck(SelfcheckState::HARDWARE_CHECKING, selfcheck_status_); 95 | error_context_ptr_->Init(); 96 | // 硬件setup 97 | mssc_context_ptr_->ExecuteSetUp(false); 98 | mssc_context_ptr_->SetLowpowerEnterAndExitCallback( 99 | std::bind( 100 | &PowerConsumptionInfoNode::EnterLowPower, power_consumption_node_ptr, 101 | std::placeholders::_1)); 102 | mssc_context_ptr_->SetExceptionPlaySoundCallback( 103 | std::bind( 104 | &AudioInfoNode::SpeechNotify, audio_node_ptr, 105 | std::placeholders::_1)); 106 | ready_node_ptr->SetExceptionPlaySoundCallback( 107 | std::bind( 108 | &AudioInfoNode::SpeechNotify, audio_node_ptr, 109 | std::placeholders::_1)); 110 | mssc_context_ptr_->SetShutdownRebootCallback( 111 | std::bind( 112 | &PowerConsumptionInfoNode::ShutdownOrReboot, power_consumption_node_ptr, 113 | std::placeholders::_1)); 114 | mssc_context_ptr_->SetControlTailLedCallback( 115 | std::bind( 116 | &PowerConsumptionInfoNode::TailLedControl, power_consumption_node_ptr, 117 | std::placeholders::_1, std::placeholders::_2)); 118 | mssc_context_ptr_->SetControlLedShutdownCallback( 119 | std::bind( 120 | &LedInfoNode::ShutdownLightEffect, led_node_ptr, std::placeholders::_1)); 121 | error_context_ptr_->ClearError(); 122 | Config(); 123 | mssc_context_ptr_->ExecuteSelfCheck(selfcheck_status_); 124 | if (ready_node_ptr->IsSelfcheckError(selfcheck_status_)) { 125 | ERROR(">>>XXXXX--- hardware machine state self check error!"); 126 | if (ready_node_ptr->IsCriticalError(selfcheck_status_)) { 127 | ready_node_ptr->SelfCheck(SelfcheckState::CRITICAL_HARDWARE_FAILED, selfcheck_status_); 128 | ready_node_ptr->UploadEvents(); 129 | } else { 130 | ready_node_ptr->SelfCheck(SelfcheckState::UNCRITICAL_HARDWARE_FAILED, selfcheck_status_); 131 | OnActive(); 132 | while (1 == ready_node_ptr->GetAudioOccupancyState()) { 133 | INFO("Waiting for the audio to stop"); 134 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 135 | } 136 | audio_node_ptr->Init(); 137 | // 软件setup 138 | mssc_context_ptr_->ExecuteSetUp(true); 139 | OnActive(); 140 | ready_node_ptr->SelfCheck(SelfcheckState::SOFTWARE_SETUP_SUCCESS, selfcheck_status_); 141 | audio_node_ptr->SpeechNotify(5300); 142 | ready_node_ptr->UploadEvents(); 143 | } 144 | } else { 145 | OnActive(); 146 | ready_node_ptr->SelfCheck(SelfcheckState::HARDWARE_SUCCESS, selfcheck_status_); 147 | while (1 == ready_node_ptr->GetAudioOccupancyState()) { 148 | INFO("Waiting for the audio to stop"); 149 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 150 | } 151 | audio_node_ptr->Init(); 152 | // 软件setup 153 | mssc_context_ptr_->ExecuteSetUp(true); 154 | OnActive(); 155 | ready_node_ptr->SelfCheck(SelfcheckState::ALL_SUCCESS, selfcheck_status_); 156 | audio_node_ptr->SpeechNotify(5300); 157 | } 158 | power_consumption_node_ptr->Init(); 159 | mssc_context_ptr_->Init(); 160 | heart_beat_ptr_->Init(); 161 | bcin_node_ptr->Init(); 162 | ready_node_ptr->StoreSelfcheckResults(); 163 | return true; 164 | } 165 | 166 | bool cyberdog::manager::CyberdogManager::SetState(int8_t state, std::string json_data) 167 | { 168 | (void) state; 169 | (void) json_data; 170 | return true; 171 | } 172 | 173 | void cyberdog::manager::CyberdogManager::Run() 174 | { 175 | executor_.spin(); 176 | rclcpp::shutdown(); 177 | } 178 | 179 | void cyberdog::manager::CyberdogManager::OnActive() 180 | { 181 | INFO("trigger state:on active"); 182 | bool result = mssc_context_ptr_->ExecuteActive(); 183 | mssc_context_ptr_->KeepMsState(); 184 | if (result) { 185 | INFO("!!! All node in detected machine state is acitve ok !!!"); 186 | ready_node_ptr->Ready(true); 187 | ready_node_ptr->MachineState(0); 188 | } else { 189 | // audio_node_ptr->Error("自检失败!自检失败!自检失败!"); 190 | ready_node_ptr->Ready(false); 191 | ready_node_ptr->MachineState(-2); 192 | } 193 | // bcin_node_ptr->SetBms(BatteryMachineState::BMS_NORMAL); 194 | } 195 | -------------------------------------------------------------------------------- /user_info_manager/test/test.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 "std_msgs/msg/string.hpp" 21 | #include "user_info_manager/UserAccountManager.hpp" 22 | #include "cyberdog_common/cyberdog_log.hpp" 23 | #include "cyberdog_common/cyberdog_json.hpp" 24 | 25 | // class TestSubscribe : public rclcpp::Node 26 | // { 27 | // public: 28 | // explicit TestSubscribe(std::string name) 29 | // : Node(name) 30 | // { 31 | // RCLCPP_INFO(this->get_logger(), "TEST NODE STARTED %s.", name.c_str()); 32 | // // 订阅话题，增加用户 33 | // account_add_subscribe_ = this->create_subscription( 34 | // "account_add", 10, std::bind( 35 | // &TestSubscribe::account_add_callback, 36 | // this, std::placeholders::_1)); 37 | // // 订阅话题，搜索所有用户信息 38 | // account_search_all_subscribe_ = this->create_subscription( 39 | // "account_search_all", 10, std::bind( 40 | // &TestSubscribe::account_search_all_callback, 41 | // this, std::placeholders::_1)); 42 | // // 订阅话题，搜索单个用户信息 43 | // account_search_one_subscribe_ = this->create_subscription( 44 | // "account_search_one", 10, std::bind( 45 | // &TestSubscribe::account_search_one_callback, 46 | // this, std::placeholders::_1)); 47 | // // 订阅话题，删除一个用户 48 | // account_delete_subscribe_ = this->create_subscription( 49 | // "account_delete", 10, std::bind( 50 | // &TestSubscribe::account_delete_callback, 51 | // this, std::placeholders::_1)); 52 | // // 订阅话题，修改用户的voice或face状态(只能改一个) 53 | // account_modify_voice_or_face_subscribe_ = this->create_subscription( 54 | // "account_modify_voice_or_face", 10, std::bind( 55 | // &TestSubscribe:: 56 | // account_modify_voice_or_face_callback, this, std::placeholders::_1)); 57 | 58 | // // 订阅话题，同时修改用户的voice和face状态 59 | // account_modify_voice_and_face_subscribe_ = this->create_subscription( 60 | // "account_modify_voice_and_face", 10, std::bind( 61 | // &TestSubscribe:: 62 | // account_modify_voice_and_face_callback, this, std::placeholders::_1)); 63 | // // 订阅话题，将用户的voice状态置0 64 | // account_modify_voice_to0_subscribe_ = this->create_subscription( 65 | // "account_modify_voice_to0", 10, std::bind( 66 | // &TestSubscribe:: 67 | // account_modify_voice_to0_callback, this, std::placeholders::_1)); 68 | 69 | // // 订阅话题，将用户的face状态置0 70 | // account_modify_face_to0_subscribe_ = this->create_subscription( 71 | // "account_modify_face_to0", 10, std::bind( 72 | // &TestSubscribe::account_modify_face_to0_callback, 73 | // this, std::placeholders::_1)); 74 | // } 75 | 76 | // private: 77 | // // 声明订阅者 78 | // rclcpp::Subscription::SharedPtr account_add_subscribe_; 79 | // rclcpp::Subscription::SharedPtr account_search_all_subscribe_; 80 | // rclcpp::Subscription::SharedPtr account_search_one_subscribe_; 81 | // rclcpp::Subscription::SharedPtr account_delete_subscribe_; 82 | // rclcpp::Subscription::SharedPtr account_modify_voice_or_face_subscribe_; 83 | // rclcpp::Subscription::SharedPtr account_modify_voice_and_face_subscribe_; 84 | // rclcpp::Subscription::SharedPtr account_modify_voice_to0_subscribe_; 85 | // rclcpp::Subscription::SharedPtr account_modify_face_to0_subscribe_; 86 | 87 | // // 回调函数，增加用户 88 | // void account_add_callback(const std_msgs::msg::String::SharedPtr msg) 89 | // { 90 | // cyberdog::common::CyberdogAccountManager obj; 91 | // std::string name = msg->data; 92 | // INFO("account name %s", msg->data.c_str()); 93 | 94 | // auto result = obj.AddMember(msg->data); 95 | // INFO("AddMember() return is %d", result); 96 | // } 97 | 98 | // // 回调函数，搜索所有用户 99 | // void account_search_all_callback(const std_msgs::msg::String::SharedPtr msg) 100 | // { 101 | // INFO("search all accoiunt: %s", msg); 102 | // cyberdog::common::CyberdogAccountManager obj; 103 | // std::vector member_data; 104 | 105 | // auto result = obj.SearAllUser(member_data); 106 | // INFO("SearAllUser() return is %d", result); 107 | 108 | // rapidjson::Document json_info(rapidjson::kObjectType); 109 | // rapidjson::Value arr(rapidjson::kArrayType); 110 | // std::string data; 111 | 112 | // for (unsigned int i = 0; i < member_data.size(); ++i) { 113 | // rapidjson::Value js_obj(rapidjson::kObjectType); 114 | // rapidjson::Value value(member_data[i].username.c_str(), json_info.GetAllocator()); 115 | // js_obj.AddMember("name", value, json_info.GetAllocator()); 116 | // js_obj.AddMember("face_state", member_data[i].faceStatus, json_info.GetAllocator()); 117 | // js_obj.AddMember("voice_state", member_data[i].voiceStatus, json_info.GetAllocator()); 118 | // arr.PushBack(js_obj, json_info.GetAllocator()); 119 | // } 120 | // json_info.AddMember("accounts", arr, json_info.GetAllocator()); 121 | // cyberdog::common::CyberdogJson::Document2String(json_info, data); 122 | // INFO("All account info is: %s", data.c_str()); 123 | // } 124 | 125 | // // 回调函数，搜索单一用户 126 | // void account_search_one_callback(const std_msgs::msg::String::SharedPtr msg) 127 | // { 128 | // cyberdog::common::CyberdogAccountManager obj; 129 | // std::string name = msg->data; 130 | // INFO("account name is %s", msg->data.c_str()); 131 | 132 | // int state[2]; 133 | 134 | // auto result = obj.SearchUser(name, state); 135 | // INFO("SearchUser() return is %d", result); 136 | 137 | // INFO("account name:%s,voice_state:%d,face_stare:%d", name.c_str(), state[0], state[1]); 138 | // } 139 | 140 | // // 回调函数，删除用户 141 | // void account_delete_callback(const std_msgs::msg::String::SharedPtr msg) 142 | // { 143 | // cyberdog::common::CyberdogAccountManager obj; 144 | // std::string name = msg->data; 145 | // INFO("account name is %s", msg->data.c_str()); 146 | 147 | // auto result = obj.DeleteUserInformation(name); 148 | // INFO("SearchOneUser() return is %d", result); 149 | // } 150 | 151 | // // 回调函数，修改用户voice或face的状态 152 | // void account_modify_voice_or_face_callback(const std_msgs::msg::String::SharedPtr msg) 153 | // { 154 | // cyberdog::common::CyberdogAccountManager obj; 155 | // std::string name = msg->data; 156 | // int voice_state = 4; 157 | // int face_state = 8; 158 | // INFO("account name is %s", msg->data.c_str()); 159 | // auto result = obj.ModifyUserInformation(name, voice_state, 0); 160 | // auto result2 = obj.ModifyUserInformation(name, face_state, 1); 161 | // INFO("ModifyUserInformation() modify voice return is %d", result); 162 | // INFO("ModifyUserInformation() modify face return is %d", result2); 163 | // } 164 | 165 | // // 回调函数，同时修改用户voice和face状态 166 | // void account_modify_voice_and_face_callback(const std_msgs::msg::String::SharedPtr msg) 167 | // { 168 | // cyberdog::common::CyberdogAccountManager obj; 169 | // std::string name = msg->data; 170 | // int voice_state = 9; 171 | // int face_state = 6; 172 | // INFO("account name is %s", msg->data.c_str()); 173 | // auto result = obj.ModifyInformation(msg->data, voice_state, face_state); 174 | // INFO("ModifyInformation() return is %d", result); 175 | // } 176 | 177 | // // 回调函数，将用户face状态置0 178 | // void account_modify_face_to0_callback(const std_msgs::msg::String::SharedPtr msg) 179 | // { 180 | // cyberdog::common::CyberdogAccountManager obj; 181 | // std::string name = msg->data; 182 | // INFO("account name is %s", msg->data.c_str()); 183 | // auto result = obj.DeleteFace(msg->data); 184 | // INFO("DeleteFace() return is %d", result); 185 | // } 186 | 187 | // // 回调函数，将用户voice状态置0 188 | // void account_modify_voice_to0_callback(const std_msgs::msg::String::SharedPtr msg) 189 | // { 190 | // cyberdog::common::CyberdogAccountManager obj; 191 | // INFO("account name is %s", msg->data.c_str()); 192 | // auto result = obj.DeleteVoice(msg->data); 193 | // INFO("DeleteVoice() return is %d", result); 194 | // } 195 | // }; 196 | 197 | int main(int argc, char ** argv) 198 | { 199 | // rclcpp::init(argc, argv); 200 | // /*产生一个的节点*/ 201 | // auto node = std::make_shared("topic_subscribe_01"); 202 | // /* 运行节点，并检测退出信号*/ 203 | // rclcpp::spin(node); 204 | // rclcpp::shutdown(); 205 | return 0; 206 | } 207 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/power_brd_info.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 CYBERDOG_MANAGER__POWER_BRD_INFO_HPP_ 16 | #define CYBERDOG_MANAGER__POWER_BRD_INFO_HPP_ 17 | 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "std_srvs/srv/trigger.hpp" 31 | #include "protocol/srv/motion_result_cmd.hpp" 32 | #include "protocol/srv/bes_http_send_file.hpp" 33 | #include "rclcpp/rclcpp.hpp" 34 | #include "cyberdog_common/cyberdog_log.hpp" 35 | 36 | namespace cyberdog 37 | { 38 | namespace manager 39 | { 40 | class PowerboardIndoNode 41 | { 42 | public: 43 | explicit PowerboardIndoNode(rclcpp::Node::SharedPtr node_ptr) 44 | : power_brd_info_node_(node_ptr) 45 | { 46 | power_brd_callback_group_ = 47 | power_brd_info_node_->create_callback_group( 48 | rclcpp::CallbackGroupType::MutuallyExclusive); 49 | 50 | motion_excute_client_ = 51 | power_brd_info_node_->create_client( 52 | "motion_result_cmd", 53 | rmw_qos_profile_services_default, power_brd_callback_group_); 54 | 55 | shutdown_client_ = 56 | power_brd_info_node_->create_client( 57 | "poweroff", 58 | rmw_qos_profile_services_default, power_brd_callback_group_); 59 | 60 | bes_http_send_event_client_ = 61 | power_brd_info_node_->create_client< 62 | protocol::srv::BesHttpSendFile>( 63 | "bes_http_send_file_srv", 64 | rmw_qos_profile_services_default, power_brd_callback_group_); 65 | Init(); 66 | } 67 | 68 | void Init() 69 | { 70 | std::thread power_brd_thread(&PowerboardIndoNode::PowerBrdProcess, this, nullptr); 71 | power_brd_thread.detach(); 72 | } 73 | 74 | private: 75 | int openInput(const std::string & input_name) 76 | { 77 | int fd = -1; 78 | const char * dirname = "/dev/input"; 79 | char devname[64]; 80 | char * filename; 81 | DIR * dir; 82 | struct dirent * de; 83 | dir = opendir(dirname); 84 | if (dir == nullptr) { 85 | INFO("[power_bre_button]: opendir error code: %d,%s", errno, strerror(errno)); 86 | return -1; 87 | } 88 | 89 | snprintf(devname, strlen(dirname) + 1, "%s", dirname); 90 | filename = devname + strlen(devname); 91 | *filename++ = '/'; 92 | while ((de = readdir(dir))) { 93 | if (de->d_name[0] == '.' && 94 | (de->d_name[1] == '\0' || (de->d_name[1] == '.' && de->d_name[2] == '\0'))) 95 | { 96 | continue; 97 | } 98 | snprintf(filename, strlen(de->d_name) + 1, "%s", de->d_name); 99 | fd = open(devname, O_RDONLY | O_NONBLOCK); 100 | if (fd >= 0) { 101 | char name[80]; 102 | if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) { 103 | name[0] = '\0'; 104 | } 105 | 106 | if (!strcmp(name, input_name.c_str())) { 107 | INFO("get wanted input device(%s)", devname); 108 | break; 109 | } else { 110 | close(fd); 111 | fd = -1; 112 | } 113 | } else { 114 | INFO("[power_button]: input device open error code: %d,%s", errno, strerror(errno)); 115 | } 116 | } 117 | closedir(dir); 118 | return fd; 119 | } 120 | 121 | int WaitPowerKeyEvent(int fd, int time_sec, int time_ms) 122 | { 123 | fd_set rdfds; 124 | struct timeval tv; 125 | int ret; 126 | FD_ZERO(&rdfds); 127 | FD_SET(fd, &rdfds); 128 | tv.tv_sec = time_sec; 129 | tv.tv_usec = time_ms * 1000; 130 | ret = select(fd + 1, &rdfds, nullptr, nullptr, &tv); 131 | if (ret < 0) { 132 | return -1; 133 | } else if (ret == 0) { 134 | return 0; 135 | } 136 | return 1; 137 | } 138 | 139 | void PowerBrdProcess(void * pd) 140 | { 141 | (void)pd; 142 | struct input_event e, esync; 143 | int ret; 144 | // pthread_t id; 145 | 146 | int fd = openInput("gpio-keys"); 147 | if (fd < 0) { 148 | INFO("[power_button]: open powerkey dev err:%s", strerror(errno)); 149 | return; 150 | } 151 | 152 | while (1) { 153 | if (WaitPowerKeyEvent(fd, 1, 0) <= 0) { 154 | continue; 155 | } 156 | ret = read(fd, &e, sizeof(e)); 157 | if (ret < 0) { 158 | continue; 159 | } 160 | read(fd, &esync, sizeof(e)); // read sync event 161 | if (e.type == EV_KEY) { 162 | if (e.value == 1) { // press down 163 | if (WaitPowerKeyEvent(fd, 3, 0) <= 0) { 164 | INFO("[power_button]:powerkey long press"); 165 | // control_led_shutdown(true); 166 | // INFO("call led finsihed"); 167 | // MotionContrl(102); // Control the dog to lie down 168 | SendEvent("shutdown by long pressing the powerkey"); 169 | Shutdown(); 170 | std::this_thread::sleep_for(std::chrono::seconds(1)); 171 | } else { 172 | INFO("[power_button]:powerkey short press"); 173 | } 174 | } else {} // press up 175 | } 176 | } 177 | } 178 | 179 | bool MotionContrl(const int motion_id) 180 | { 181 | if (!motion_excute_client_->wait_for_service(std::chrono::seconds(3))) { 182 | ERROR("[power_button]:call led_excute server not avalible"); 183 | return false; 184 | } 185 | auto request_motion = std::make_shared(); 186 | request_motion->motion_id = motion_id; 187 | request_motion->cmd_source = 0; 188 | auto future_result_motion = motion_excute_client_->async_send_request(request_motion); 189 | std::future_status status_motion = future_result_motion.wait_for(std::chrono::seconds(10)); 190 | if (status_motion == std::future_status::timeout) { 191 | ERROR("[power_button]:call motion service failed"); 192 | } 193 | if (future_result_motion.get()->code != 0) { 194 | ERROR( 195 | "[power_button]:control motion fialed, error code is:%d", 196 | future_result_motion.get()->code); 197 | } 198 | INFO("[power_button]:control motion success, return is %d", future_result_motion.get()->code); 199 | return true; 200 | } 201 | 202 | bool Shutdown() 203 | { 204 | // 关机服务先返回结果，再执行状态切换及关机操作 205 | if (!shutdown_client_->wait_for_service(std::chrono::seconds(5))) { 206 | ERROR("call shutdown server not avalible"); 207 | return false; 208 | } 209 | auto request_shutdown = std::make_shared(); 210 | auto future_result_shutdown = shutdown_client_->async_send_request(request_shutdown); 211 | std::future_status status_shutdown = future_result_shutdown.wait_for(std::chrono::seconds(10)); 212 | if (status_shutdown == std::future_status::timeout) { 213 | ERROR("call shutdown service failed"); 214 | } 215 | if (future_result_shutdown.get()->success != true) { 216 | ERROR("shutdown fialed, error code is %d", future_result_shutdown.get()->success); 217 | } 218 | return 0; 219 | } 220 | 221 | bool SendEvent(const std::string & msg) 222 | { 223 | if (!bes_http_send_event_client_->wait_for_service(std::chrono::seconds(3))) { 224 | WARN("bes_http_send_file_srv server not avalible"); 225 | return false; 226 | } 227 | int64_t now_ms = std::chrono::duration_cast( 228 | std::chrono::system_clock::now().time_since_epoch()).count(); 229 | std::string event = "{" + std::string("\"type\": \"shutdown\"") + 230 | ", \"info\": " + msg + std::string(", \"timestamp\": ") + 231 | std::to_string(now_ms) + "}"; 232 | auto request = std::make_shared(); 233 | request->method = protocol::srv::BesHttpSendFile::Request::HTTP_METHOD_POST; 234 | request->url = "device/system/log"; 235 | request->info = event; 236 | request->milsecs = 10000; // 10s 237 | auto future_result = bes_http_send_event_client_->async_send_request(request); 238 | std::future_status status = future_result.wait_for(std::chrono::seconds(5)); 239 | if (status != std::future_status::ready) { 240 | WARN("call bes_http_send_file_srv service failed"); 241 | return false; 242 | } 243 | INFO("successed send event of shoutdown"); 244 | return true; 245 | } 246 | 247 | private: 248 | rclcpp::Node::SharedPtr power_brd_info_node_{nullptr}; 249 | rclcpp::CallbackGroup::SharedPtr power_brd_callback_group_; 250 | rclcpp::Client::SharedPtr motion_excute_client_; 251 | rclcpp::Client::SharedPtr shutdown_client_; 252 | rclcpp::Client::SharedPtr bes_http_send_event_client_; 253 | }; 254 | } // namespace manager 255 | } // namespace cyberdog 256 | 257 | 258 | #endif // CYBERDOG_MANAGER__POWER_BRD_INFO_HPP_ 259 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/led_info.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 CYBERDOG_MANAGER__LED_INFO_HPP_ 15 | #define CYBERDOG_MANAGER__LED_INFO_HPP_ 16 | 17 | #include 18 | #include 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "std_msgs/msg/bool.hpp" 21 | #include "protocol/srv/led_execute.hpp" 22 | #include "protocol/msg/bms_status.hpp" 23 | #include "protocol/msg/motion_status.hpp" 24 | #include "cyberdog_common/cyberdog_log.hpp" 25 | 26 | namespace cyberdog 27 | { 28 | namespace manager 29 | { 30 | struct LedMode 31 | { 32 | bool occupation; 33 | std::string client; 34 | uint8_t target; 35 | uint8_t mode; 36 | uint8_t effect; 37 | uint8_t r_value; 38 | uint8_t g_value; 39 | uint8_t b_value; 40 | }; 41 | 42 | class LedInfoNode final 43 | { 44 | public: 45 | explicit LedInfoNode(rclcpp::Node::SharedPtr node_ptr) 46 | : led_info_node_(node_ptr) 47 | { 48 | led_callback_group_ = 49 | led_info_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 50 | led_excute_client_ = 51 | led_info_node_->create_client( 52 | "led_execute", 53 | rmw_qos_profile_services_default, led_callback_group_); 54 | rclcpp::SubscriptionOptions sub_options; 55 | sub_options.callback_group = led_callback_group_; 56 | } 57 | 58 | void BmsStatus(const protocol::msg::BmsStatus::SharedPtr msg) 59 | { 60 | power_charging_ = msg->power_wired_charging; 61 | battery_soc_ = msg->batt_soc; 62 | static int start_battery_soc = battery_soc_; 63 | int charging_status_switch = false; 64 | 65 | // 充电状态切换flag 66 | if (pre_charging_status_ != power_charging_) { 67 | charging_status_switch = true; 68 | } 69 | // 非充电状态，Bms释放灯效 70 | if (battery_soc_ > 20) { 71 | if (!power_charging_ && !bms_occupied_led_) { 72 | bms_occupied_led_ = true; 73 | LedMode head{false, "bms", 1, 0x02, 0x09, 0xFF, 0x32, 0x32}; 74 | LedMode tail{false, "bms", 2, 0x02, 0x09, 0xFF, 0x32, 0x32}; 75 | LedMode mini{false, "bms", 3, 0x02, 0x30, 0xFF, 0x32, 0x32}; 76 | bool result = ReqLedService(head, tail, mini); 77 | INFO("Bms %s to release led occupation ", result ? "successed" : "failed"); 78 | } 79 | if (power_charging_) { 80 | bms_occupied_led_ = false; 81 | } 82 | } 83 | 84 | if ((pre_battery_soc_ == 1 && battery_soc_ == 0) || start_battery_soc == 0) { 85 | if (!power_charging_) { 86 | bool result = ShutdownLightEffect(true); 87 | INFO("%s set led when the soc is 0", result ? "successed" : "failed"); 88 | } 89 | } 90 | 91 | if ((pre_battery_soc_ == 21 && battery_soc_ == 20) || 92 | (start_battery_soc > 0 && start_battery_soc <= 20) || 93 | (charging_status_switch && battery_soc_ <= 20)) 94 | { 95 | if (power_charging_) { 96 | LedMode head{true, "bms", 1, 0x02, 0x06, 0xFF, 0x32, 0x32}; 97 | LedMode tail{true, "bms", 2, 0x02, 0x06, 0xFF, 0x32, 0x32}; 98 | LedMode mini{true, "bms", 3, 0x02, 0x30, 0xFF, 0x00, 0x00}; 99 | bool result = ReqLedService(head, tail, mini); 100 | INFO("%s set the charging led when the soc less than 20", result ? "successed" : "failed"); 101 | } else { 102 | LedMode head{true, "bms", 1, 0x02, 0x09, 0xFF, 0x32, 0x32}; 103 | LedMode tail{true, "bms", 2, 0x02, 0x09, 0xFF, 0x32, 0x32}; 104 | LedMode mini{true, "bms", 3, 0x02, 0x30, 0xFF, 0x00, 0x00}; 105 | bool result = ReqLedService(head, tail, mini); 106 | INFO("%s set led when the soc less than 20", result ? "successed" : "failed"); 107 | } 108 | } 109 | 110 | if ((pre_battery_soc_ == 20 && battery_soc_ == 21) || 111 | (pre_battery_soc_ == 80 && battery_soc_ == 79) || 112 | (start_battery_soc > 20 && start_battery_soc < 80) || 113 | (charging_status_switch && battery_soc_ > 20 && battery_soc_ < 80)) 114 | { 115 | if (power_charging_) { 116 | LedMode head{true, "bms", 1, 0x02, 0x06, 0x75, 0xFC, 0xF6}; 117 | LedMode tail{true, "bms", 2, 0x02, 0x06, 0x75, 0xFC, 0xF6}; 118 | LedMode mini{true, "bms", 3, 0x02, 0x30, 0x75, 0xFC, 0xF6}; 119 | bool result = ReqLedService(head, tail, mini); 120 | INFO( 121 | "%s set the charging less, the soc more than 20 and less than 80 with charing", 122 | result ? "successed" : "failed"); 123 | } else { 124 | // 释放Bms灯效 125 | LedMode bringup_head{false, "bms", 1, 0x02, 0x09, 0xFF, 0x32, 0x32}; 126 | LedMode bringup_tail{false, "bms", 2, 0x02, 0x09, 0xFF, 0x32, 0x32}; 127 | LedMode bringup_mini{false, "bms", 3, 0x02, 0x30, 0xFF, 0x32, 0x32}; 128 | bool result = ReqLedService(bringup_head, bringup_tail, bringup_mini); 129 | INFO("bms %s release led when the soc less than 80", result ? "successed" : "failed"); 130 | } 131 | // 更改系统灯效 132 | LedMode sys_bringup_head{true, "system", 1, 0x02, 0x09, 0x75, 0xFC, 0xF6}; 133 | LedMode sys_bringup_tail{true, "system", 2, 0x02, 0x09, 0x75, 0xFC, 0xF6}; 134 | LedMode sys_bringup_mini{true, "system", 3, 0x02, 0x30, 0x75, 0xFC, 0xF6}; 135 | bool result2 = ReqLedService(sys_bringup_head, sys_bringup_tail, sys_bringup_mini); 136 | INFO( 137 | "%s set sys led, the soc more than 20 an less than 80", 138 | result2 ? "successed" : "failed"); 139 | } 140 | 141 | if ((pre_battery_soc_ == 79 && battery_soc_ == 80) || 142 | start_battery_soc >= 80 || (charging_status_switch && battery_soc_ >= 80)) 143 | { 144 | // 更改系统灯效 145 | LedMode bringup_head{true, "system", 1, 0x02, 0x09, 0x06, 0x21, 0xE2}; 146 | LedMode bringup_tail{true, "system", 2, 0x02, 0x09, 0x06, 0x21, 0xE2}; 147 | LedMode bringup_mini{true, "system", 3, 0x02, 0x30, 0x06, 0x21, 0xE2}; 148 | bool result = ReqLedService(bringup_head, bringup_tail, bringup_mini); 149 | INFO("%s set sys led, the soc more than 80", result ? "successed" : "failed"); 150 | if (power_charging_) { 151 | LedMode bringup_head{true, "bms", 1, 0x02, 0x06, 0x06, 0x21, 0xE2}; 152 | LedMode bringup_tail{true, "bms", 2, 0x02, 0x06, 0x06, 0x21, 0xE2}; 153 | LedMode bringup_mini{true, "bms", 3, 0x02, 0x30, 0x06, 0x21, 0xE2}; 154 | bool result = ReqLedService(bringup_head, bringup_tail, bringup_mini); 155 | INFO( 156 | "%s set the charging, the soc more than 80 with charing", 157 | result ? "successed" : "failed"); 158 | } 159 | } 160 | start_battery_soc = -1; 161 | pre_battery_soc_ = battery_soc_; 162 | pre_charging_status_ = power_charging_; 163 | } 164 | 165 | bool ShutdownLightEffect(bool trigger) 166 | { 167 | WARN("watiting for shutdown light effect"); 168 | LedMode poweroff_head{trigger, "bms", 1, 0x01, 0xA3, 0x00, 0x00, 0x00}; 169 | LedMode poweroff_tail{trigger, "bms", 2, 0x01, 0xA3, 0x00, 0x00, 0x00}; 170 | LedMode poweroff_mini{trigger, "bms", 3, 0x02, 0x31, 0xFF, 0x00, 0x00}; 171 | return ReqLedService(poweroff_head, poweroff_tail, poweroff_mini); 172 | } 173 | 174 | private: 175 | void ReqAssignment(std::shared_ptr req, LedMode & data) 176 | { 177 | req->occupation = data.occupation; 178 | req->client = data.client; 179 | req->target = data.target; 180 | req->mode = data.mode; 181 | req->effect = data.effect; 182 | req->r_value = data.r_value; 183 | req->g_value = data.g_value; 184 | req->b_value = data.b_value; 185 | } 186 | 187 | bool ReqLedService(LedMode & head, LedMode & tail, LedMode & mini) 188 | { 189 | if (!led_excute_client_->wait_for_service(std::chrono::seconds(2))) { 190 | ERROR("call led_excute server not avalible"); 191 | return false; 192 | } 193 | 194 | auto request_led = std::make_shared(); 195 | ReqAssignment(request_led, head); 196 | auto future_result_head = led_excute_client_->async_send_request(request_led); 197 | std::future_status status_head = future_result_head.wait_for(std::chrono::seconds(2)); 198 | 199 | ReqAssignment(request_led, tail); 200 | auto future_result_tail = led_excute_client_->async_send_request(request_led); 201 | std::future_status status_tail = future_result_head.wait_for(std::chrono::seconds(2)); 202 | 203 | ReqAssignment(request_led, mini); 204 | auto future_result_mini = led_excute_client_->async_send_request(request_led); 205 | std::future_status status_mini = future_result_mini.wait_for(std::chrono::seconds(2)); 206 | 207 | if (status_head != std::future_status::ready || 208 | status_tail != std::future_status::ready || 209 | status_mini != std::future_status::ready) 210 | { 211 | INFO("call led_execute service failed"); 212 | return false; 213 | } 214 | 215 | if (future_result_head.get()->code == 0 && 216 | future_result_tail.get()->code == 0 && 217 | future_result_mini.get()->code == 0) 218 | { 219 | INFO("call led service successed"); 220 | return true; 221 | } else { 222 | INFO( 223 | "call led service fialed, error code[head, tail, mini] is:%d %d %d", 224 | future_result_head.get()->code, 225 | future_result_tail.get()->code, 226 | future_result_mini.get()->code); 227 | return false; 228 | } 229 | } 230 | 231 | bool ReqLedService(LedMode & tail) 232 | { 233 | if (!led_excute_client_->wait_for_service(std::chrono::seconds(2))) { 234 | ERROR("call led_excute server not avalible"); 235 | return false; 236 | } 237 | auto request_led = std::make_shared(); 238 | ReqAssignment(request_led, tail); 239 | auto future_result_tail = led_excute_client_->async_send_request(request_led); 240 | std::future_status status_tail = future_result_tail.wait_for(std::chrono::seconds(2)); 241 | 242 | if (status_tail != std::future_status::ready) { 243 | INFO("call led_execute service failed"); 244 | return false; 245 | } 246 | 247 | if (future_result_tail.get()->code == 0) { 248 | INFO("call led service successed"); 249 | return true; 250 | } else { 251 | INFO( 252 | "call led service fialed, error code[tail] is: %d", 253 | future_result_tail.get()->code); 254 | return false; 255 | } 256 | } 257 | 258 | private: 259 | rclcpp::Node::SharedPtr led_info_node_{nullptr}; 260 | rclcpp::CallbackGroup::SharedPtr led_callback_group_; 261 | rclcpp::Subscription::SharedPtr wake_up_sub_ {nullptr}; 262 | rclcpp::Client::SharedPtr led_excute_client_ {nullptr}; 263 | uint8_t battery_soc_ {100}; 264 | uint8_t pre_battery_soc_ {0}; 265 | bool power_charging_ {false}; 266 | bool pre_charging_status_ {false}; 267 | bool bms_occupied_led_ {false}; 268 | }; 269 | } // namespace manager 270 | } // namespace cyberdog 271 | 272 | #endif // CYBERDOG_MANAGER__LED_INFO_HPP_ 273 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/power_consumption_info.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 CYBERDOG_MANAGER__POWER_CONSUMPTION_INFO_HPP_ 15 | #define CYBERDOG_MANAGER__POWER_CONSUMPTION_INFO_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "std_srvs/srv/set_bool.hpp" 23 | #include "std_srvs/srv/trigger.hpp" 24 | #include "protocol/srv/led_execute.hpp" 25 | #include "protocol/msg/motion_status.hpp" 26 | #include "low_power_consumption/low_power_consumption.hpp" 27 | #include "protocol/msg/state_switch_status.hpp" 28 | #include "protocol/msg/audio_play_extend.hpp" 29 | #include "protocol/srv/motion_result_cmd.hpp" 30 | 31 | namespace cyberdog 32 | { 33 | namespace manager 34 | { 35 | enum class PowerMachineState : uint8_t 36 | { 37 | PMS_NORMAL = 0, // 正常 38 | PMS_PROTECT = 1, // 保护 39 | PMS_LOWPOWER = 2, // 低功耗 40 | PMS_UNKOWN = 255, // 未知 41 | }; 42 | 43 | class PowerConsumptionInfoNode final 44 | { 45 | using PCIN_CALLBACK = std::function; 46 | 47 | public: 48 | explicit PowerConsumptionInfoNode(rclcpp::Node::SharedPtr node_ptr, PCIN_CALLBACK callback) 49 | : enter_lowpower_handler(callback) 50 | { 51 | power_consumption_info_node_ = node_ptr; 52 | lpc_ptr_ = std::make_unique(); 53 | power_consumption_callback_group_ = 54 | power_consumption_info_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 55 | rclcpp::PublisherOptions pub_options; 56 | pub_options.callback_group = power_consumption_callback_group_; 57 | led_excute_client_ = 58 | power_consumption_info_node_->create_client( 59 | "led_execute", 60 | rmw_qos_profile_services_default, power_consumption_callback_group_); 61 | motion_excute_client_ = 62 | power_consumption_info_node_->create_client( 63 | "motion_result_cmd", 64 | rmw_qos_profile_services_default, power_consumption_callback_group_); 65 | dog_leg_calibration_srv_ = 66 | power_consumption_info_node_->create_service( 67 | "dog_leg_calibration", 68 | std::bind( 69 | &PowerConsumptionInfoNode::MotorControl, this, std::placeholders::_1, 70 | std::placeholders::_2), 71 | rmw_qos_profile_services_default, power_consumption_callback_group_); 72 | audio_play_extend_pub_ = 73 | power_consumption_info_node_->create_publisher( 74 | "speech_play_extend", 75 | rclcpp::SystemDefaultsQoS(), pub_options); 76 | 77 | // sub motion init 78 | rclcpp::SubscriptionOptions options; 79 | options.callback_group = power_consumption_callback_group_; 80 | state_swith_status_sub_ = power_consumption_info_node_-> 81 | create_subscription( 82 | "state_switch_status", rclcpp::SystemDefaultsQoS(), std::bind( 83 | &PowerConsumptionInfoNode::sub_state_switch_status_callback, 84 | this, std::placeholders::_1), options); 85 | } 86 | 87 | void Init() 88 | { 89 | // sub motion init 90 | rclcpp::SubscriptionOptions options; 91 | options.callback_group = power_consumption_callback_group_; 92 | motion_status_sub_ = power_consumption_info_node_-> 93 | create_subscription( 94 | "motion_status", rclcpp::SystemDefaultsQoS(), std::bind( 95 | &PowerConsumptionInfoNode::sub_mostion_status_callback, 96 | this, std::placeholders::_1), options); 97 | start = std::chrono::steady_clock::now(); 98 | } 99 | 100 | bool EnterLowPower(bool is_enter) 101 | { 102 | static int r_count = 0; 103 | PM_DEV pd = PM_ALL_NO_TOF; 104 | PM_DEV turn_on_led = PM_TOF; 105 | unsigned int err; 106 | int code = -1; 107 | bool result = false; 108 | if (is_enter) { 109 | INFO("[LowPower]: [%d]LowPower enter inner-call:start", (r_count + 1)); 110 | code = lpc_ptr_->LpcRelease(pd, &err); 111 | if (code == 0) { 112 | is_lowpower_ = true; 113 | INFO("[LowPower]: [%d]LowPower enter inner-call:success", (r_count + 1)); 114 | result = true; 115 | } else { 116 | INFO( 117 | "[LowPower]: [%d]LowPower enter inner-call:failed! error code is %d", 118 | (r_count + 1), code); 119 | } 120 | ++r_count; 121 | } else { 122 | INFO("[LowPower]: [%d]LowPower exit inner-call:start", (r_count + 1)); 123 | code = lpc_ptr_->LpcRequest(turn_on_led, &err); 124 | TailLedControl(true, true); 125 | code = lpc_ptr_->LpcRequest(pd, &err); 126 | if (code == 0) { 127 | start = std::chrono::steady_clock::now(); 128 | is_lowpower_ = false; 129 | INFO("[LowPower]: [%d]LowPower exit inner-call:success", (r_count + 1)); 130 | result = true; 131 | } else { 132 | INFO( 133 | "[LowPower]: [%d]LowPower exit inner-call:failed! error code is %d", 134 | (r_count + 1), code); 135 | } 136 | ++r_count; 137 | } 138 | return result; 139 | } 140 | 141 | void MotorControl( 142 | const std_srvs::srv::SetBool::Request::SharedPtr request, 143 | std_srvs::srv::SetBool::Response::SharedPtr response) 144 | { 145 | if (!motor_mutex_.try_lock()) { 146 | WARN( 147 | "The motor is powering %s and rejecting duplicate requests", 148 | (request->data ? "up" : "down")); 149 | return; 150 | } 151 | PM_DEV pd = PM_MOTOR; 152 | int code = -1; 153 | unsigned int err; 154 | if (!request->data) { 155 | // 控制铁蛋高阻尼趴下,电机下电 156 | PlayAudio("铁蛋即将趴下后进行断电，请注意安全"); 157 | sleep(4); 158 | MotionContrl(102); 159 | code = lpc_ptr_->LpcRelease(pd, &err); 160 | response->success = (code ? false : true); 161 | INFO("motor shutdown is %s, code is %d", (code ? "failed" : "successed"), code); 162 | if (code != 0) { 163 | return; 164 | } 165 | PlayAudio("断电成功，请帮我把腿摆放正常后，点击上电"); 166 | } else { 167 | // 电机上电 168 | code = lpc_ptr_->LpcRequest(pd, &err); 169 | response->success = (code ? false : true); 170 | if (code != 0) { 171 | return; 172 | } 173 | PlayAudio("铁蛋上电成功后将站立，请注意安全"); 174 | sleep(3); 175 | MotionContrl(111); 176 | INFO("motor start is %s, code is %d", (code ? "failed" : "successed"), code); 177 | } 178 | motor_mutex_.unlock(); 179 | } 180 | 181 | void PlayAudio(const std::string & text) 182 | { 183 | protocol::msg::AudioPlayExtend msg; 184 | msg.is_online = true; 185 | msg.module_name = power_consumption_info_node_->get_name(); 186 | msg.text = text; 187 | audio_play_extend_pub_->publish(msg); 188 | } 189 | 190 | bool MotionContrl(const int motion_id) 191 | { 192 | if (!motion_excute_client_->wait_for_service(std::chrono::seconds(5))) { 193 | ERROR("call led_excute server not avalible"); 194 | return false; 195 | } 196 | auto request_motion = std::make_shared(); 197 | request_motion->motion_id = motion_id; 198 | request_motion->cmd_source = 0; 199 | auto future_result_motion = motion_excute_client_->async_send_request(request_motion); 200 | std::future_status status_motion = future_result_motion.wait_for(std::chrono::seconds(10)); 201 | if (status_motion == std::future_status::timeout) { 202 | ERROR("call motion service failed"); 203 | } 204 | if (future_result_motion.get()->code != 0) { 205 | ERROR("control motion fialed, error code is:%d", future_result_motion.get()->code); 206 | } 207 | return true; 208 | } 209 | 210 | bool QueryLowPower() 211 | { 212 | PM_DEV pd = PM_CAM_REALSNS; 213 | int code = lpc_ptr_->LpcQuery(pd); 214 | return (code == 0) ? true : false; 215 | } 216 | 217 | int ShutdownOrReboot(bool trigger) 218 | { 219 | // trigger == true --> reboot, trigger == false --> shutdown 220 | PM_SYS pd = (trigger ? PM_SYS_REBOOT : PM_SYS_SHUTDOWN); 221 | INFO("[PowerConsumption]: %s", (trigger ? "reboot..." : "poweroff...")); 222 | int code = -1; 223 | code = lpc_ptr_->LpcSysRequest(pd); 224 | if (code != 0) { 225 | INFO( 226 | "[PowerConsumption]: %s failed, function LpcRequest error code is %d", 227 | (trigger ? "reboot..." : "poweroff..."), code); 228 | } else { 229 | INFO("[PowerConsumption]: %s successfully", (trigger ? "reboot..." : "poweroff...")); 230 | } 231 | return code; 232 | } 233 | 234 | void TailLedControl(bool is_light_off, bool is_exiting = false) 235 | { 236 | if (!led_excute_client_->wait_for_service(std::chrono::seconds(2))) { 237 | ERROR("call led_excute server not avalible"); 238 | return; 239 | } 240 | auto request_led = std::make_shared(); 241 | request_led->occupation = is_light_off; 242 | request_led->client = "lowpower"; 243 | request_led->target = 2; 244 | request_led->mode = 0x01; 245 | request_led->effect = 0xA0; 246 | if (is_exiting) { 247 | request_led->mode = 0x02; 248 | request_led->effect = 0x04; 249 | request_led->r_value = 0x06; 250 | request_led->g_value = 0x21; 251 | request_led->b_value = 0xE2; 252 | } 253 | auto future_result_tail = led_excute_client_->async_send_request(request_led); 254 | std::future_status status_tail = future_result_tail.wait_for(std::chrono::seconds(2)); 255 | if (status_tail != std::future_status::ready) { 256 | ERROR("call led_execute service failed"); 257 | return; 258 | } 259 | if (future_result_tail.get()->code == 0) { 260 | INFO("call led service successed"); 261 | } else { 262 | ERROR( 263 | "control tail led fialed, error code is:%d", future_result_tail.get()->code); 264 | } 265 | } 266 | 267 | private: 268 | void sub_mostion_status_callback(const protocol::msg::MotionStatus::SharedPtr msg) 269 | { 270 | // 状态机切换也有判断 271 | if (is_ota_) { 272 | INFO_MILLSECONDS(10000, "[LowPower]: in ota state return."); 273 | return; 274 | } 275 | // motion_id: 趴下(101)、站立(111) 276 | int motion_id = msg->motion_id; 277 | if (is_lowpower_) { 278 | return; 279 | } 280 | 281 | if (motion_id == 0 || motion_id == 101) { 282 | auto end = std::chrono::steady_clock::now(); 283 | auto diff = std::chrono::duration_cast(end - start); 284 | if (diff.count() >= enter_lowpower_time_) { 285 | // INFO("[LowPower]: enter lowpower, get down for more than 30s"); 286 | start = std::chrono::steady_clock::now(); 287 | enter_lowpower_handler(); 288 | start = std::chrono::steady_clock::now(); 289 | } 290 | } else { 291 | start = std::chrono::steady_clock::now(); 292 | } 293 | } 294 | 295 | void sub_state_switch_status_callback(const protocol::msg::StateSwitchStatus::SharedPtr msg) 296 | { 297 | if (msg->state == 3) { 298 | is_ota_ = true; 299 | } else { 300 | is_ota_ = false; 301 | } 302 | } 303 | 304 | private: 305 | rclcpp::Node::SharedPtr power_consumption_info_node_ {nullptr}; 306 | rclcpp::CallbackGroup::SharedPtr power_consumption_callback_group_; 307 | rclcpp::Client::SharedPtr led_excute_client_; 308 | rclcpp::Client::SharedPtr motion_excute_client_; 309 | rclcpp::Service::SharedPtr dog_leg_calibration_srv_; 310 | std::unique_ptr lpc_ptr_ {nullptr}; 311 | rclcpp::Subscription::SharedPtr motion_status_sub_ {nullptr}; 312 | rclcpp::Publisher::SharedPtr audio_play_extend_pub_ {nullptr}; 313 | rclcpp::Subscription::SharedPtr \ 314 | state_swith_status_sub_ {nullptr}; 315 | PCIN_CALLBACK request_handler; 316 | PCIN_CALLBACK release_handler; 317 | PowerMachineState pms {PowerMachineState::PMS_UNKOWN}; 318 | PCIN_CALLBACK enter_lowpower_handler; 319 | bool is_lowpower_ {false}; 320 | bool is_ota_ {false}; 321 | int enter_lowpower_time_ {30}; 322 | std::chrono::steady_clock::time_point start; 323 | std::mutex motor_mutex_; 324 | }; 325 | } // namespace manager 326 | } // namespace cyberdog 327 | 328 | #endif // CYBERDOG_MANAGER__POWER_CONSUMPTION_INFO_HPP_ 329 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/account_info.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 CYBERDOG_MANAGER__ACCOUNT_INFO_HPP_ 15 | #define CYBERDOG_MANAGER__ACCOUNT_INFO_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "cyberdog_common/cyberdog_json.hpp" 22 | #include "protocol/srv/account_add.hpp" 23 | #include "protocol/srv/account_search.hpp" 24 | #include "protocol/srv/account_delete.hpp" 25 | #include "protocol/srv/account_change.hpp" 26 | #include "protocol/srv/all_user_search.hpp" 27 | #include "protocol/srv/audio_voiceprint_entry.hpp" 28 | #include "protocol/srv/face_entry.hpp" 29 | #include "user_info_manager/UserAccountManager.hpp" 30 | 31 | using cyberdog::common::CyberdogJson; 32 | using rapidjson::Document; 33 | using rapidjson::kObjectType; 34 | 35 | namespace cyberdog 36 | { 37 | namespace manager 38 | { 39 | 40 | enum class AccountErrorCode : int32_t 41 | { 42 | kAccountSearchError = 21, 43 | kAccountDatabaseError = 22, 44 | kAccounVoiceprintError = 23, 45 | kAccountFaceError = 24, 46 | }; 47 | 48 | class AccountInfoNode final 49 | { 50 | public: 51 | explicit AccountInfoNode(rclcpp::Node::SharedPtr node_ptr) 52 | { 53 | code_ptr_ = std::make_shared>( 54 | cyberdog::system::ModuleCode::kRobot); 55 | account_info_node_ = node_ptr; 56 | account_callback_group_ = 57 | account_info_node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); 58 | account_add_srv_ = 59 | account_info_node_->create_service( 60 | "account_add", 61 | std::bind( 62 | &AccountInfoNode::QueryAccountAdd, this, std::placeholders::_1, 63 | std::placeholders::_2), 64 | rmw_qos_profile_services_default, account_callback_group_); 65 | account_search_srv_ = 66 | account_info_node_->create_service( 67 | "account_search", 68 | std::bind( 69 | &AccountInfoNode::QueryAccountSearch, this, std::placeholders::_1, 70 | std::placeholders::_2), 71 | rmw_qos_profile_services_default, account_callback_group_); 72 | account_delete_srv_ = 73 | account_info_node_->create_service( 74 | "account_delete", 75 | std::bind( 76 | &AccountInfoNode::QueryAccountDelete, this, std::placeholders::_1, 77 | std::placeholders::_2), 78 | rmw_qos_profile_services_default, account_callback_group_); 79 | account_search_all_ = 80 | account_info_node_->create_service( 81 | "all_user_search", 82 | std::bind( 83 | &AccountInfoNode::AllUserSearch, this, std::placeholders::_1, 84 | std::placeholders::_2), 85 | rmw_qos_profile_services_default, account_callback_group_); 86 | account_change_srv_ = 87 | account_info_node_->create_service( 88 | "account_change", 89 | std::bind( 90 | &AccountInfoNode::ChangeUserName, this, std::placeholders::_1, 91 | std::placeholders::_2), 92 | rmw_qos_profile_services_default, account_callback_group_); 93 | } 94 | 95 | private: 96 | void AllUserSearch( 97 | const protocol::srv::AllUserSearch::Request::SharedPtr request, 98 | protocol::srv::AllUserSearch::Response::SharedPtr response) 99 | { 100 | INFO("enter search all user service callback"); 101 | cyberdog::common::CyberdogAccountManager obj; 102 | std::vector member_data; 103 | int result[2]; 104 | if (request->command == "") { 105 | INFO("search all user"); 106 | if (obj.SearAllUser(member_data)) { 107 | response->code = code_ptr_->GetKeyCode(cyberdog::system::KeyCode::kOK); 108 | } else { 109 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountSearchError); 110 | return; 111 | } 112 | int len = member_data.size(); 113 | response->result.resize(len); 114 | for (int i = 0; i < len; i++) { 115 | response->result[i].id = member_data[i].id; 116 | response->result[i].username = member_data[i].name; 117 | response->result[i].voicestatus = member_data[i].voiceStatus; 118 | response->result[i].facestatus = member_data[i].faceStatus; 119 | } 120 | INFO("response->result length is %d", response->result.size()); 121 | } else { 122 | INFO("search one user"); 123 | std::string username = request->command; 124 | response->result.resize(1); 125 | if (obj.SearchUser(username, result)) { 126 | // INFO( 127 | // "search all service (serach one user : name:%s, %d, %d)", 128 | // username.c_str(), result[0], result[1]); 129 | response->result[0].username = username; 130 | response->result[0].voicestatus = result[0]; 131 | response->result[0].facestatus = result[1]; 132 | response->code = code_ptr_->GetKeyCode(cyberdog::system::KeyCode::kOK); 133 | } else { 134 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountSearchError); 135 | return; 136 | } 137 | } 138 | } 139 | 140 | void QueryAccountAdd( 141 | const protocol::srv::AccountAdd::Request::SharedPtr request, 142 | protocol::srv::AccountAdd::Response::SharedPtr response) 143 | { 144 | std::string name = request->member; 145 | INFO("add_account_name is: %s", name.c_str()); 146 | cyberdog::common::CyberdogAccountManager obj; 147 | if (obj.AddMember(name)) { 148 | response->code = code_ptr_->GetKeyCode(cyberdog::system::KeyCode::kOK); 149 | INFO("add_account_success"); 150 | } else { 151 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountDatabaseError); 152 | INFO("add_account_fail"); 153 | } 154 | } 155 | 156 | void QueryAccountSearch( 157 | const protocol::srv::AccountSearch::Request::SharedPtr request, 158 | protocol::srv::AccountSearch::Response::SharedPtr response) 159 | { 160 | std::string account_name = request->member; 161 | INFO("search user_name: %s", account_name.c_str()); 162 | Document json_info(rapidjson::kObjectType); 163 | rapidjson::Value arr(rapidjson::kArrayType); 164 | std::string data; 165 | cyberdog::common::CyberdogAccountManager obj; 166 | if (account_name.empty()) { 167 | INFO("search_all_user"); 168 | std::vector member_data; 169 | if (obj.SearAllUser(member_data)) { 170 | for (unsigned int i = 0; i < member_data.size(); ++i) { 171 | rapidjson::Value js_obj(rapidjson::kObjectType); 172 | rapidjson::Value value(member_data[i].name.c_str(), json_info.GetAllocator()); 173 | js_obj.AddMember("id", member_data[i].id, json_info.GetAllocator()); 174 | js_obj.AddMember("name", value, json_info.GetAllocator()); 175 | js_obj.AddMember("face_state", member_data[i].faceStatus, json_info.GetAllocator()); 176 | js_obj.AddMember("voice_state", member_data[i].voiceStatus, json_info.GetAllocator()); 177 | arr.PushBack(js_obj, json_info.GetAllocator()); 178 | } 179 | json_info.AddMember("accounts", arr, json_info.GetAllocator()); 180 | response->code = code_ptr_->GetKeyCode(cyberdog::system::KeyCode::kOK); 181 | } else { 182 | INFO("Search ALL Account faile!"); 183 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountSearchError); 184 | response->data = "{\"code\":121}"; 185 | } 186 | } else { 187 | INFO("search_one_user"); 188 | int result[2]{-1, -1}; 189 | rapidjson::Value js_obj(rapidjson::kObjectType); 190 | if (!obj.SearchUser(account_name, result)) { 191 | INFO("search %s faild, SearchUser() return 0", account_name.c_str()); 192 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountSearchError); 193 | response->data = "{\"code\":121}"; 194 | return; 195 | } 196 | int voice_state = result[0]; 197 | int face_state = result[1]; 198 | INFO( 199 | "account name:%s \n voice_state: %d \n face_stare: %d", 200 | account_name.c_str(), result[0], result[1]); 201 | json_info.AddMember("code", 0, json_info.GetAllocator()); 202 | rapidjson::Value value(account_name.c_str(), json_info.GetAllocator()); 203 | js_obj.AddMember("name", value, json_info.GetAllocator()); 204 | js_obj.AddMember("face_state", face_state, json_info.GetAllocator()); 205 | js_obj.AddMember("voice_state", voice_state, json_info.GetAllocator()); 206 | arr.PushBack(js_obj, json_info.GetAllocator()); 207 | json_info.AddMember("accounts", arr, json_info.GetAllocator()); 208 | response->code = code_ptr_->GetKeyCode(cyberdog::system::KeyCode::kOK); 209 | } 210 | 211 | if (!CyberdogJson::Document2String(json_info, data)) { 212 | ERROR("error while encoding to json"); 213 | data = "{\"error\": \"unkown encoding json error!\"}"; 214 | } 215 | response->data = data; 216 | INFO("Search result is :%s", data.c_str()); 217 | } 218 | void QueryAccountDelete( 219 | const protocol::srv::AccountDelete::Request::SharedPtr request, 220 | protocol::srv::AccountDelete::Response::SharedPtr response) 221 | { 222 | INFO("enter delete account callback"); 223 | std::chrono::seconds timeout(3); 224 | // voice delete 225 | rclcpp::Client::SharedPtr voice_delete_client_; 226 | voice_delete_client_ = 227 | account_info_node_->create_client( 228 | "audio_voiceprint_entry"); 229 | if (!voice_delete_client_->wait_for_service(std::chrono::seconds(2))) { 230 | WARN("call voice service server not avalible"); 231 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccounVoiceprintError); 232 | return; 233 | } 234 | auto request_voice = std::make_shared(); 235 | request_voice->command = 4; 236 | request_voice->username = request->member; 237 | auto future_result_voice = voice_delete_client_->async_send_request(request_voice); 238 | std::future_status status_voice = future_result_voice.wait_for(timeout); 239 | if (status_voice == std::future_status::ready) { 240 | INFO( 241 | "success to call voice delete response services."); 242 | } else { 243 | INFO( 244 | "Failed to call voice delete response services."); 245 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccounVoiceprintError); 246 | return; 247 | } 248 | 249 | // face delete 250 | rclcpp::Client::SharedPtr face_delete_client_; 251 | face_delete_client_ = 252 | account_info_node_->create_client("cyberdog_face_entry_srv"); 253 | if (!face_delete_client_->wait_for_service(std::chrono::seconds(2))) { 254 | ERROR("call face service server not avalible"); 255 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountFaceError); 256 | return; 257 | } 258 | auto request_face = std::make_shared(); 259 | request_face->command = 4; 260 | request_face->username = request->member; 261 | auto future_result_face = face_delete_client_->async_send_request(request_face); 262 | std::future_status status_face = future_result_face.wait_for(timeout); 263 | if (status_face != std::future_status::ready) { 264 | WARN("Failed to call face delete response services."); 265 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountFaceError); 266 | return; 267 | } 268 | // delete account 269 | std::string name = request->member; 270 | INFO("delete_account_name is: %s", name.c_str()); 271 | if (future_result_voice.get()->success) { 272 | if (future_result_face.get()->result == 0) { 273 | cyberdog::common::CyberdogAccountManager obj; 274 | if (obj.DeleteUserInformation(name)) { 275 | response->code = code_ptr_->GetKeyCode(cyberdog::system::KeyCode::kOK); 276 | } else { 277 | WARN("delete account failed"); 278 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountDatabaseError); 279 | } 280 | } else { 281 | WARN("delete face failed"); 282 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountFaceError); 283 | } 284 | } else { 285 | WARN("delete voice failed"); 286 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccounVoiceprintError); 287 | } 288 | } 289 | void ChangeUserName( 290 | const protocol::srv::AccountChange::Request::SharedPtr request, 291 | protocol::srv::AccountChange::Response::SharedPtr response) 292 | { 293 | INFO("enter change account callback"); 294 | std::string pre_name = request->pre_name; 295 | std::string new_name = request->new_name; 296 | std::chrono::seconds timeout(3); 297 | 298 | int result[2]{-1, -1}; 299 | cyberdog::common::CyberdogAccountManager obj; 300 | rapidjson::Value js_obj(rapidjson::kObjectType); 301 | if (!obj.SearchUser(pre_name, result)) { 302 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountSearchError); 303 | return; 304 | } 305 | // face state is equal to result[1] 306 | if (result[1]) { 307 | rclcpp::Client::SharedPtr face_delete_client_; 308 | face_delete_client_ = 309 | account_info_node_->create_client("cyberdog_face_entry_srv"); 310 | if (!face_delete_client_->wait_for_service(std::chrono::seconds(2))) { 311 | WARN("call face service server not avalible"); 312 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountFaceError); 313 | return; 314 | } 315 | auto request_face = std::make_shared(); 316 | request_face->command = 3; 317 | request_face->username = new_name; 318 | request_face->oriname = pre_name; 319 | auto future_result_face = face_delete_client_->async_send_request(request_face); 320 | std::future_status status_face = future_result_face.wait_for(timeout); 321 | if (status_face == std::future_status::ready) { 322 | if (future_result_face.get()->result != 0) { 323 | WARN("change user face name failed."); 324 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountFaceError); 325 | return; 326 | } 327 | } else { 328 | WARN("failed to call face services."); 329 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountFaceError); 330 | return; 331 | } 332 | } 333 | // change account 334 | INFO("change account name from %s to %s", pre_name.c_str(), new_name.c_str()); 335 | if (obj.ModifyUserName(pre_name, new_name)) { 336 | response->code = code_ptr_->GetKeyCode(cyberdog::system::KeyCode::kOK); 337 | } else { 338 | WARN("change account nickname failed"); 339 | response->code = code_ptr_->GetCode(AccountErrorCode::kAccountDatabaseError); 340 | } 341 | } 342 | 343 | private: 344 | rclcpp::Node::SharedPtr account_info_node_{nullptr}; 345 | std::shared_ptr> code_ptr_ {nullptr}; 346 | rclcpp::CallbackGroup::SharedPtr account_callback_group_; 347 | rclcpp::Service::SharedPtr account_add_srv_; 348 | rclcpp::Service::SharedPtr account_search_srv_; 349 | rclcpp::Service::SharedPtr account_delete_srv_; 350 | rclcpp::Service::SharedPtr account_search_all_; 351 | rclcpp::Service::SharedPtr account_change_srv_; 352 | }; 353 | 354 | } // namespace manager 355 | } // namespace cyberdog 356 | #endif // CYBERDOG_MANAGER__ACCOUNT_INFO_HPP_ 357 | -------------------------------------------------------------------------------- /cyberdog_manager/include/cyberdog_manager/ready_info.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 CYBERDOG_MANAGER__READY_INFO_HPP_ 15 | #define CYBERDOG_MANAGER__READY_INFO_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "std_msgs/msg/bool.hpp" 23 | #include "cyberdog_common/cyberdog_json.hpp" 24 | #include "cyberdog_common/cyberdog_log.hpp" 25 | #include "protocol/msg/self_check_status.hpp" 26 | #include "protocol/msg/state_switch_status.hpp" 27 | #include "protocol/srv/motion_result_cmd.hpp" 28 | #include "protocol/srv/audio_text_play.hpp" 29 | #include "protocol/msg/bms_status.hpp" 30 | #include "protocol/srv/bes_http_send_file.hpp" 31 | #include "ament_index_cpp/get_package_share_directory.hpp" 32 | #include "cyberdog_common/cyberdog_toml.hpp" 33 | 34 | 35 | using cyberdog::common::CyberdogJson; 36 | using rapidjson::Document; 37 | using rapidjson::Value; 38 | using rapidjson::kObjectType; 39 | 40 | namespace cyberdog 41 | { 42 | namespace manager 43 | { 44 | enum SelfcheckState 45 | { 46 | UNKNOW = -1, 47 | ALL_SUCCESS = 0, 48 | HARDWARE_CHECKING = 1, 49 | CRITICAL_HARDWARE_FAILED = 2, 50 | HARDWARE_SUCCESS = 3, 51 | UNCRITICAL_HARDWARE_FAILED = 4, 52 | SOFTWARE_SETUP_FAILED = 5, 53 | SOFTWARE_SETUP_SUCCESS = 6 54 | }; 55 | enum ErrorCode 56 | { 57 | kLedSelfcheckError = 1109, 58 | kBmsSelfcheckError = 1409, 59 | kTouchSelfcheckError = 1509, 60 | kUWBSelfcheckError = 1609, 61 | kLidarSelfcheckError = 2109, 62 | kTofSelfcheckError = 2209, 63 | // kUltrasnoicSelfcheckError = 2309, 64 | // kGpsSelfcheckError = 2409, 65 | 66 | kLedAndUWBSelfcheckError = 1109 + 1609, 67 | kTofAndLidarSelfcheckError = 2109 + 2209 68 | }; 69 | class ReadyNotifyNode final 70 | { 71 | using EXCEPTION_PLAYSOUND_CALLBACK = std::function; 72 | 73 | public: 74 | explicit ReadyNotifyNode(rclcpp::Node::SharedPtr node_ptr) 75 | { 76 | ready_notify_node_ = node_ptr; 77 | ready_notify_callback_group_ = 78 | ready_notify_node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); 79 | rclcpp::PublisherOptions pub_options; 80 | pub_options.callback_group = ready_notify_callback_group_; 81 | rclcpp::SubscriptionOptions sub_options; 82 | sub_options.callback_group = ready_notify_callback_group_; 83 | ready_notify_pub_ = ready_notify_node_->create_publisher( 84 | "ready_notify", 85 | rclcpp::SystemDefaultsQoS(), 86 | pub_options 87 | ); 88 | self_check_status_pub_ = ready_notify_node_->create_publisher( 89 | "self_check_status", 90 | rclcpp::SystemDefaultsQoS(), 91 | pub_options 92 | ); 93 | state_swith_status_pub_ = 94 | ready_notify_node_->create_publisher( 95 | "state_switch_status", rclcpp::SystemDefaultsQoS(), pub_options); 96 | 97 | app_connect_state_sub_ = ready_notify_node_->create_subscription( 98 | "app_connection_state", rclcpp::SystemDefaultsQoS(), 99 | std::bind(&ReadyNotifyNode::AppConnectState, this, std::placeholders::_1), 100 | sub_options); 101 | 102 | motion_excute_client_ = 103 | ready_notify_node_->create_client( 104 | "motion_result_cmd", 105 | rmw_qos_profile_services_default, ready_notify_callback_group_); 106 | 107 | audio_play_client_ = 108 | ready_notify_node_->create_client( 109 | "speech_text_play", 110 | rmw_qos_profile_services_default, ready_notify_callback_group_); 111 | 112 | bms_status_sub_ = 113 | ready_notify_node_->create_subscription( 114 | "bms_status", rclcpp::SystemDefaultsQoS(), 115 | std::bind(&ReadyNotifyNode::BmsStatus, this, std::placeholders::_1), sub_options); 116 | 117 | upload_events_client_ = ready_notify_node_->create_client( 118 | "bes_http_send_file_srv", rmw_qos_profile_services_default); 119 | // std::thread( 120 | // [this]() { 121 | // executor_.add_node(train_plan_node_ptr_); 122 | // executor_.spin(); 123 | // rclcpp::shutdown(); 124 | // }).detach(); 125 | GetCriticalMoudle(); 126 | } 127 | 128 | void Ready(bool ready) 129 | { 130 | count_ = 3000; 131 | ready_ = ready; 132 | if (!notify_message_thread.joinable()) { 133 | notify_message_thread = std::thread( 134 | [this]() { 135 | while (!exit_ && count_ > 0 && rclcpp::ok()) { 136 | std_msgs::msg::Bool msg; 137 | msg.data = ready_; 138 | ready_notify_pub_->publish(msg); 139 | std::this_thread::sleep_for(std::chrono::milliseconds(100)); 140 | --count_; 141 | } 142 | }); 143 | } 144 | } 145 | 146 | void SelfCheck(int32_t state, const std::map & stmap) 147 | { 148 | selfcheck_state_ = state; 149 | INFO("current state is %d", selfcheck_state_); 150 | if (!notify_selfcheck_thread.joinable()) { 151 | notify_selfcheck_thread = std::thread( 152 | [this, &stmap]() { 153 | static int32_t scs = -1; 154 | while (!exit_ && rclcpp::ok()) { 155 | protocol::msg::SelfCheckStatus msg; 156 | if (0 == selfcheck_state_ || 6 == selfcheck_state_) { 157 | static std::string describ = UpSelfCheckState(selfcheck_state_, stmap); 158 | msg.description = describ; 159 | } else { 160 | msg.description = UpSelfCheckState(selfcheck_state_, stmap); 161 | } 162 | msg.code = selfcheck_state_; 163 | self_check_status_pub_->publish(msg); 164 | INFO_EXPRESSION( 165 | (selfcheck_state_ != scs), "self check status:%s", 166 | msg.description.c_str()); 167 | scs = selfcheck_state_; 168 | std::this_thread::sleep_for(std::chrono::milliseconds(1000)); 169 | } 170 | }); 171 | } 172 | } 173 | 174 | bool UploadEvents() 175 | { 176 | if (!upload_events_client_->wait_for_service(std::chrono::seconds(3))) { 177 | WARN("call upload events service not avalible!"); 178 | return false; 179 | } 180 | std::this_thread::sleep_for(std::chrono::seconds(2)); 181 | int64_t now_ms = std::chrono::duration_cast( 182 | std::chrono::system_clock::now().time_since_epoch()).count(); 183 | std::string events = "{" + std::string("\"timestamp\": ") + 184 | std::to_string(now_ms) + std::string(" , \"type\": \"selfcheck\"") + 185 | ", \"info\": " + state_str_ + "}"; 186 | INFO("upload events which selfcheck fialed is %s", events.c_str()); 187 | auto req = std::make_shared(); 188 | req->method = protocol::srv::BesHttpSendFile::Request::HTTP_METHOD_POST; 189 | req->url = "device/system/log"; 190 | req->info = events; 191 | req->milsecs = 60000; // 60s 192 | auto future_result = upload_events_client_->async_send_request(req); 193 | std::future_status status = future_result.wait_for(std::chrono::seconds(60)); 194 | if (status == std::future_status::ready) { 195 | auto response = future_result.get()->data; 196 | INFO("upload log respon: %s", response.c_str()); 197 | } else { 198 | WARN("calling bes_http_send_file_srv service timeout!"); 199 | return false; 200 | } 201 | return true; 202 | } 203 | 204 | void SetExceptionPlaySoundCallback(EXCEPTION_PLAYSOUND_CALLBACK callback) 205 | { 206 | play_sound = callback; 207 | } 208 | 209 | void GetCriticalMoudle() 210 | { 211 | auto local_share_dir = ament_index_cpp::get_package_share_directory("params"); 212 | auto path = local_share_dir + std::string("/toml_config/manager/selfcheck_config.toml"); 213 | toml::value config; 214 | if (common::CyberdogToml::ParseFile(path, config)) { 215 | INFO("Parse Selfcheck config file started, toml file is valid"); 216 | toml::value sensors_sec; 217 | if (common::CyberdogToml::Get(config, "sensors", sensors_sec)) { 218 | INFO("Selfheck init started, parse sensors config succeed"); 219 | if (common::CyberdogToml::Get(sensors_sec, "critical", critical_sensor_)) { 220 | INFO("Self Check init started, parse sensors jump array succeed"); 221 | } 222 | } 223 | toml::value device_sec; 224 | if (common::CyberdogToml::Get(config, "devices", device_sec)) { 225 | INFO("Selfcheck init started, parse devices config succeed"); 226 | if (common::CyberdogToml::Get(device_sec, "critical", critical_device_)) { 227 | INFO("Selfcheck init started, parse device jump array succeed"); 228 | } 229 | } 230 | } 231 | } 232 | 233 | bool IsSelfcheckError(const std::map & stmap) 234 | { 235 | for (const auto & [moudle, code] : stmap) { 236 | (void)moudle; 237 | if (code != 0) { 238 | return true; 239 | } 240 | } 241 | 242 | return false; 243 | } 244 | 245 | bool IsCriticalError(const std::map & stmap) 246 | { 247 | for (const auto & [module, code] : stmap) { 248 | INFO("[moudle,code]:[%s, %d]", module.c_str(), code); 249 | if (0 == code) { 250 | continue; 251 | } 252 | 253 | if ("motion_manager" == module || "audio_manager" == module) { 254 | play_sound(code); 255 | return true; 256 | } 257 | 258 | if ("device_manager" == module) { 259 | std::vector error_code; 260 | for (auto n : critical_device_) { 261 | auto itr = module_map_.find(n); 262 | if (itr != module_map_.end()) { 263 | error_code.emplace_back(static_cast(itr->second)); 264 | } 265 | INFO("critical devices array is %s", n.c_str()); 266 | } 267 | 268 | if (std::find(error_code.begin(), error_code.end(), code) != error_code.end()) { 269 | play_sound(code); 270 | return true; 271 | } 272 | } 273 | 274 | if ("sensor_manager" == module) { 275 | std::vector error_code; 276 | for (auto n : critical_sensor_) { 277 | auto itr = module_map_.find(n); 278 | if (itr != module_map_.end()) { 279 | error_code.emplace_back(static_cast(itr->second)); 280 | } 281 | INFO("critical sensors array is %s", n.c_str()); 282 | } 283 | 284 | if (std::find(error_code.begin(), error_code.end(), code) != error_code.end()) { 285 | play_sound(code); 286 | return true; 287 | } 288 | } 289 | } 290 | return false; 291 | } 292 | 293 | 294 | void StoreSelfcheckResults() 295 | { 296 | std::this_thread::sleep_for(std::chrono::seconds(2)); 297 | auto path = SELFCHECK_DIR + "/" + FILE_NAME; 298 | if (access(SELFCHECK_DIR.c_str(), F_OK) != 0) { 299 | std::string cmd = "mkdir -p " + SELFCHECK_DIR; 300 | std::system(cmd.c_str()); 301 | cmd = "chmod 777 " + SELFCHECK_DIR; 302 | std::system(cmd.c_str()); 303 | } 304 | CyberdogJson::WriteJsonToFile(path, selfcheck_info_); 305 | } 306 | 307 | std::string UpSelfCheckState(const int32_t state, const std::map & stmap) 308 | { 309 | int32_t value {0}; 310 | Document self_check_state(kObjectType); 311 | Document device(kObjectType); 312 | Document sensor(kObjectType); 313 | Document::AllocatorType & allocator = self_check_state.GetAllocator(); 314 | if (1 == state) { 315 | value = -1; 316 | } 317 | CyberdogJson::Add(device, "bms", value); 318 | CyberdogJson::Add(device, "led", value); 319 | CyberdogJson::Add(device, "touch", value); 320 | CyberdogJson::Add(device, "uwb", value); 321 | CyberdogJson::Add(device, "bluetooth", value); 322 | CyberdogJson::Add(device, "others", value); 323 | CyberdogJson::Add(sensor, "lidar", value); 324 | // CyberdogJson::Add(sensor, "gps", value); 325 | // CyberdogJson::Add(sensor, "ultrasonic", value); 326 | CyberdogJson::Add(sensor, "tof", value); 327 | CyberdogJson::Add(sensor, "others", value); 328 | CyberdogJson::Add(self_check_state, "audio", value); 329 | CyberdogJson::Add(self_check_state, "motion_manager", value); 330 | if (3 == state) { 331 | value = -1; 332 | } 333 | CyberdogJson::Add(self_check_state, "algorithm_manager", value); 334 | CyberdogJson::Add(self_check_state, "vp_engine", value); 335 | CyberdogJson::Add(self_check_state, "RealSenseActuator", value); 336 | if (2 == state || 4 == state || 6 == state) { 337 | for (const auto & [moudle, code] : stmap) { 338 | if (moudle == "algorithm_manager") { 339 | Value & tmp = self_check_state["algorithm_manager"]; 340 | tmp.SetInt(code); 341 | continue; 342 | } 343 | if (moudle == "device_manager") { 344 | switch (static_cast(code)) { 345 | case kLedSelfcheckError: { 346 | Value & tmp = device["led"]; 347 | tmp.SetInt(static_cast(kLedSelfcheckError)); 348 | break; 349 | } 350 | case kBmsSelfcheckError: { 351 | Value & tmp = device["bms"]; 352 | tmp.SetInt(static_cast(kBmsSelfcheckError)); 353 | break; 354 | } 355 | case kTouchSelfcheckError: { 356 | Value & tmp = device["touch"]; 357 | tmp.SetInt(static_cast(kTouchSelfcheckError)); 358 | break; 359 | } 360 | case kUWBSelfcheckError: { 361 | Value & tmp = device["uwb"]; 362 | tmp.SetInt(static_cast(kUWBSelfcheckError)); 363 | break; 364 | } 365 | case kLedAndUWBSelfcheckError: { 366 | Value & tmp = device["led"]; 367 | tmp.SetInt(static_cast(kLedSelfcheckError)); 368 | Value & tmp2 = device["uwb"]; 369 | tmp2.SetInt(static_cast(kUWBSelfcheckError)); 370 | Value & tmp3 = device["others"]; 371 | tmp3.SetInt( 372 | static_cast(kUWBSelfcheckError) + 373 | static_cast(kLedSelfcheckError)); 374 | break; 375 | } 376 | case kTofAndLidarSelfcheckError: 377 | case kTofSelfcheckError: 378 | case kLidarSelfcheckError: 379 | default: { 380 | Value & tmp = device["others"]; 381 | tmp.SetInt(static_cast(code)); 382 | break; 383 | } 384 | } 385 | continue; 386 | } 387 | if (moudle == "motion_manager") { 388 | Value & tmp = self_check_state["motion_manager"]; 389 | tmp.SetInt(code); 390 | continue; 391 | } 392 | if (moudle == "sensor_manager") { 393 | switch (static_cast(code)) { 394 | case kLidarSelfcheckError: { 395 | Value & tmp = sensor["lidar"]; 396 | tmp.SetInt(static_cast(kLidarSelfcheckError)); 397 | break; 398 | } 399 | case kTofSelfcheckError: { 400 | Value & tmp = sensor["tof"]; 401 | tmp.SetInt(static_cast(kTofSelfcheckError)); 402 | break; 403 | } 404 | case kTofAndLidarSelfcheckError: { 405 | Value & tmp = sensor["lidar"]; 406 | tmp.SetInt(static_cast(kLidarSelfcheckError)); 407 | Value & tmp2 = sensor["tof"]; 408 | tmp2.SetInt(static_cast(kTofSelfcheckError)); 409 | Value & tmp3 = sensor["others"]; 410 | tmp3.SetInt( 411 | static_cast(kLidarSelfcheckError) + 412 | static_cast(kTofSelfcheckError)); 413 | break; 414 | } 415 | case kLedSelfcheckError: 416 | case kBmsSelfcheckError: 417 | case kTouchSelfcheckError: 418 | case kUWBSelfcheckError: 419 | case kLedAndUWBSelfcheckError: 420 | default: { 421 | Value & tmp = sensor["others"]; 422 | tmp.SetInt(static_cast(code)); 423 | break; 424 | } 425 | } 426 | continue; 427 | } 428 | if (moudle == "vp_engine") { 429 | Value & tmp = self_check_state["vp_engine"]; 430 | tmp.SetInt(code); 431 | continue; 432 | } 433 | if (moudle == "RealSenseActuator") { 434 | Value & tmp = self_check_state["RealSenseActuator"]; 435 | tmp.SetInt(code); 436 | continue; 437 | } 438 | } 439 | } 440 | self_check_state.AddMember("device", device, allocator); 441 | self_check_state.AddMember("sensor", sensor, allocator); 442 | CyberdogJson::Document2String(self_check_state, state_str_); 443 | selfcheck_info_.CopyFrom(self_check_state, allocator); 444 | return state_str_; 445 | } 446 | 447 | void MachineState(int32_t state) 448 | { 449 | protocol::msg::StateSwitchStatus sss; 450 | sss.code = 0; 451 | sss.state = state; 452 | state_swith_status_pub_->publish(sss); 453 | } 454 | 455 | void AppConnectState(const std_msgs::msg::Bool msg) 456 | { 457 | static bool pre_connect_state{false}; 458 | if (selfcheck_state_ == -1 || selfcheck_state_ == 1 || selfcheck_state_ == 2) { 459 | return; 460 | } 461 | // app第一次连接后站立 462 | if (pre_connect_state == false && msg.data == true) { 463 | pre_connect_state = true; 464 | if (is_charging) { 465 | return; 466 | } 467 | cyberdog_standing_ = 1; 468 | INFO("app connected, dog standup"); 469 | PlayAudioService(2002); 470 | cyberdog_standing_ = 2; 471 | if (!motion_excute_client_->wait_for_service(std::chrono::seconds(5))) { 472 | ERROR("call motion server not avalible"); 473 | } 474 | auto request_motion = std::make_shared(); 475 | request_motion->motion_id = 111; 476 | request_motion->cmd_source = 0; 477 | auto future_result_motion = motion_excute_client_->async_send_request(request_motion); 478 | std::future_status status_motion = future_result_motion.wait_for(std::chrono::seconds(5)); 479 | if (status_motion != std::future_status::ready) { 480 | ERROR("call motion service failed"); 481 | } 482 | if (future_result_motion.get()->result != 0) { 483 | ERROR("control motion fialed, error code is:%d", future_result_motion.get()->code); 484 | } 485 | pre_connect_state = true; 486 | } 487 | // app重连后站立 488 | // if (pre_connect_state == true && msg.data == false) { 489 | // pre_connect_state = false; 490 | // } 491 | } 492 | 493 | void PlayAudioService(const uint16_t play_id) 494 | { 495 | auto request_audio = std::make_shared(); 496 | request_audio->module_name = ready_notify_node_->get_name(); 497 | request_audio->is_online = false; 498 | request_audio->speech.play_id = play_id; 499 | auto callback = [](rclcpp::Client::SharedFuture future) { 500 | INFO("Audio play result: %s", future.get()->status == 0 ? "success" : "failed"); 501 | }; 502 | auto future_result_audio = audio_play_client_->async_send_request(request_audio, callback); 503 | if (future_result_audio.wait_for(std::chrono::milliseconds(3000)) == 504 | std::future_status::timeout) 505 | { 506 | ERROR("Cannot get response from AudioPlay"); 507 | } 508 | } 509 | 510 | void BmsStatus(const protocol::msg::BmsStatus::SharedPtr msg) 511 | { 512 | is_charging = msg->power_wired_charging; 513 | } 514 | 515 | int GetAudioOccupancyState() 516 | { 517 | std::this_thread::sleep_for(std::chrono::milliseconds(800)); 518 | return cyberdog_standing_; 519 | } 520 | 521 | ~ReadyNotifyNode() 522 | { 523 | exit_ = true; 524 | if (notify_message_thread.joinable()) { 525 | notify_message_thread.join(); 526 | } 527 | } 528 | 529 | private: 530 | bool exit_ {false}; 531 | bool ready_ {false}; 532 | bool is_charging {false}; 533 | int32_t selfcheck_state_ {-1}; 534 | std::string name_; 535 | rclcpp::Node::SharedPtr ready_notify_node_ {nullptr}; 536 | // rclcpp::executors::MultiThreadedExecutor executor_; 537 | rclcpp::CallbackGroup::SharedPtr ready_notify_callback_group_; 538 | rclcpp::Publisher::SharedPtr ready_notify_pub_; 539 | rclcpp::Publisher::SharedPtr self_check_status_pub_; 540 | rclcpp::Publisher::SharedPtr state_swith_status_pub_; 541 | rclcpp::Subscription::SharedPtr bms_status_sub_; 542 | rclcpp::Subscription::SharedPtr app_connect_state_sub_; 543 | rclcpp::Client::SharedPtr motion_excute_client_; 544 | rclcpp::Client::SharedPtr audio_play_client_ {nullptr}; 545 | rclcpp::Client::SharedPtr upload_events_client_ {nullptr}; 546 | std::thread notify_message_thread; 547 | std::thread notify_selfcheck_thread; 548 | int count_; 549 | int cyberdog_standing_ {0}; 550 | std::string state_str_ {}; 551 | EXCEPTION_PLAYSOUND_CALLBACK play_sound {[](int32_t) {}}; 552 | rapidjson::Document selfcheck_info_; 553 | const std::string SELFCHECK_DIR {"/home/mi/.ros/log"}; 554 | const std::string FILE_NAME {"last_selfcheck_info.json"}; 555 | std::vector critical_sensor_; 556 | std::vector critical_device_; 557 | const std::map module_map_ { 558 | {"led", kLedSelfcheckError}, 559 | {"bms", kBmsSelfcheckError}, 560 | {"touch", kTouchSelfcheckError}, 561 | {"lidar", kLidarSelfcheckError}, 562 | {"tof", kBmsSelfcheckError}, 563 | // {"Ultrasnoic", kUltrasnoicSelfcheckError}, 564 | // {"Gps", kGpsSelfcheckError}, 565 | }; 566 | }; 567 | } // namespace manager 568 | } // namespace cyberdog 569 | 570 | #endif // CYBERDOG_MANAGER__READY_INFO_HPP_ 571 | --------------------------------------------------------------------------------