├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── custom.md │ └── feature_request.md └── workflows │ └── workflow.yml ├── .gitignore ├── LICENSE.txt ├── README.md ├── elec_skin ├── CMakeLists.txt ├── include │ └── elec_skin │ │ ├── elec_skin.hpp │ │ └── elec_skin_base.hpp ├── package.xml ├── src │ └── main.cpp └── toml_config │ └── elec_skin.toml ├── motion_action ├── CMakeLists.txt ├── include │ └── motion_action │ │ ├── motion_action.hpp │ │ └── motion_macros.hpp ├── launch │ ├── AMENT_IGNORE │ ├── test_as_manager.launch │ └── test_as_manager.py ├── motion_action │ └── __init__.py ├── package.xml ├── preset │ ├── 0.toml │ ├── 101.toml │ ├── 102.toml │ ├── 111.toml │ ├── 121.toml │ ├── 122.toml │ ├── 123.toml │ ├── 124.toml │ ├── 125.toml │ ├── 126.toml │ ├── 127.toml │ ├── 128.toml │ ├── 129.toml │ ├── 130.toml │ ├── 131.toml │ ├── 132.toml │ ├── 133.toml │ ├── 134.toml │ ├── 135.toml │ ├── 136.toml │ ├── 137.toml │ ├── 138.toml │ ├── 140.toml │ ├── 141.toml │ ├── 142.toml │ ├── 143.toml │ ├── 144.toml │ ├── 145.toml │ ├── 146.toml │ ├── 147.toml │ ├── 148.toml │ ├── 149.toml │ ├── 150.toml │ ├── 151.toml │ ├── 152.toml │ ├── 153.toml │ ├── 154.toml │ ├── 155.toml │ ├── 156.toml │ ├── 157.toml │ ├── 158.toml │ ├── 159.toml │ ├── 160.toml │ ├── 161.toml │ ├── 162.toml │ ├── 163.toml │ ├── 164.toml │ ├── 165.toml │ ├── 166.toml │ ├── 167.toml │ ├── 168.toml │ ├── 169.toml │ ├── 170.toml │ ├── 171.toml │ ├── 172.toml │ ├── 173.toml │ ├── 174.toml │ ├── 175.toml │ ├── 176.toml │ ├── 177.toml │ ├── 178.toml │ ├── 179.toml │ ├── 180.toml │ ├── 181.toml │ ├── 201.toml │ ├── 202.toml │ ├── 211.toml │ ├── 212.toml │ ├── 301.toml │ ├── 302.toml │ ├── 303.toml │ ├── 305.toml │ ├── 400.toml │ ├── elec_skin.toml │ ├── motion_id_map.toml │ └── user_gait_400.toml ├── scripts │ ├── AMENT_IGNORE │ ├── motion_demo.py │ ├── motion_teleop.py │ ├── motion_units.py │ ├── pose_teleop.py │ └── statistics.py ├── src │ └── motion_action.cpp └── test │ ├── AMENT_IGNORE │ ├── client_test.cpp │ ├── controller_test.cpp │ ├── manager_test.cpp │ └── publisher_test.cpp ├── motion_bridge ├── CMakeLists.txt ├── include │ └── motion_bridge │ │ ├── elevation_bridge.hpp │ │ ├── file_bridge.hpp │ │ ├── imu_bridge.hpp │ │ ├── motor_bridge.hpp │ │ └── odom_out_publisher.hpp ├── package.xml ├── src │ ├── elevation_bridge.cpp │ ├── file_bridge.cpp │ ├── imu_bridge.cpp │ ├── motor_bridge.cpp │ └── odom_out_publisher.cpp └── test │ ├── AMENT_IGNORE │ ├── as_controller.cpp │ └── as_controller_file.cpp ├── motion_manager ├── CMakeLists.txt ├── config │ └── priority.toml ├── include │ └── motion_manager │ │ ├── motion_decision.hpp │ │ ├── motion_handler.hpp │ │ └── motion_manager.hpp ├── package.xml ├── src │ ├── main.cpp │ ├── motion_decision.cpp │ ├── motion_handler.cpp │ └── motion_manager.cpp └── test │ └── gait_factory.toml ├── motion_utils ├── CMakeLists.txt ├── config │ ├── edge_align.toml │ └── stair_align.toml ├── include │ └── motion_utils │ │ ├── edge_align.hpp │ │ ├── edge_perception.hpp │ │ ├── motion_utils.hpp │ │ ├── stair_align.hpp │ │ └── stair_perception.hpp ├── launch │ ├── stair_align_launch.py │ └── tof_node_launch.py ├── motion_utils │ └── __init__.py ├── package.xml ├── scripts │ ├── AMENT_IGNORE │ ├── head_tof_pcl_publisher.py │ └── servo_delay_statistics.py ├── src │ └── motion_utils.cpp └── test │ ├── AMENT_IGNORE │ ├── distance_test.cpp │ ├── duration_test.cpp │ ├── edge_align.cpp │ ├── edge_perception.cpp │ ├── edge_perception_test.cpp │ ├── stair_align.cpp │ ├── stair_perception.cpp │ └── stair_perception_test.cpp └── skin_manager ├── CMakeLists.txt ├── config └── elec_skin.toml ├── include └── skin_manager │ └── skin_manager.hpp ├── package.xml └── src └── skin_manager.cpp /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Desktop (please complete the following information):** 27 | - OS: [e.g. iOS] 28 | - Browser [e.g. chrome, safari] 29 | - Version [e.g. 22] 30 | 31 | **Smartphone (please complete the following information):** 32 | - Device: [e.g. iPhone6] 33 | - OS: [e.g. iOS8.1] 34 | - Browser [e.g. stock browser, safari] 35 | - Version [e.g. 22] 36 | 37 | **Additional context** 38 | Add any other context about the problem here. 39 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/custom.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Custom issue template 3 | about: Describe this issue template's purpose here. 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | 11 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /.github/workflows/workflow.yml: -------------------------------------------------------------------------------- 1 | name: GitHub Actions CI 2 | run-name: ${{ github.actor }} is run GitHub Actions 🚀 3 | on: [push] 4 | defaults: 5 | run: 6 | shell: bash 7 | jobs: 8 | build-job: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: Login to Docker Hub 12 | uses: docker/login-action@v2 13 | with: 14 | registry: ghcr.io 15 | username: ${{ github.actor }} 16 | password: ${{ secrets.GIT_TOKEN }} 17 | - name: Install dependence 18 | run: | 19 | sudo apt install -y qemu-user-static binfmt-support 20 | - name: Download code 21 | uses: actions/checkout@v3 22 | - name: Build and code test 23 | run: | 24 | docker pull ghcr.io/miroboticslab/cyberdog:v1 25 | docker run -i -v $GITHUB_WORKSPACE:/home/ros2/src ghcr.io/miroboticslab/cyberdog:v1 bash -c \ 26 | "cd /home/ros2 && source /opt/ros2/galactic/setup.bash \ 27 | && colcon build --packages-up-to motion_manager motion_action motion_bridge motion_utils elec_skin skin_manager \ 28 | && colcon test --event-handlers console_cohesion+ --return-code-on-test-failure --packages-select motion_manager motion_action motion_bridge motion_utils elec_skin skin_manager" 29 | # colcon build test 30 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # cache and binary files during ros build 2 | build/ 3 | log/ 4 | install/ 5 | 6 | # files create by macOS 7 | *.DS_Store 8 | 9 | # vs code workspace 10 | .vscode/ 11 | 12 | # swp files 13 | *.swp 14 | 15 | .github 16 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # cyberdog_motion 2 | 3 | *L91主控运动管理* 4 | 5 | ### 软件架构 6 | 7 | [设计文档](blob:https://xiaomi.f.mioffice.cn/29484161-1078-4de7-9dee-0ac4db7e3b0f) 8 | 9 | - motion_manager：运动管理单元，所有L91软件调用运动能力，均由该模块负责管理； 10 | - motion_action：运动指令单元，所有对运动的调用均经过该模块转发； 11 | -------------------------------------------------------------------------------- /elec_skin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(elec_skin) 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(toml REQUIRED) 20 | find_package(ament_index_cpp REQUIRED) 21 | find_package(cyberdog_common REQUIRED) 22 | find_package(cyberdog_embed_protocol REQUIRED) 23 | 24 | include_directories(include) 25 | 26 | add_executable(${PROJECT_NAME} src/main.cpp) 27 | 28 | # MESSAGE('--------------${CMAKE_INSTALL_PREFIX}------------------') 29 | 30 | ament_target_dependencies( ${PROJECT_NAME} 31 | rclcpp 32 | std_msgs 33 | toml 34 | ament_index_cpp 35 | cyberdog_common 36 | cyberdog_embed_protocol ) 37 | 38 | install( 39 | DIRECTORY include/ 40 | DESTINATION include 41 | ) 42 | 43 | install(TARGETS ${PROJECT_NAME} 44 | DESTINATION lib/${PROJECT_NAME}) 45 | 46 | install( 47 | DIRECTORY include/ 48 | DESTINATION include/ 49 | ) 50 | 51 | install(DIRECTORY toml_config/ 52 | DESTINATION share/${PROJECT_NAME}/toml_config/ 53 | ) 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 | -------------------------------------------------------------------------------- /elec_skin/include/elec_skin/elec_skin.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 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 ELEC_SKIN__ELEC_SKIN_HPP_ 15 | #define ELEC_SKIN__ELEC_SKIN_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "ament_index_cpp/get_package_share_directory.hpp" 22 | #include "embed_protocol/embed_protocol.hpp" 23 | #include "cyberdog_common/cyberdog_log.hpp" 24 | #include "elec_skin/elec_skin_base.hpp" 25 | 26 | #define EVM cyberdog::embed 27 | 28 | namespace cyberdog 29 | { 30 | namespace motion 31 | { 32 | 33 | typedef struct _can_data 34 | { 35 | uint8_t data0; 36 | uint8_t data1; 37 | uint8_t data2; 38 | uint8_t data3; 39 | } can_data; 40 | 41 | class ElecSkin final : public ElecSkinBase 42 | { 43 | public: 44 | ElecSkin() 45 | : ms_(ModelSwitch::MS_FLASH) 46 | { 47 | std::string path = ament_index_cpp::get_package_share_directory("elec_skin") + 48 | "/toml_config/elec_skin.toml"; 49 | ptr_can_protocol_ = std::make_shared>(path, false); 50 | ptr_can_protocol_->SetDataCallback( 51 | std::bind( 52 | &ElecSkin::recv_can_callback, this, 53 | std::placeholders::_1, std::placeholders::_2)); 54 | } 55 | 56 | ~ElecSkin() 57 | { 58 | } 59 | 60 | void ModelControl(ModelSwitch ms, uint16_t milsecs) 61 | { 62 | ms_ = ms; 63 | uint8_t wave_mode = static_cast(ms_); 64 | uint8_t wave_cycle_time_high = *(reinterpret_cast(&milsecs)); 65 | uint8_t wave_cycle_time_low = *(reinterpret_cast(&milsecs) + 1); 66 | ptr_can_protocol_->Operate( 67 | "model_on", 68 | std::vector{wave_mode, wave_cycle_time_low, wave_cycle_time_high}); 69 | } 70 | 71 | void PositionContril( 72 | PositionSkin ps, PositionColorChangeDirection pccd, 73 | PositionColorStartDirection pcsd, uint16_t milsecs) 74 | { 75 | if (ms_ != ModelSwitch::MS_CONTROL) { 76 | ModelControl(ModelSwitch::MS_CONTROL, milsecs); 77 | } 78 | uint8_t change_direction = static_cast(pccd); 79 | uint8_t start_direction = static_cast(pcsd); 80 | uint8_t wave_cycle_time_high = *(reinterpret_cast(&milsecs)); 81 | uint8_t wave_cycle_time_low = *(reinterpret_cast(&milsecs) + 1); 82 | ptr_can_protocol_->Operate( 83 | control_dog_action_map.at(ps), 84 | std::vector{change_direction, start_direction, 85 | wave_cycle_time_low, wave_cycle_time_high}); 86 | } 87 | 88 | private: 89 | void recv_can_callback(std::string & name, std::shared_ptr data) 90 | { 91 | (void) name; 92 | (void) data; 93 | } 94 | 95 | private: 96 | std::shared_ptr> ptr_can_protocol_; 97 | ModelSwitch ms_; 98 | const std::map control_dog_action_map = { 99 | {PositionSkin::PS_BODYM, "body_middle"}, 100 | {PositionSkin::PS_LBLEG, "left_back_leg"}, 101 | {PositionSkin::PS_BODYL, "body_left"}, 102 | {PositionSkin::PS_LFLEG, "left_front_leg"}, 103 | {PositionSkin::PS_FRONT, "front_chest"}, 104 | {PositionSkin::PS_RFLEG, "right_front_leg"}, 105 | {PositionSkin::PS_BODYR, "body_right"}, 106 | {PositionSkin::PS_RBLEG, "right_back_leg"} 107 | }; 108 | }; // class ElecSkin 109 | } // namespace motion 110 | } // namespace cyberdog 111 | 112 | #endif // ELEC_SKIN__ELEC_SKIN_HPP_ 113 | -------------------------------------------------------------------------------- /elec_skin/include/elec_skin/elec_skin_base.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 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 ELEC_SKIN__ELEC_SKIN_BASE_HPP_ 15 | #define ELEC_SKIN__ELEC_SKIN_BASE_HPP_ 16 | 17 | #include 18 | 19 | namespace cyberdog 20 | { 21 | namespace motion 22 | { 23 | 24 | enum class ModelSwitch : uint8_t 25 | { 26 | MS_FLASH = 0, // 闪烁 27 | MS_WAVEF = 1, // 动画前向后变 28 | MS_RANDOM = 2, // 随机 29 | MS_WAVEB = 3, // 动画后向前变 30 | MS_CONTROL = 4, // 上位机实时控制模式 31 | }; // enum class PositionSkin 32 | 33 | enum class PositionSkin : uint8_t 34 | { 35 | PS_BODYM = 0, // 背部 36 | PS_LBLEG = 1, // 左后腿 37 | PS_BODYL = 2, // 左侧 38 | PS_LFLEG = 3, // 左前腿 39 | PS_FRONT = 4, // 前胸 40 | PS_RFLEG = 5, // 右前腿 41 | PS_BODYR = 6, // 右侧 42 | PS_RBLEG = 7, // 右后腿 43 | }; // enum class PositionSkin 44 | 45 | enum class PositionColorChangeDirection : uint8_t 46 | { 47 | PCCD_WTOB = 1, // 白变黑 48 | PCCD_BTOW = 0, // 黑变白 49 | }; // enum class PositionColorChangeDirection 50 | 51 | enum class PositionColorStartDirection : uint8_t 52 | { 53 | PCSD_FRONT = 0, // 前方先变色 54 | PCSD_BACK = 1, // 后方先变色 55 | }; // enum class PositionColorStartDirection 56 | 57 | class ElecSkinBase 58 | { 59 | public: 60 | virtual void ModelControl(ModelSwitch ms, uint16_t milsecs) = 0; 61 | virtual void PositionContril( 62 | PositionSkin ps, PositionColorChangeDirection pccd, 63 | PositionColorStartDirection pcsd, uint16_t milsecs) = 0; 64 | }; 65 | 66 | } // namespace motion 67 | } // namespace cyberdog 68 | 69 | #endif // ELEC_SKIN__ELEC_SKIN_BASE_HPP_ 70 | -------------------------------------------------------------------------------- /elec_skin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | elec_skin 5 | 0.0.0 6 | TODO: Package description 7 | liukai21 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rclcpp 12 | std_msgs 13 | toml 14 | ament_index_cpp 15 | cyberdog_common 16 | cyberdog_embed_protocol 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /elec_skin/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 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 "elec_skin/elec_skin_base.hpp" 20 | #include "elec_skin/elec_skin.hpp" 21 | #include "cyberdog_common/cyberdog_log.hpp" 22 | #include "std_msgs/msg/string.hpp" 23 | 24 | #define NODE_NAME "elec_skin_node_test" 25 | 26 | // 使用字符分割 27 | void string_split( 28 | const std::string & str, 29 | const char split, std::vector & res) 30 | { 31 | if (str == "") { 32 | return; 33 | } 34 | // 在字符串末尾也加入分隔符，方便截取最后一段 35 | std::string strs = str + split; 36 | size_t pos = strs.find(split); 37 | // 若找不到内容则字符串搜索函数返回 npos 38 | while (pos != strs.npos) { 39 | std::string temp = strs.substr(0, pos); 40 | res.push_back(temp); 41 | // 去掉已分割的字符串,在剩下的字符串中进行分割 42 | strs = strs.substr(pos + 1, strs.size()); 43 | pos = strs.find(split); 44 | } 45 | } 46 | 47 | class ElecSkinNode : public rclcpp::Node 48 | { 49 | public: 50 | ElecSkinNode() 51 | : Node(NODE_NAME) 52 | { 53 | subscription_ = this->create_subscription( 54 | "elec_skin_test", 55 | 10, 56 | [this](std_msgs::msg::String::SharedPtr msg) { 57 | std::vector str_vec; 58 | string_split(msg->data, ',', str_vec); 59 | if (str_vec.size() < 1) { 60 | return; 61 | } 62 | uint8_t method = 255; 63 | std::map m_cmd {{"model", 0}, {"position", 1}}; 64 | if (m_cmd.find(str_vec[0]) == m_cmd.end()) { 65 | return; 66 | } 67 | INFO("handle elec skin control:>>>"); 68 | method = m_cmd[str_vec[0]]; 69 | switch (method) { 70 | case 0: 71 | { 72 | if (str_vec.size() != 3) { 73 | INFO("parameter counts error, should be 3!"); 74 | return; 75 | } 76 | cyberdog::motion::ModelSwitch ms = 77 | cyberdog::motion::ModelSwitch(atoi(str_vec[1].c_str())); 78 | uint16_t milsecs = static_cast(atoi(str_vec[2].c_str())); 79 | can_control_->ModelControl(ms, milsecs); 80 | } 81 | break; 82 | case 1: 83 | { 84 | if (str_vec.size() != 5) { 85 | INFO("parameter counts error, should be 4!"); 86 | return; 87 | } 88 | cyberdog::motion::PositionSkin ps = 89 | cyberdog::motion::PositionSkin(atoi(str_vec[1].c_str())); 90 | cyberdog::motion::PositionColorChangeDirection pccd = 91 | cyberdog::motion::PositionColorChangeDirection(atoi(str_vec[2].c_str())); 92 | cyberdog::motion::PositionColorStartDirection pcsd = 93 | cyberdog::motion::PositionColorStartDirection(atoi(str_vec[3].c_str())); 94 | uint16_t milsecs = static_cast(atoi(str_vec[4].c_str())); 95 | can_control_->PositionContril(ps, pccd, pcsd, milsecs); 96 | } 97 | break; 98 | default: 99 | break; 100 | } 101 | }); 102 | can_control_ = std::make_unique(); 103 | } 104 | 105 | private: 106 | rclcpp::Subscription::SharedPtr subscription_; 107 | std::unique_ptr can_control_; 108 | }; 109 | 110 | int main(int argc, char ** argv) 111 | { 112 | rclcpp::init(argc, argv); 113 | LOGGER_MAIN_INSTANCE("elec_skin"); 114 | rclcpp::spin(std::make_shared()); 115 | rclcpp::shutdown(); 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /elec_skin/toml_config/elec_skin.toml: -------------------------------------------------------------------------------- 1 | # Testing normal usage STD_CAN with std_frame 2 | protocol = "can" 3 | name = "elec_skin" 4 | 5 | can_interface = "can0" 6 | extended_frame = true 7 | canfd_enable = false 8 | timeout_us = 1234567 9 | 10 | 11 | [[cmd]] 12 | description = "" 13 | cmd_name = "model_on" 14 | can_id = "0x0e802f00" 15 | ctrl_len = 0 16 | 17 | [[cmd]] 18 | description = "body middle" 19 | cmd_name = "body_middle" 20 | can_id = "0x0e802000" 21 | ctrl_len = 0 22 | 23 | [[cmd]] 24 | description = "left back leg" 25 | cmd_name = "left_back_leg" 26 | can_id = "0x0e802100" 27 | ctrl_len = 0 28 | 29 | [[cmd]] 30 | description = "" 31 | cmd_name = "body_left" 32 | can_id = "0x0e802200" 33 | ctrl_len = 0 34 | 35 | [[cmd]] 36 | description = "" 37 | cmd_name = "left_front_leg" 38 | can_id = "0x0e802300" 39 | ctrl_len = 0 40 | 41 | [[cmd]] 42 | description = "" 43 | cmd_name = "front_chest" 44 | can_id = "0x0e802400" 45 | ctrl_len = 0 46 | 47 | [[cmd]] 48 | description = "" 49 | cmd_name = "right_front_leg" 50 | can_id = "0x0e802500" 51 | ctrl_len = 0 52 | 53 | [[cmd]] 54 | description = "" 55 | cmd_name = "body_right" 56 | can_id = "0x0e802600" 57 | ctrl_len = 0 58 | 59 | [[cmd]] 60 | description = "" 61 | cmd_name = "right_back_leg" 62 | can_id = "0x0e802700" 63 | ctrl_len = 0 -------------------------------------------------------------------------------- /motion_action/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(motion_action) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic -g) 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(ament_cmake_python REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(rclpy REQUIRED) 21 | find_package(protocol REQUIRED) 22 | find_package(cyberdog_system REQUIRED) 23 | find_package(cyberdog_debug REQUIRED) 24 | find_package(cyberdog_common) 25 | find_package(lcm REQUIRED) 26 | find_package(skin_manager REQUIRED) 27 | 28 | include_directories(include) 29 | 30 | add_library(${PROJECT_NAME} SHARED src/motion_action.cpp) 31 | target_link_libraries(${PROJECT_NAME} lcm) 32 | ament_target_dependencies(${PROJECT_NAME} 33 | rclcpp 34 | protocol 35 | cyberdog_system 36 | cyberdog_common 37 | cyberdog_debug 38 | skin_manager 39 | ) 40 | 41 | # add_executable(manager_test test/manager_test.cpp) 42 | # target_link_libraries(manager_test ${PROJECT_NAME} lcm) 43 | # ament_target_dependencies(manager_test lcm cyberdog_common cyberdog_debug) 44 | 45 | add_executable(controller_test test/controller_test.cpp) 46 | target_link_libraries(controller_test ${PROJECT_NAME} lcm) 47 | ament_target_dependencies(controller_test lcm cyberdog_common cyberdog_debug) 48 | 49 | add_executable(publisher_test test/publisher_test.cpp) 50 | target_link_libraries(publisher_test ${PROJECT_NAME} lcm) 51 | ament_target_dependencies(publisher_test lcm cyberdog_common cyberdog_debug) 52 | 53 | add_executable(client_test test/client_test.cpp) 54 | target_link_libraries(client_test ${PROJECT_NAME} lcm) 55 | ament_target_dependencies(client_test lcm cyberdog_common cyberdog_debug) 56 | 57 | install( 58 | DIRECTORY include/ 59 | DESTINATION include 60 | ) 61 | install( 62 | DIRECTORY launch 63 | DESTINATION share/${PROJECT_NAME} 64 | ) 65 | install( 66 | DIRECTORY preset 67 | DESTINATION share/${PROJECT_NAME} 68 | ) 69 | install(TARGETS 70 | ${PROJECT_NAME} 71 | # manager_test 72 | controller_test 73 | publisher_test 74 | client_test 75 | LIBRARY DESTINATION lib 76 | RUNTIME DESTINATION lib/${PROJECT_NAME} 77 | ) 78 | 79 | ament_python_install_package(${PROJECT_NAME}) 80 | 81 | install(PROGRAMS 82 | scripts/motion_teleop.py 83 | scripts/pose_teleop.py 84 | scripts/statistics.py 85 | scripts/motion_units.py 86 | scripts/motion_demo.py 87 | DESTINATION lib/${PROJECT_NAME} 88 | ) 89 | 90 | if(BUILD_TESTING) 91 | find_package(ament_lint_auto REQUIRED) 92 | ament_lint_auto_find_test_dependencies() 93 | endif() 94 | 95 | ament_export_include_directories( 96 | include 97 | ) 98 | ament_export_libraries( 99 | ${PROJECT_NAME} 100 | ) 101 | ament_export_dependencies(lcm cyberdog_common cyberdog_debug skin_manager) 102 | ament_package() 103 | -------------------------------------------------------------------------------- /motion_action/include/motion_action/motion_action.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 MOTION_ACTION__MOTION_ACTION_HPP_ 15 | #define MOTION_ACTION__MOTION_ACTION_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 "protocol/msg/motion_servo_cmd.hpp" 28 | #include "protocol/srv/motion_result_cmd.hpp" 29 | #include "protocol/msg/motion_status.hpp" 30 | #include "protocol/msg/motion_servo_response.hpp" 31 | #include "protocol/lcm/robot_control_response_lcmt.hpp" 32 | #include "protocol/lcm/robot_control_cmd_lcmt.hpp" 33 | #include "protocol/lcm/state_estimator_lcmt.hpp" 34 | #include "cyberdog_common/cyberdog_log.hpp" 35 | #include "cyberdog_common/cyberdog_toml.hpp" 36 | #include "motion_action/motion_macros.hpp" 37 | #include "ament_index_cpp/get_package_share_directory.hpp" 38 | #include "ament_index_cpp/get_package_prefix.hpp" 39 | // #include "elec_skin/elec_skin_base.hpp" 40 | #include "skin_manager/skin_manager.hpp" 41 | 42 | namespace cyberdog 43 | { 44 | namespace motion 45 | { 46 | 47 | struct MotionIdMap 48 | { 49 | std::vector map; 50 | std::vector pre_motion; 51 | std::vector post_motion; 52 | int32_t min_exec_time; 53 | }; // struct MotionIdMap 54 | 55 | inline bool CompareLcmResponse(const LcmResponse & res1, const LcmResponse & res2) 56 | { 57 | bool flag = true; 58 | for (uint8_t i = 0; i < 12; ++i) { 59 | flag &= (res1.motor_error[i] == res2.motor_error[i]); 60 | } 61 | return res1.mode == res2.mode && 62 | res1.gait_id == res2.gait_id && 63 | res1.contact == res2.contact && 64 | res1.order_process_bar == res2.order_process_bar && 65 | res1.switch_status == res2.switch_status && 66 | res1.ori_error == res2.ori_error && 67 | res1.footpos_error == res2.footpos_error && 68 | res1.motor_error && flag; 69 | } 70 | class MotionAction final 71 | { 72 | public: 73 | MotionAction(); 74 | ~MotionAction(); 75 | 76 | public: 77 | void Execute(const MotionServoCmdMsg::SharedPtr msg); 78 | void Execute(const MotionResultSrv::Request::SharedPtr request); 79 | void Execute(const MotionSequenceShowSrv::Request::SharedPtr request); 80 | void Execute(const MotionQueueCustomSrv::Request::SharedPtr request); 81 | void Execute(const robot_control_cmd_lcmt & lcm); 82 | void RegisterFeedback(std::function feedback); 83 | void RegisterTomlLog(std::function toml_log); 84 | bool Init( 85 | const std::string & publish_url = kLCMActionPublishURL, 86 | const std::string & subscribe_url = kLCMActionSubscibeURL); 87 | bool SelfCheck(); 88 | std::map GetMotionIdMap() {return motion_id_map_;} 89 | bool SequenceDefImpl(const std::string & toml_data); 90 | void ShowDebugLog(bool show) {show_ = show;} 91 | // void ShowDefaultSkin(bool default_color, bool align_contact) 92 | // { 93 | // INFO("ShowDefaultSkin"); 94 | // align_contact_ = align_contact; 95 | // PositionColorChangeDirection dir; 96 | // dir = default_color ? change_dir_.front() : change_dir_.back(); 97 | // for (auto leg : leg_map) { 98 | // for (auto p : leg.second) { 99 | // elec_skin_->PositionContril( 100 | // p, 101 | // dir, 102 | // start_dir_, 103 | // gradual_duration_); 104 | // } 105 | // } 106 | // } 107 | // void ShowStandElecSkin() 108 | // { 109 | // INFO("ShowStandElecSkin"); 110 | // align_contact_ = false; 111 | // elec_skin_->ModelControl(ModelSwitch::MS_WAVEF, stand_gradual_duration_); 112 | // } 113 | // void ShowTwinkElecSkin() 114 | // { 115 | // INFO("ShowTwinkElecSkin"); 116 | // align_contact_ = false; 117 | // elec_skin_->ModelControl(ModelSwitch::MS_FLASH, twink_gradual_duration_); 118 | // } 119 | // void ShowRandomElecSkin() 120 | // { 121 | // INFO("ShowRandomElecSkin"); 122 | // align_contact_ = false; 123 | // elec_skin_->ModelControl(ModelSwitch::MS_RANDOM, random_gradual_duration_); 124 | // } 125 | // void SetAlignContact(bool align_contact) 126 | // { 127 | // align_contact_ = align_contact; 128 | // } 129 | void ShowBlackSkin() 130 | { 131 | bool enable = elec_skin_manager_->GetEnableStatus(); 132 | if (!enable) { 133 | return; 134 | } 135 | elec_skin_manager_->SetAlignContact(false); 136 | elec_skin_manager_->ShowBlackElecSkin(gradual_duration_); 137 | } 138 | 139 | void ShowWhiteSkin() 140 | { 141 | bool enable = elec_skin_manager_->GetEnableStatus(); 142 | if (!enable) { 143 | return; 144 | } 145 | elec_skin_manager_->SetAlignContact(true); 146 | elec_skin_manager_->ShowWhiteElecSkin(gradual_duration_); 147 | } 148 | 149 | void SetState(const MotionMgrState & state) 150 | { 151 | state_ = state; 152 | } 153 | 154 | private: 155 | void WriteLcm(); 156 | void ReadActionResponseLcm( 157 | const lcm::ReceiveBuffer *, const std::string &, 158 | const robot_control_response_lcmt * msg); 159 | void ReadSeqDefResultLcm( 160 | const lcm::ReceiveBuffer *, const std::string &, 161 | const file_recv_lcmt * msg); 162 | void ReadStateEstimatorLcm( 163 | const lcm::ReceiveBuffer *, const std::string &, 164 | const state_estimator_lcmt * msg); 165 | bool ParseMotionIdMap(); 166 | // bool ParseElecSkin(); 167 | 168 | private: 169 | std::thread control_thread_, response_thread_; 170 | std::function feedback_func_; 171 | std::function toml_log_func_; 172 | std::shared_ptr lcm_publish_instance_, lcm_subscribe_instance_; 173 | std::shared_ptr lcm_recv_subscribe_instance_; 174 | std::shared_ptr lcm_state_estimator_subscribe_instance_; 175 | // std::shared_ptr elec_skin_{nullptr}; 176 | std::shared_ptr elec_skin_manager_{nullptr}; 177 | // std::unordered_map> leg_map; 178 | std::mutex lcm_write_mutex_; 179 | std::mutex seq_def_result_mutex_; 180 | std::condition_variable seq_def_result_cv_; 181 | robot_control_cmd_lcmt lcm_cmd_; 182 | std::map motion_id_map_; 183 | // std::vector change_dir_; 184 | // PositionColorStartDirection start_dir_; 185 | MotionMgrState state_; 186 | int32_t last_motion_id_{0}; 187 | int32_t gradual_duration_{50}; 188 | // int32_t stand_gradual_duration_{0}; 189 | // int32_t twink_gradual_duration_{0}; 190 | // int32_t random_gradual_duration_{0}; 191 | uint8_t lcm_publish_duration_; 192 | int8_t last_res_mode_{0}, last_res_gait_id_{0}; 193 | int8_t life_count_{0}; 194 | bool lcm_cmd_init_{false}, ins_init_{false}; 195 | bool sequence_recv_result_{false}, sequence_def_result_waiting_{false}; 196 | bool show_{false}; 197 | // bool align_contact_{true}; 198 | bool lcm_ready_{false}; 199 | LOGGER_MINOR_INSTANCE("MotionAction"); 200 | }; // class MotionAction 201 | } // namespace motion 202 | } // namespace cyberdog 203 | #endif // MOTION_ACTION__MOTION_ACTION_HPP_ 204 | -------------------------------------------------------------------------------- /motion_action/include/motion_action/motion_macros.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 MOTION_ACTION__MOTION_MACROS_HPP_ 15 | #define MOTION_ACTION__MOTION_MACROS_HPP_ 16 | #include 17 | #include 18 | #include "cyberdog_system/robot_code.hpp" 19 | #include "protocol/msg/motion_servo_cmd.hpp" 20 | #include "protocol/msg/motion_status.hpp" 21 | #include "protocol/msg/motion_servo_response.hpp" 22 | #include "protocol/msg/motion_id.hpp" 23 | #include "protocol/msg/motion_code.hpp" 24 | #include "protocol/msg/motion_sequence_param.hpp" 25 | #include "protocol/lcm/robot_control_response_lcmt.hpp" 26 | #include "protocol/srv/motion_result_cmd.hpp" 27 | #include "protocol/srv/motion_queue_custom_cmd.hpp" 28 | #include "protocol/srv/motion_custom_cmd.hpp" 29 | #include "protocol/srv/motion_sequence.hpp" 30 | #include "protocol/srv/motion_sequence_show.hpp" 31 | #include "protocol/lcm/file_send_lcmt.hpp" 32 | #include "protocol/lcm/file_recv_lcmt.hpp" 33 | 34 | namespace cyberdog 35 | { 36 | namespace motion 37 | { 38 | 39 | // 所有的motion相关code都从3000开始，该值为全局架构设计分配 40 | enum class MotionCode : int32_t 41 | { 42 | kMotionSwitchEstop = 21, 43 | kMotionSwitchBantrans = 22, 44 | kMotionSwitchEdamp = 23, 45 | kMotionSwitchLifted = 24, 46 | kMotionSwitchOverHeat = 25, 47 | kMotionSwitchLowBat = 26, 48 | kMotionTransitionTimeout = 27, 49 | kMotionExecuteTimeout = 28, 50 | kMotionExecuteError = 29, 51 | 52 | // kCommandInvalid = 30, 53 | kSequenceDefError = 31, 54 | kHwMotorOffline = 32, 55 | kHwMotorOverHeat = 33, 56 | kHwMotorOverLoad = 34, 57 | kComLcmTimeout = 35, 58 | kEstop = 40, 59 | kStuck = 41, 60 | // kBusy = 42 61 | kMotionSwitchOriErr = 50, 62 | kMotionSwitchFootPosErr = 51, 63 | kMotionSwitchStandStuck = 52, 64 | kMotionSwitchMotorOverHeat = 53, 65 | kMotionSwitchMotorOverCurr = 54, 66 | kMotionSwitchMotorErr = 55, 67 | kMotionSwitchCharging = 56, 68 | }; // enum class MotionCode 69 | 70 | using MotionServoCmdMsg = protocol::msg::MotionServoCmd; 71 | using LcmResponse = robot_control_response_lcmt; 72 | using MotionResultSrv = protocol::srv::MotionResultCmd; 73 | using MotionQueueCustomSrv = protocol::srv::MotionQueueCustomCmd; 74 | using MotionSequenceSrv = protocol::srv::MotionSequence; 75 | using MotionSequenceShowSrv = protocol::srv::MotionSequenceShow; 76 | using MotionCustomSrv = protocol::srv::MotionCustomCmd; 77 | using MotionStatusMsg = protocol::msg::MotionStatus; 78 | using MotionServoResponseMsg = protocol::msg::MotionServoResponse; 79 | using MotionIDMsg = protocol::msg::MotionID; 80 | using MCode = cyberdog::system::CyberdogCode; 81 | 82 | constexpr uint8_t kActionLcmPublishFrequency = 20; 83 | constexpr uint8_t kServoDataLostTimesThreshold = 4; 84 | constexpr uint16_t kTransitioningTimeout = 3000; // millisecond 85 | constexpr uint16_t kAcitonLcmReadTimeout = 200; // millisecond 86 | constexpr int kMotorNormal = -2147483648; 87 | constexpr const char * kLCMActionPublishURL = "udpm://239.255.76.67:7671?ttl=255"; 88 | constexpr const char * kLCMActionSubscibeURL = "udpm://239.255.76.67:7670?ttl=255"; 89 | constexpr const char * kLCMBirdgeSubscribeURL = "udpm://239.255.76.67:7667?ttl=255"; 90 | constexpr const char * kLCMActionControlChannel = "robot_control_cmd"; 91 | constexpr const char * kLCMActionResponseChannel = "robot_control_response"; 92 | constexpr const char * kLCMActionSequenceDefChannel = "user_gait_file"; 93 | constexpr const char * kLCMActionSeqDefResultChannel = "user_gait_result"; 94 | constexpr const char * kLCMBridgeImuChannel = "external_imu"; 95 | constexpr const char * kLCMBridgeElevationChannel = "local_heightmap"; 96 | constexpr const char * kLCMBridgeOdomChannel = "global_to_robot"; 97 | constexpr const char * kLCMBridgeFileChannel = "custom_motion"; 98 | constexpr const char * kLCMBridgeMotorChannel = "motor_temperature"; 99 | constexpr const char * kMotionServoCommandTopicName = "motion_servo_cmd"; 100 | constexpr const char * kMotionServoResponseTopicName = "motion_servo_response"; 101 | constexpr const char * kMotionResultServiceName = "motion_result_cmd"; 102 | constexpr const char * kMotionCustomServiceName = "motion_custom_cmd"; 103 | constexpr const char * kMotionQueueServiceName = "motion_queue_cmd"; 104 | constexpr const char * kMotionSequenceServiceName = "motion_sequence_cmd"; 105 | constexpr const char * kMotionQueueCommandTopicName = "motion_queue_cmd_test"; 106 | constexpr const char * kBridgeOdomTopicName = "odom_out"; 107 | constexpr const char * kMotionStatusTopicName = "motion_status"; 108 | constexpr const char * kGlobalScanTopicName = "scan"; 109 | constexpr const char * kMotionCustomCmdConfigPath = "/"; 110 | // a: src, b: des, c: size, d: description 111 | #define GET_VALUE(a, b, c, d) \ 112 | if (a.size() != c) { \ 113 | DEBUG("Size of %s (%ld) is invalid, all elements will set to 0", d, a.size()); \ 114 | for (uint8_t i = 0; i < c; ++i) { \ 115 | b[i] = 0; \ 116 | } \ 117 | } else { \ 118 | for (uint8_t i = 0; i < c; ++i) { \ 119 | b[i] = a[i]; \ 120 | } \ 121 | } \ 122 | 123 | #define GET_TOML_VALUE(a, b, c) \ 124 | if (!cyberdog::common::CyberdogToml::Get(a, b, c)) { \ 125 | FATAL("Cannot get value %s", b); \ 126 | exit(-1); \ 127 | } \ 128 | 129 | #define GET_TOML_VALUE_ARR(a, b, c, d) \ 130 | if (!cyberdog::common::CyberdogToml::Get(a, b, c)) { \ 131 | FATAL("Cannot get value %s", b); \ 132 | exit(-1); \ 133 | } \ 134 | std::copy(c.begin(), c.end(), d); \ 135 | c.clear(); \ 136 | 137 | /** 138 | * @brief 震荡计数器 139 | * 1. 用于记录及刷新过去一段时间计数器是否被置位 140 | * 2. 计数器只有true / false两种状态，为最简单寄存器模型 141 | * 3. 重复置位为true为无效操作，不影响计数器状态 142 | * 4. 重复置位为false为无效操作，会得到false返回值； 143 | * 5. 该计数器用于检测消息频率合法性场景； 144 | * 145 | */ 146 | struct ServoClick 147 | { 148 | void Tick() 149 | { 150 | data_ = true; 151 | } 152 | 153 | bool Tock() 154 | { 155 | if (data_ == false) { 156 | return false; 157 | } else { 158 | data_ = false; 159 | } 160 | return true; 161 | } 162 | 163 | std::atomic_bool data_ {false}; 164 | }; // struct HeartQueue 165 | 166 | enum class MotionID : int32_t 167 | { 168 | kEstop = 0, 169 | kGetDown = 101, 170 | kRecoveryStand = 111, 171 | kForceControlDefinitively = 201, 172 | kForceControlRelatively = 202, 173 | kPoseControlDefinitively = 211, 174 | kPoseControlRelatively = 212 175 | }; // enmu calss MotionID 176 | 177 | enum class MotionMgrState : uint8_t 178 | { 179 | kUninit, 180 | kSetup, 181 | kTearDown, 182 | kSelfCheck, 183 | kActive, 184 | kDeactive, 185 | kProtected, 186 | kLowPower, 187 | kOTA, 188 | kError 189 | }; 190 | 191 | enum class HandlerStatus : uint8_t 192 | { 193 | kIdle = 0, 194 | kExecutingServoCmd = 1, 195 | kExecutingResultCmd = 2 196 | }; // enum class HandlerStatus 197 | } // namespace motion 198 | } // namespace cyberdog 199 | #endif // MOTION_ACTION__MOTION_MACROS_HPP_ 200 | -------------------------------------------------------------------------------- /motion_action/launch/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/motion/4d241d1cfca1610f3658bac31acea01277b7a47c/motion_action/launch/AMENT_IGNORE -------------------------------------------------------------------------------- /motion_action/launch/test_as_manager.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /motion_action/launch/test_as_manager.py: -------------------------------------------------------------------------------- 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 | import os 16 | 17 | from matplotlib.pyplot import text 18 | from ament_index_python import get_package_share_directory 19 | from launch import LaunchDescription 20 | from launch.actions import DeclareLaunchArgument 21 | from launch.actions import IncludeLaunchDescription 22 | from launch.actions import GroupAction 23 | from launch.launch_description_sources import PythonLaunchDescriptionSource 24 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, LocalSubstitution, PythonExpression 25 | from launch.substitutions import TextSubstitution 26 | from launch_ros.actions import Node 27 | from launch_ros.actions import PushRosNamespace 28 | 29 | def generate_launch_description(): 30 | preset_cmd = LaunchConfiguration('cmd') 31 | sim_cmd = LaunchConfiguration('sim') 32 | declare_cmd_arg = DeclareLaunchArgument('cmd', default_value='1.toml') 33 | declare_sim_arg = DeclareLaunchArgument('sim', default_value='False') 34 | cmd_preset = PathJoinSubstitution([get_package_share_directory('motion_action'), 'preset', preset_cmd]) 35 | 36 | start_test_as_manager_cmd = Node( 37 | package='motion_action', 38 | executable='test_as_manager', 39 | name='test_as_manager', 40 | output='screen', 41 | parameters=[ 42 | {'publish_url' : ''}, 43 | {'subscribe_url' : ''}, 44 | {'cmd_preset' : cmd_preset}] 45 | ) 46 | ld = LaunchDescription() 47 | ld.add_action(declare_cmd_arg) 48 | ld.add_action(declare_sim_arg) 49 | ld.add_action(start_test_as_manager_cmd) 50 | return ld -------------------------------------------------------------------------------- /motion_action/motion_action/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/motion/4d241d1cfca1610f3658bac31acea01277b7a47c/motion_action/motion_action/__init__.py -------------------------------------------------------------------------------- /motion_action/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | motion_action 5 | 0.0.0 6 | TODO: Package description 7 | dukun 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclcpp 14 | rclpy 15 | cyberdog_system 16 | cyberdog_common 17 | cyberdog_debug 18 | protocol 19 | skin_manager 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /motion_action/preset/0.toml: -------------------------------------------------------------------------------- 1 | # OFF状态，即停状态 2 | motion_id = 0 3 | cmd_type = 0 4 | vel_des = [0.0, 0.0, 0.0] 5 | rpy_des = [0.0, 0.0, 0.0] 6 | pos_des = [0.0, 0.0, 0.0] 7 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 8 | ctrl_point = [0.0, 0.0, 0.0] 9 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 10 | step_height = [0.0, 0.0] 11 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/101.toml: -------------------------------------------------------------------------------- 1 | # 高阻尼趴下 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/102.toml: -------------------------------------------------------------------------------- 1 | # 高阻尼趴下 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/111.toml: -------------------------------------------------------------------------------- 1 | # 恢复站立 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/121.toml: -------------------------------------------------------------------------------- 1 | # 后空翻 2 | cmd_type = 0 3 | vel_des = [0.1, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/122.toml: -------------------------------------------------------------------------------- 1 | # 前空翻 2 | cmd_type = 0 3 | vel_des = [0.1, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/123.toml: -------------------------------------------------------------------------------- 1 | # 作揖 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/124.toml: -------------------------------------------------------------------------------- 1 | # 向左打滚一圈 2 | cmd_type = 0 3 | vel_des = [0.1, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/125.toml: -------------------------------------------------------------------------------- 1 | # 遛狗 2 | cmd_type = 0 3 | vel_des = [0.1, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.2] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.05, 0.05] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/126.toml: -------------------------------------------------------------------------------- 1 | # 跳上台阶 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/127.toml: -------------------------------------------------------------------------------- 1 | # 跳上台阶 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/128.toml: -------------------------------------------------------------------------------- 1 | # 跳上台阶 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/129.toml: -------------------------------------------------------------------------------- 1 | # 跳上台阶 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/130.toml: -------------------------------------------------------------------------------- 1 | # 3D跳跃左转90度 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/131.toml: -------------------------------------------------------------------------------- 1 | # 3D跳跃右转90度 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/132.toml: -------------------------------------------------------------------------------- 1 | # 3D跳跃前跳60cm 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/133.toml: -------------------------------------------------------------------------------- 1 | # 3D跳跃前跳30cm 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/134.toml: -------------------------------------------------------------------------------- 1 | # 3D跳跃左跳20cm 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/135.toml: -------------------------------------------------------------------------------- 1 | # 3D跳跃右跳20cm 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/136.toml: -------------------------------------------------------------------------------- 1 | # 3D跳跃向上30cm 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/137.toml: -------------------------------------------------------------------------------- 1 | # 跳下台阶 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/138.toml: -------------------------------------------------------------------------------- 1 | # 向右侧躺下后恢复站立 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/140.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/141.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/142.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/143.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/144.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/145.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/146.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/147.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/148.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/149.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/150.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/151.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/152.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/153.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/154.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/155.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/156.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/157.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/158.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/159.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/160.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/161.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/162.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/163.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/164.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/165.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/166.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/167.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/168.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/169.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/170.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/171.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/172.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/173.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/174.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/175.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/176.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/177.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/178.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/179.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/180.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/181.toml: -------------------------------------------------------------------------------- 1 | # 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.00, 0.00] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/201.toml: -------------------------------------------------------------------------------- 1 | # 绝对量力控姿态 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.2] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/202.toml: -------------------------------------------------------------------------------- 1 | # 增量力控姿态 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 2000 -------------------------------------------------------------------------------- /motion_action/preset/211.toml: -------------------------------------------------------------------------------- 1 | # 绝对量位控姿态 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.2] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 1000 -------------------------------------------------------------------------------- /motion_action/preset/212.toml: -------------------------------------------------------------------------------- 1 | # 相对量位控姿态站立 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 1000 -------------------------------------------------------------------------------- /motion_action/preset/301.toml: -------------------------------------------------------------------------------- 1 | # 前后跳 2 | cmd_type = 0 3 | vel_des = [0.0, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.2] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.05, 0.05] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/302.toml: -------------------------------------------------------------------------------- 1 | # 四足小跳 2 | cmd_type = 0 3 | vel_des = [0.1, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.2] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.05, 0.05] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/303.toml: -------------------------------------------------------------------------------- 1 | # 自变频行走 2 | cmd_type = 0 3 | vel_des = [0.1, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.0] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.05, 0.05] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/305.toml: -------------------------------------------------------------------------------- 1 | # 快跑 2 | cmd_type = 0 3 | vel_des = [0.1, 0.0, 0.0] 4 | rpy_des = [0.0, 0.0, 0.0] 5 | pos_des = [0.0, 0.0, 0.2] 6 | acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 7 | ctrl_point = [0.0, 0.0, 0.0] 8 | foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 9 | step_height = [0.05, 0.05] 10 | duration = 0 -------------------------------------------------------------------------------- /motion_action/preset/400.toml: -------------------------------------------------------------------------------- 1 | [[step]] 2 | mode = 11 3 | gait_id = 81 4 | contact = 30 5 | life_count = 0 6 | vel_des = [ 0.0, 0.0, 0.0,] 7 | rpy_des = [ 0.0, 0.0, 0.0,] 8 | pos_des = [ 0.0, 0.0, 0.0,] 9 | acc_des = [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,] 10 | ctrl_point = [ 0.0, 0.0, 1.0,] 11 | foot_pose = [ 0.0, -0.02, 0.0, 0.02, 0.0, 0.0,] 12 | step_height = [ 35035.0, 20020.0,] 13 | value = 0 14 | duration = 420 15 | 16 | [[step]] 17 | mode = 11 18 | gait_id = 81 19 | contact = 30 20 | life_count = 0 21 | vel_des = [ 0.0, 0.0, 0.0,] 22 | rpy_des = [ 0.0, 0.0, 0.0,] 23 | pos_des = [ 0.0, 0.0, 0.0,] 24 | acc_des = [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,] 25 | ctrl_point = [ 0.0, 0.0, 1.0,] 26 | foot_pose = [ 0.0, 0.02, 0.0, -0.02, 0.0, 0.0,] 27 | step_height = [ 35035.0, 20020.0,] 28 | value = 0 29 | duration = 420 30 | 31 | [[step]] 32 | mode = 11 33 | gait_id = 81 34 | contact = 30 35 | life_count = 0 36 | vel_des = [ 0.0, 0.0, 0.0,] 37 | rpy_des = [ 0.0, 0.0, 0.0,] 38 | pos_des = [ 0.0, 0.0, 0.0,] 39 | acc_des = [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,] 40 | ctrl_point = [ 0.0, 0.0, 1.0,] 41 | foot_pose = [ 0.0, -0.02, 0.0, 0.02, 0.0, 0.0,] 42 | step_height = [ 35035.0, 20020.0,] 43 | value = 0 44 | duration = 420 45 | 46 | [[step]] 47 | mode = 11 48 | gait_id = 81 49 | contact = 30 50 | life_count = 0 51 | vel_des = [ 0.0, 0.0, 0.0,] 52 | rpy_des = [ 0.0, 0.0, 0.0,] 53 | pos_des = [ 0.0, 0.0, 0.0,] 54 | acc_des = [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,] 55 | ctrl_point = [ 0.0, 0.0, 1.0,] 56 | foot_pose = [ 0.0, 0.02, 0.0, -0.02, 0.0, 0.0,] 57 | step_height = [ 35035.0, 20020.0,] 58 | value = 0 59 | duration = 420 60 | 61 | [[step]] 62 | mode = 11 63 | gait_id = 81 64 | contact = 30 65 | life_count = 0 66 | vel_des = [ 0.0, 0.0, 0.0,] 67 | rpy_des = [ 0.0, 0.0, 0.0,] 68 | pos_des = [ 0.0, 0.0, 0.0,] 69 | acc_des = [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,] 70 | ctrl_point = [ 0.0, 0.0, 1.0,] 71 | foot_pose = [ 0.0, -0.02, 0.0, 0.02, 0.0, 0.0,] 72 | step_height = [ 35035.0, 20020.0,] 73 | value = 0 74 | duration = 420 75 | 76 | [[step]] 77 | mode = 11 78 | gait_id = 81 79 | contact = 30 80 | life_count = 0 81 | vel_des = [ 0.0, 0.0, 0.0,] 82 | rpy_des = [ 0.0, 0.0, 0.0,] 83 | pos_des = [ 0.0, 0.0, 0.0,] 84 | acc_des = [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,] 85 | ctrl_point = [ 0.0, 0.0, 1.0,] 86 | foot_pose = [ 0.06, 0.0, 0.06, 0.0, 0.0, 0.0,] 87 | step_height = [ 35035.0, 20020.0,] 88 | value = 0 89 | duration = 660 -------------------------------------------------------------------------------- /motion_action/preset/elec_skin.toml: -------------------------------------------------------------------------------- 1 | default_color = 0 # 0 white, 1 black 2 | start_direction = 0 # 0 front, 1 back 3 | gradual_duration = 50 # milsecs 4 | stand_gradual_duration_ = 3500 # milsecs 5 | twink_gradual_duration_ = 3500 # milsecs 6 | random_gradual_duration_ = 3500 # milsecs -------------------------------------------------------------------------------- /motion_action/preset/user_gait_400.toml: -------------------------------------------------------------------------------- 1 | [[section]] 2 | contact = [1, 1, 1, 1] 3 | duration = 8 4 | 5 | [[section]] 6 | contact = [0, 0, 1, 1] 7 | duration = 6 8 | 9 | [[section]] 10 | contact = [1, 1, 1, 1] 11 | duration = 8 12 | 13 | [[section]] 14 | contact = [0, 0, 1, 1] 15 | duration = 6 16 | 17 | [[section]] 18 | contact = [1, 1, 1, 1] 19 | duration = 8 20 | 21 | [[section]] 22 | contact = [0, 0, 1, 1] 23 | duration = 6 24 | 25 | [[section]] 26 | contact = [1, 1, 1, 1] 27 | duration = 8 28 | 29 | [[section]] 30 | contact = [0, 0, 1, 1] 31 | duration = 6 32 | 33 | [[section]] 34 | contact = [1, 1, 1, 1] 35 | duration = 8 36 | 37 | [[section]] 38 | contact = [0, 0, 1, 1] 39 | duration = 6 40 | 41 | [[section]] 42 | contact = [1, 1, 1, 1] 43 | duration = 8 44 | 45 | [[section]] 46 | contact = [0, 0, 1, 1] 47 | duration = 6 48 | 49 | [[section]] 50 | contact = [1, 1, 1, 1] 51 | duration = 8 52 | -------------------------------------------------------------------------------- /motion_action/scripts/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/motion/4d241d1cfca1610f3658bac31acea01277b7a47c/motion_action/scripts/AMENT_IGNORE -------------------------------------------------------------------------------- /motion_action/scripts/motion_demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # http://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | import motion_units 17 | import time 18 | motion_units.standup() 19 | time.sleep(2) 20 | motion_units.yaw(-0.35, 2000) 21 | time.sleep(2) 22 | motion_units.yaw(0.7, 4000) 23 | time.sleep(4) 24 | motion_units.yaw(-0.35,2000) 25 | time.sleep(2) 26 | 27 | motion_units.pitch(0.25, 2000) 28 | time.sleep(2) 29 | motion_units.pitch(-0.5, 4000) 30 | time.sleep(4) 31 | motion_units.pitch(0.25, 2000) 32 | time.sleep(2) 33 | 34 | motion_units.roll(0.3, 2000) 35 | time.sleep(2) 36 | motion_units.roll(-0.6, 4000) 37 | time.sleep(4) 38 | motion_units.roll(0.3, 2000) 39 | time.sleep(10) 40 | motion_units.getdown() 41 | -------------------------------------------------------------------------------- /motion_action/scripts/statistics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from protocol.msg import MotionServoCmd 4 | import rclpy 5 | import datetime 6 | from rclpy.node import Node 7 | from std_msgs.msg import Float32 8 | 9 | class ServoCmdSubscriber(Node): 10 | 11 | def __init__(self): 12 | super().__init__('servo_subscriber') 13 | self.last_timestamp = datetime.datetime.now().timestamp() 14 | self.subscription = self.create_subscription( 15 | MotionServoCmd, 16 | 'motion_servo_cmd', 17 | self.topic_callback, 18 | 10) 19 | self.publisher = self.create_publisher(Float32, "interval", 1) 20 | 21 | def topic_callback(self, msg): 22 | this_timestamp = datetime.datetime.now().timestamp() 23 | interval = this_timestamp - self.last_timestamp 24 | print(interval) 25 | interval_msg = Float32() 26 | interval_msg.data = interval 27 | self.publisher.publish(interval_msg) 28 | self.last_timestamp = this_timestamp 29 | 30 | def main(args=None): 31 | rclpy.init(args=args) 32 | servo_subscriber = ServoCmdSubscriber() 33 | rclpy.spin(servo_subscriber) 34 | servo_subscriber.destroy_node() 35 | rclpy.shutdown() 36 | 37 | if __name__ == '__main__': 38 | main() 39 | -------------------------------------------------------------------------------- /motion_action/test/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/motion/4d241d1cfca1610f3658bac31acea01277b7a47c/motion_action/test/AMENT_IGNORE -------------------------------------------------------------------------------- /motion_action/test/controller_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 | 20 | class SimMotionController 21 | { 22 | public: 23 | SimMotionController() 24 | { 25 | lcm_sub_.reset(new lcm::LCM(cyberdog::motion::kLCMActionPublishURL)); 26 | lcm_pub_.reset(new lcm::LCM(cyberdog::motion::kLCMActionSubscibeURL)); 27 | lcm_recv_pub_.reset(new lcm::LCM(cyberdog::motion::kLCMActionPublishURL)); 28 | lcm_sub_->subscribe("robot_control_cmd", &SimMotionController::HandleCmd, this); 29 | lcm_sub_->subscribe("user_gait_file", &SimMotionController::HandleFileCmd, this); 30 | std::thread([this]() {while (0 == this->lcm_sub_->handle()) {}}).detach(); 31 | } 32 | void Run() 33 | { 34 | while (true) { 35 | // lcm_sub_->handle(); 36 | lcm_pub_->publish("robot_control_response", &res_); 37 | // INFO( 38 | // "MotionController send res:\n mode: %d\n gait_id: %d\n contact: %d\n order_process_bar: %d\n switch_status: %d\n ori_error: %d\n footpos_error: %d\n motor_error: [%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d]\n", res_.mode, res_.gait_id, res_.contact, res_.order_process_bar, res_.switch_status, res_.ori_error, res_.footpos_error, 39 | // res_.motor_error[0], res_.motor_error[1], res_.motor_error[2], res_.motor_error[3], 40 | // res_.motor_error[4], res_.motor_error[5], res_.motor_error[6], res_.motor_error[7], 41 | // res_.motor_error[8], res_.motor_error[9], res_.motor_error[10], res_.motor_error[11]); 42 | std::this_thread::sleep_for(std::chrono::milliseconds(50)); 43 | } 44 | } 45 | 46 | private: 47 | std::shared_ptr lcm_sub_, lcm_pub_, lcm_recv_pub_; 48 | robot_control_response_lcmt res_; 49 | file_recv_lcmt recv_result_; 50 | void HandleCmd(const lcm::ReceiveBuffer *, const std::string &, 51 | const robot_control_cmd_lcmt * msg) 52 | { 53 | INFO("MotionController get cmd mode %d, gait_id %d, life_count %d", msg->mode, msg->gait_id, msg->life_count); 54 | res_.mode = msg->mode; 55 | res_.gait_id = msg->gait_id; 56 | res_.contact = msg->contact; 57 | res_.order_process_bar = 100; 58 | res_.switch_status = 0; 59 | res_.ori_error = 0; 60 | res_.footpos_error = 0; 61 | memset(res_.motor_error, 0, sizeof(res_.motor_error)); 62 | } 63 | void HandleFileCmd(const lcm::ReceiveBuffer *, const std::string &, 64 | const file_send_lcmt * msg) 65 | { 66 | INFO("MotionController get SequenceCmd data %s", msg->data.c_str()); 67 | recv_result_.result = 0; 68 | lcm_recv_pub_->publish("user_gait_result", &recv_result_); 69 | INFO("publish result: %d", recv_result_.result); 70 | } 71 | LOGGER_MINOR_INSTANCE("SimMotionController") 72 | }; 73 | 74 | void SimMotionControllerFunction() 75 | { 76 | SimMotionController smc; 77 | smc.Run(); 78 | } 79 | 80 | int main() 81 | { 82 | LOGGER_MAIN_INSTANCE("test_as_controller") 83 | std::thread t2(SimMotionControllerFunction); 84 | 85 | if (t2.joinable()) { 86 | t2.join(); 87 | } 88 | } 89 | -------------------------------------------------------------------------------- /motion_action/test/publisher_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 20 | #include 21 | #include 22 | 23 | class SimMotionPublisher 24 | { 25 | public: 26 | SimMotionPublisher(const std::string & name) 27 | { 28 | node_ptr_ = rclcpp::Node::make_shared(name); 29 | motion_cmd_pub_ = node_ptr_->create_publisher(cyberdog::motion::kMotionServoCommandTopicName, rclcpp::SystemDefaultsQoS()); 30 | moiton_result_queue_pub_ = node_ptr_->create_publisher(cyberdog::motion::kMotionQueueCommandTopicName, rclcpp::SystemDefaultsQoS()); 31 | } 32 | 33 | void Run(char ** argv) 34 | { 35 | if (std::atoi(argv[1]) >= 1000) { 36 | std_msgs::msg::Int16 msg; 37 | msg.data = std::atoi(argv[1]); 38 | moiton_result_queue_pub_->publish(msg); 39 | INFO("MotionPublisher publish queue cmd: %d", msg.data); 40 | } else { 41 | cyberdog::motion::MotionServoCmdMsg::SharedPtr msg(new cyberdog::motion::MotionServoCmdMsg); 42 | std::string cmd_preset = ament_index_cpp::get_package_share_directory("motion_action") + "/preset/" + argv[1] + ".toml"; 43 | toml::value value; 44 | if (!cyberdog::common::CyberdogToml::ParseFile(cmd_preset, value)) { 45 | FATAL("Cannot parse %s", cmd_preset.c_str()); 46 | exit(-1); 47 | } 48 | // GET_TOML_VALUE(value, "motion_id", msg->motion_id); 49 | msg->motion_id = std::atoi(argv[1]); 50 | GET_TOML_VALUE(value, "cmd_type", msg->cmd_type); 51 | GET_TOML_VALUE(value, "vel_des", msg->vel_des); 52 | GET_TOML_VALUE(value, "rpy_des", msg->rpy_des); 53 | GET_TOML_VALUE(value, "pos_des", msg->pos_des); 54 | GET_TOML_VALUE(value, "acc_des", msg->acc_des); 55 | GET_TOML_VALUE(value, "ctrl_point", msg->ctrl_point); 56 | GET_TOML_VALUE(value, "foot_pose", msg->foot_pose); 57 | GET_TOML_VALUE(value, "step_height", msg->step_height); 58 | // HandleTestCmd(msg); 59 | motion_cmd_pub_->publish(*msg); 60 | INFO( 61 | "MotionPublisher publish cmd:\n motion_id: %d\n cmd_type: %d\n vel_des: [%.2f, %.2f, %.2f]\n rpy_des: [%.2f, %.2f, %.2f]\n pos_des: [%.2f, %.2f, %.2f]\n acc_des: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]\n ctrl_point: [%.2f, %.2f, %.2f]\n foot_pose: [%.2f, %.2f, %.2f, %.2f, %.2f, %.2f]\n step_height: [%.2f, %.2f]\n", msg->motion_id, msg->cmd_type, 62 | msg->vel_des[0], msg->vel_des[1], msg->vel_des[2], msg->rpy_des[0], msg->rpy_des[1], 63 | msg->rpy_des[2], msg->pos_des[0], msg->pos_des[1], msg->pos_des[2], msg->acc_des[0], 64 | msg->acc_des[1], msg->acc_des[2], msg->acc_des[3], msg->acc_des[4], msg->acc_des[5], 65 | msg->ctrl_point[0], msg->ctrl_point[1], msg->ctrl_point[2], msg->foot_pose[0], 66 | msg->foot_pose[1], msg->foot_pose[2], msg->foot_pose[3], msg->foot_pose[4], 67 | msg->foot_pose[5], msg->step_height[0], msg->step_height[1]); 68 | } 69 | } 70 | 71 | void Spin() 72 | { 73 | // executor_->spin_once(); 74 | rclcpp::shutdown(); 75 | } 76 | 77 | private: 78 | rclcpp::Node::SharedPtr node_ptr_; 79 | rclcpp::Publisher::SharedPtr motion_cmd_pub_{nullptr}; 80 | rclcpp::Publisher::SharedPtr moiton_result_queue_pub_{nullptr}; 81 | LOGGER_MINOR_INSTANCE("SimMotionPublisher"); 82 | }; 83 | 84 | int main(int argc, char ** argv) 85 | { 86 | LOGGER_MAIN_INSTANCE("test_as_publisher") 87 | if(argc < 2){ 88 | FATAL("argc less than 2"); 89 | exit(-1); 90 | } 91 | rclcpp::init(argc, argv); 92 | SimMotionPublisher smm("test_as_publisher"); 93 | smm.Run(argv); 94 | smm.Spin(); 95 | } 96 | -------------------------------------------------------------------------------- /motion_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(motion_bridge) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic -g) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(tf2_ros REQUIRED) 11 | find_package(rclcpp REQUIRED) 12 | find_package(lcm REQUIRED) 13 | find_package(protocol REQUIRED) 14 | find_package(cyberdog_common REQUIRED) 15 | find_package(motion_action REQUIRED) 16 | find_package(grid_map REQUIRED) 17 | find_package(grid_map_core REQUIRED) 18 | find_package(grid_map_ros REQUIRED) 19 | find_package(grid_map_msgs REQUIRED) 20 | find_package(sensor_msgs REQUIRED) 21 | # uncomment the following section in order to fill in 22 | # further dependencies manually. 23 | # find_package( REQUIRED) 24 | 25 | set(dependencies 26 | rclcpp 27 | tf2_ros 28 | lcm 29 | protocol 30 | cyberdog_common 31 | motion_action 32 | grid_map 33 | grid_map_core 34 | grid_map_ros 35 | grid_map_msgs 36 | sensor_msgs) 37 | 38 | include_directories(include) 39 | 40 | add_executable(elevation_bridge 41 | src/elevation_bridge.cpp 42 | ) 43 | target_link_libraries(elevation_bridge lcm) 44 | ament_target_dependencies(elevation_bridge ${dependencies}) 45 | 46 | add_executable(imu_bridge 47 | src/imu_bridge.cpp 48 | ) 49 | target_link_libraries(imu_bridge lcm) 50 | ament_target_dependencies(imu_bridge ${dependencies}) 51 | 52 | add_executable(motor_bridge 53 | src/motor_bridge.cpp 54 | ) 55 | target_link_libraries(motor_bridge lcm) 56 | ament_target_dependencies(motor_bridge ${dependencies}) 57 | 58 | add_executable(odom_out_publisher 59 | src/odom_out_publisher.cpp 60 | ) 61 | target_link_libraries(odom_out_publisher lcm) 62 | ament_target_dependencies(odom_out_publisher ${dependencies}) 63 | 64 | add_executable(file_bridge 65 | src/file_bridge.cpp 66 | ) 67 | target_link_libraries(file_bridge lcm) 68 | ament_target_dependencies(file_bridge ${dependencies}) 69 | 70 | add_executable(as_controller 71 | test/as_controller.cpp 72 | ) 73 | target_link_libraries(as_controller lcm) 74 | ament_target_dependencies(as_controller ${dependencies}) 75 | 76 | add_executable(as_controller_file 77 | test/as_controller_file.cpp 78 | ) 79 | target_link_libraries(as_controller_file lcm) 80 | ament_target_dependencies(as_controller_file ${dependencies}) 81 | 82 | if(BUILD_TESTING) 83 | find_package(ament_lint_auto REQUIRED) 84 | # the following line skips the linter which checks for copyrights 85 | # uncomment the line when a copyright and license is not present in all source files 86 | #set(ament_cmake_copyright_FOUND TRUE) 87 | # the following line skips cpplint (only works in a git repo) 88 | # uncomment the line when this package is not in a git repo 89 | #set(ament_cmake_cpplint_FOUND TRUE)xmllint 90 | ament_lint_auto_find_test_dependencies() 91 | endif() 92 | 93 | install(TARGETS 94 | elevation_bridge 95 | imu_bridge 96 | motor_bridge 97 | odom_out_publisher 98 | file_bridge 99 | as_controller 100 | as_controller_file 101 | DESTINATION lib/${PROJECT_NAME} 102 | ) 103 | ament_export_dependencies(${dependencies}) 104 | ament_package() 105 | -------------------------------------------------------------------------------- /motion_bridge/include/motion_bridge/elevation_bridge.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 MOTION_BRIDGE__ELEVATION_BRIDGE_HPP_ 15 | #define MOTION_BRIDGE__ELEVATION_BRIDGE_HPP_ 16 | 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 "motion_action/motion_macros.hpp" 30 | 31 | namespace cyberdog 32 | { 33 | namespace motion 34 | { 35 | constexpr int8_t SIZE_X = 80; 36 | constexpr int8_t SIZE_Y = 80; 37 | constexpr float RESOLUTION = 0.02; 38 | constexpr const char * LAYER_ELEVATION = "elevation"; 39 | constexpr const char * LAYER_TRAVERSABILITY = " "; 40 | class ElevationBridge 41 | { 42 | public: 43 | explicit ElevationBridge(const rclcpp::Node::SharedPtr node); 44 | ~ElevationBridge() {} 45 | void Spin(); 46 | 47 | private: 48 | void GridMapCallback(const grid_map_msgs::msg::GridMap::SharedPtr msg); 49 | bool GetTransform( 50 | Eigen::Isometry3d & transform, const std::string & target_frame, 51 | const std::string & source_frame); 52 | std::shared_ptr tf2_listener_; 53 | std::shared_ptr tf2_buffer_; 54 | rclcpp::Node::SharedPtr node_; 55 | std::shared_ptr lcm_; 56 | rclcpp::Subscription::SharedPtr gridmap_sub_; 57 | heightmap_t elevation_; 58 | std::string map_frame_{"map"}, odom_frame_{"odom"}, base_frame_{"base_link"}; 59 | }; // class ElevationBridge 60 | } // namespace motion 61 | } // namespace cyberdog 62 | #endif // MOTION_BRIDGE__ELEVATION_BRIDGE_HPP_ 63 | -------------------------------------------------------------------------------- /motion_bridge/include/motion_bridge/file_bridge.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 MOTION_BRIDGE__FILE_BRIDGE_HPP_ 15 | #define MOTION_BRIDGE__FILE_BRIDGE_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include "motion_action/motion_macros.hpp" 25 | 26 | namespace cyberdog 27 | { 28 | namespace motion 29 | { 30 | class FileBridge 31 | { 32 | public: 33 | explicit FileBridge(const rclcpp::Node::SharedPtr node); 34 | ~FileBridge() {} 35 | void Spin(); 36 | 37 | private: 38 | void HandleFileBridgeCallback(); 39 | rclcpp::Node::SharedPtr node_; 40 | std::shared_ptr lcm_; 41 | file_send_lcmt file_; 42 | }; // class FileBridge 43 | } // namespace motion 44 | } // namespace cyberdog 45 | #endif // MOTION_BRIDGE__FILE_BRIDGE_HPP_ 46 | -------------------------------------------------------------------------------- /motion_bridge/include/motion_bridge/imu_bridge.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 MOTION_BRIDGE__IMU_BRIDGE_HPP_ 15 | #define MOTION_BRIDGE__IMU_BRIDGE_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "motion_action/motion_macros.hpp" 24 | #include "protocol/lcm/microstrain_lcmt.hpp" 25 | #include "protocol/lcm/robot_control_response_lcmt.hpp" 26 | #include "sensor_msgs/msg/imu.hpp" 27 | namespace cyberdog 28 | { 29 | namespace motion 30 | { 31 | constexpr int8_t SIZE_X = 80; 32 | constexpr int8_t SIZE_Y = 80; 33 | constexpr float RESOLUTION = 0.05; 34 | constexpr const char * LAYER_ELEVATION = "elevation"; 35 | constexpr const char * LAYER_TRAVERSABILITY = " "; 36 | class ImuBridge 37 | { 38 | public: 39 | explicit ImuBridge(const rclcpp::Node::SharedPtr node); 40 | ~ImuBridge() {} 41 | void Spin(); 42 | 43 | private: 44 | void ReadLcm( 45 | const lcm::ReceiveBuffer *, const std::string &, 46 | const microstrain_lcmt * msg); 47 | rclcpp::Node::SharedPtr node_; 48 | std::shared_ptr lcm_subscribe_instance_; 49 | rclcpp::Publisher::SharedPtr imu_pub_ {nullptr}; 50 | sensor_msgs::msg::Imu::SharedPtr imu_ros_data_ {nullptr}; 51 | microstrain_lcmt imu_lcm_data_; 52 | robot_control_response_lcmt response_lcm_data_; 53 | std::thread topic_publish_thread_, lcm_handle_thread_; 54 | std::string imu_frame_{"imu"}; 55 | }; // class ImuBridge 56 | } // namespace motion 57 | } // namespace cyberdog 58 | #endif // MOTION_BRIDGE__IMU_BRIDGE_HPP_ 59 | -------------------------------------------------------------------------------- /motion_bridge/include/motion_bridge/motor_bridge.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 MOTION_BRIDGE__MOTOR_BRIDGE_HPP_ 15 | #define MOTION_BRIDGE__MOTOR_BRIDGE_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "motion_action/motion_macros.hpp" 23 | #include "protocol/lcm/danger_states_lcmt.hpp" 24 | #include "protocol/srv/motor_temp.hpp" 25 | namespace cyberdog 26 | { 27 | namespace motion 28 | { 29 | class MotorBridge 30 | { 31 | public: 32 | explicit MotorBridge(const rclcpp::Node::SharedPtr node); 33 | ~MotorBridge() {} 34 | void Spin(); 35 | 36 | private: 37 | void ReadLcm( 38 | const lcm::ReceiveBuffer *, const std::string &, 39 | const danger_states_lcmt * msg); 40 | void HandleMotorTempCallback( 41 | const protocol::srv::MotorTemp_Request::SharedPtr req, 42 | protocol::srv::MotorTemp_Response::SharedPtr res); 43 | rclcpp::Node::SharedPtr node_; 44 | rclcpp::Service::SharedPtr motor_temp_srv_ {nullptr}; 45 | std::shared_ptr lcm_subscribe_instance_; 46 | danger_states_lcmt motor_temp_; 47 | std::thread lcm_handle_thread_; 48 | }; // class MotorBridge 49 | } // namespace motion 50 | } // namespace cyberdog 51 | #endif // MOTION_BRIDGE__MOTOR_BRIDGE_HPP_ 52 | -------------------------------------------------------------------------------- /motion_bridge/include/motion_bridge/odom_out_publisher.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 MOTION_BRIDGE__ODOM_OUT_PUBLISHER_HPP_ 15 | #define MOTION_BRIDGE__ODOM_OUT_PUBLISHER_HPP_ 16 | 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 | #include "motion_action/motion_macros.hpp" 31 | 32 | namespace cyberdog 33 | { 34 | namespace motion 35 | { 36 | 37 | class OdomOutPublisher 38 | { 39 | LOGGER_MINOR_INSTANCE("OdomOutPublisher"); 40 | 41 | public: 42 | explicit OdomOutPublisher(const rclcpp::Node::SharedPtr node); 43 | ~OdomOutPublisher() {} 44 | bool Init(); 45 | void Spin(); 46 | 47 | private: 48 | void OdomLCMCabllback( 49 | const lcm::ReceiveBuffer *, const std::string &, 50 | const localization_lcmt * msg); 51 | std::shared_ptr tf2_broadcaster_; 52 | rclcpp::Node::SharedPtr node_; 53 | std::shared_ptr lcm_; 54 | rclcpp::Publisher::SharedPtr leg_odom_publisher_; 55 | nav_msgs::msg::Odometry odom_; 56 | geometry_msgs::msg::TransformStamped transform_stamped_; 57 | std::string map_frame_{"map"}, odom_frame_{"odom"}, base_frame_{"base_link_leg"}; 58 | std::condition_variable cv_; 59 | std::mutex mutex_; 60 | bool ready_publish_; 61 | bool tf_pub_; 62 | }; // class OdomOutPublisher 63 | } // namespace motion 64 | } // namespace cyberdog 65 | #endif // MOTION_BRIDGE__ODOM_OUT_PUBLISHER_HPP_ 66 | -------------------------------------------------------------------------------- /motion_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | motion_bridge 5 | 0.0.0 6 | TODO: Package description 7 | harvey 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | lcm 14 | cyberdog_common 15 | motion_action 16 | tf2_ros 17 | grid_map 18 | grid_map_core 19 | grid_map_ros 20 | nav2_msgs 21 | sensor_msgs 22 | 23 | 24 | ament_lint_auto 25 | ament_lint_common 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /motion_bridge/src/elevation_bridge.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 "motion_bridge/elevation_bridge.hpp" 18 | 19 | namespace cyberdog 20 | { 21 | namespace motion 22 | { 23 | ElevationBridge::ElevationBridge(const rclcpp::Node::SharedPtr node) 24 | { 25 | node_ = node; 26 | gridmap_sub_ = node_->create_subscription( 27 | "elevation_map_raw", 28 | rclcpp::SystemDefaultsQoS(), 29 | std::bind(&ElevationBridge::GridMapCallback, this, std::placeholders::_1)); 30 | lcm_ = std::make_shared(kLCMBirdgeSubscribeURL); 31 | tf2_buffer_ = std::make_shared(node_->get_clock()); 32 | auto timer_interface = std::make_shared( 33 | node_->get_node_base_interface(), 34 | node_->get_node_timers_interface()); 35 | tf2_buffer_->setCreateTimerInterface(timer_interface); 36 | tf2_listener_ = std::make_shared( 37 | *tf2_buffer_, 38 | std::make_shared(std::string(node_->get_name()) + "_tf_listener")); 39 | std::thread{[this]() { 40 | while (lcm_->good()) { 41 | lcm_->publish(kLCMBridgeElevationChannel, &elevation_); 42 | std::this_thread::sleep_for(std::chrono::milliseconds(50)); 43 | } 44 | } 45 | }.detach(); 46 | } 47 | 48 | void ElevationBridge::Spin() 49 | { 50 | rclcpp::spin(node_); 51 | rclcpp::shutdown(); 52 | } 53 | 54 | bool ElevationBridge::GetTransform( 55 | Eigen::Isometry3d & transform, const std::string & target_frame, 56 | const std::string & source_frame) 57 | { 58 | geometry_msgs::msg::TransformStamped transform_stamped; 59 | try { 60 | transform_stamped = 61 | tf2_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero); 62 | } catch (tf2::TransformException & e) { 63 | WARN("%s", e.what()); 64 | return false; 65 | } 66 | transform.translation()(0) = transform_stamped.transform.translation.x; 67 | transform.translation()(1) = transform_stamped.transform.translation.y; 68 | transform.translation()(2) = transform_stamped.transform.translation.z; 69 | auto q = transform_stamped.transform.rotation; 70 | Eigen::Quaterniond quat(q.x, q.y, q.z, q.w); 71 | transform.rotate(quat); 72 | return true; 73 | } 74 | 75 | void ElevationBridge::GridMapCallback(const grid_map_msgs::msg::GridMap::SharedPtr msg) 76 | { 77 | double resolution = msg->info.resolution; 78 | if (std::abs(resolution - RESOLUTION) > 0.001) { 79 | ERROR("Resolution error"); 80 | // return; 81 | } 82 | double length_x = resolution * SIZE_X; 83 | double length_y = resolution * SIZE_Y; 84 | if (msg->info.length_x < length_x || msg->info.length_y < length_y) { 85 | ERROR("Lenth error"); 86 | // return; 87 | } 88 | grid_map::GridMap map, submap, tranformed_map; 89 | if (!grid_map::GridMapRosConverter::fromMessage(*msg, map)) { 90 | ERROR("Cannot convert grid map from ros msg"); 91 | // return; 92 | } 93 | static float x_diff = 0; 94 | static float y_diff = 0; 95 | static float z_diff = 0; 96 | if (msg->header.frame_id != odom_frame_) { 97 | // trans msg to odom frame 98 | Eigen::Isometry3d transform; 99 | Eigen::Isometry3d transform1; 100 | // if (!GetTransform(transform, odom_frame_, msg->header.frame_id)) { 101 | if (!GetTransform( 102 | transform, "odom", 103 | "base_link_leg") || !GetTransform(transform1, "vodom", "base_link")) 104 | { 105 | ERROR("Cannot transform"); 106 | } else { 107 | // tranformed_map = map.getTransformedMap(transform, LAYER_ELEVATION, odom_frame_); 108 | x_diff = transform.translation().x() - transform1.translation().x(); 109 | y_diff = transform.translation().y() - transform1.translation().y(); 110 | z_diff = transform.translation().z() - transform1.translation().z(); 111 | } 112 | } 113 | double center_pos_x = map.getPosition()(0); // tranformed_map.getPosition()(0); 114 | double center_pos_y = map.getPosition()(1); // tranformed_map.getPosition()(1); 115 | elevation_.robot_loc[0] = center_pos_x + x_diff; 116 | elevation_.robot_loc[1] = center_pos_y + y_diff; 117 | 118 | /**************************************************************************/ 119 | // bool success = false; 120 | // submap = map.getSubmap(map.getPosition(), grid_map::Length(length_x, length_y), success); 121 | // if (!success) { 122 | // ERROR("Cannot get submap"); 123 | // } 124 | // INFO("map size: %d, %d", map.getSize()(0), map.getSize()(1)); 125 | // INFO("submap size: %d, %d", submap.getSize()(0), submap.getSize()(1)); 126 | // if (submap.getSize()(0) != SIZE_X || submap.getSize()(1) != SIZE_Y) { 127 | // ERROR("Submap size error"); 128 | // } 129 | // for (grid_map::GridMapIterator iter(submap); iter.isPastEnd(); ++iter) { 130 | // for (int8_t i = 0; i < SIZE_X; i++) { 131 | // for (int8_t j = 0; j < SIZE_Y; j++) { 132 | // elevation_.map[i][j] = map.at(LAYER_ELEVATION, *iter); 133 | // ++iter; 134 | // } 135 | // } 136 | // } 137 | /**************************************************************************/ 138 | double submap_left_top_x = center_pos_x + length_x / 2; 139 | double submap_left_top_y = center_pos_y + length_y / 2; 140 | grid_map::Index submap_left_top_index; 141 | if (!map.getIndex( 142 | grid_map::Position(submap_left_top_x, submap_left_top_y), 143 | submap_left_top_index)) 144 | { 145 | ERROR("Cannot get lefttop corner"); 146 | // return; 147 | } 148 | grid_map::SubmapIterator iter(map, submap_left_top_index, grid_map::Size(SIZE_X, SIZE_Y)); 149 | for (int8_t i = 0; i < SIZE_X; i++) { 150 | for (int8_t j = 0; j < SIZE_Y; j++) { 151 | elevation_.map[i][j] = map.at(LAYER_ELEVATION, *iter) + z_diff; 152 | ++iter; 153 | } 154 | } 155 | /**************************************************************************/ 156 | } 157 | } // namespace motion 158 | } // namespace cyberdog 159 | 160 | int main(int argc, char const * argv[]) 161 | { 162 | rclcpp::init(argc, argv); 163 | rclcpp::Node::SharedPtr node(new rclcpp::Node("elevation_bridge")); 164 | cyberdog::motion::ElevationBridge eb(node); 165 | eb.Spin(); 166 | return 0; 167 | } 168 | -------------------------------------------------------------------------------- /motion_bridge/src/file_bridge.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 "motion_bridge/file_bridge.hpp" 18 | 19 | namespace cyberdog 20 | { 21 | namespace motion 22 | { 23 | FileBridge::FileBridge(const rclcpp::Node::SharedPtr node) 24 | { 25 | node_ = node; 26 | lcm_ = std::make_shared(kLCMBirdgeSubscribeURL); 27 | // std::thread{[this]() { 28 | // while (lcm_->good()) { 29 | // lcm_->publish(kLCMBridgeElevationChannel, &elevation_); 30 | // std::this_thread::sleep_for(std::chrono::milliseconds(50)); 31 | // } 32 | // } 33 | // }.detach(); 34 | std::ifstream file("/home/harvey/Downloads/user_gait_01.toml"); 35 | std::string s; 36 | while (getline(file, s)) { 37 | INFO("%s", s.c_str()); 38 | file_.data += s + "\n"; 39 | } 40 | lcm_->publish(kLCMBridgeFileChannel, &file_); 41 | } 42 | 43 | void FileBridge::Spin() 44 | { 45 | } 46 | 47 | 48 | void FileBridge::HandleFileBridgeCallback() 49 | { 50 | // std::ifstream file(""); 51 | // std::string s; 52 | // while(file >> s) { 53 | // INFO("%s", s.c_str()); 54 | // } 55 | } 56 | } // namespace motion 57 | } // namespace cyberdog 58 | 59 | int main(int argc, char const * argv[]) 60 | { 61 | rclcpp::init(argc, argv); 62 | rclcpp::Node::SharedPtr node(new rclcpp::Node("elevation_bridge")); 63 | cyberdog::motion::FileBridge fb(node); 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /motion_bridge/src/imu_bridge.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 "motion_bridge/imu_bridge.hpp" 18 | 19 | namespace cyberdog 20 | { 21 | namespace motion 22 | { 23 | ImuBridge::ImuBridge(const rclcpp::Node::SharedPtr node) 24 | { 25 | node_ = node; 26 | imu_pub_ = node->create_publisher("imu", rclcpp::SystemDefaultsQoS()); 27 | lcm_subscribe_instance_ = std::make_shared(kLCMBirdgeSubscribeURL); 28 | lcm_subscribe_instance_->subscribe(kLCMBridgeImuChannel, &ImuBridge::ReadLcm, this); 29 | imu_ros_data_.reset(new sensor_msgs::msg::Imu); 30 | 31 | lcm_handle_thread_ = 32 | std::thread( 33 | [this]() { 34 | while (rclcpp::ok()) { 35 | while (0 == this->lcm_subscribe_instance_->handleTimeout(kAcitonLcmReadTimeout)) { 36 | ERROR("Cannot read LCM from MR813"); 37 | } 38 | } 39 | }); 40 | lcm_handle_thread_.detach(); 41 | } 42 | 43 | void ImuBridge::Spin() 44 | { 45 | rclcpp::spin(node_); 46 | rclcpp::shutdown(); 47 | } 48 | 49 | void ImuBridge::ReadLcm( 50 | const lcm::ReceiveBuffer *, const std::string &, 51 | const microstrain_lcmt * msg) 52 | { 53 | imu_ros_data_->header.frame_id = imu_frame_; 54 | imu_ros_data_->header.stamp = node_->get_clock()->now(); 55 | imu_ros_data_->linear_acceleration.x = msg->acc[0]; 56 | imu_ros_data_->linear_acceleration.y = msg->acc[1]; 57 | imu_ros_data_->linear_acceleration.z = msg->acc[2]; 58 | imu_ros_data_->angular_velocity.x = msg->omega[0]; 59 | imu_ros_data_->angular_velocity.y = msg->omega[1]; 60 | imu_ros_data_->angular_velocity.z = msg->omega[2]; 61 | imu_ros_data_->orientation.x = msg->quat[0]; 62 | imu_ros_data_->orientation.y = msg->quat[1]; 63 | imu_ros_data_->orientation.z = msg->quat[2]; 64 | imu_ros_data_->orientation.w = msg->quat[3]; 65 | imu_pub_->publish(*imu_ros_data_); 66 | } 67 | } // namespace motion 68 | } // namespace cyberdog 69 | 70 | int main(int argc, char const * argv[]) 71 | { 72 | rclcpp::init(argc, argv); 73 | rclcpp::Node::SharedPtr node(new rclcpp::Node("test_imu_bridege")); 74 | cyberdog::motion::ImuBridge ib(node); 75 | ib.Spin(); 76 | return 0; 77 | } 78 | -------------------------------------------------------------------------------- /motion_bridge/src/motor_bridge.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 "motion_bridge/motor_bridge.hpp" 19 | 20 | namespace cyberdog 21 | { 22 | namespace motion 23 | { 24 | MotorBridge::MotorBridge(const rclcpp::Node::SharedPtr node) 25 | { 26 | node_ = node; 27 | motor_temp_srv_ = node_->create_service( 28 | "motor_temp", 29 | std::bind( 30 | &MotorBridge::HandleMotorTempCallback, 31 | this, std::placeholders::_1, std::placeholders::_2)); 32 | lcm_subscribe_instance_ = std::make_shared(kLCMBirdgeSubscribeURL); 33 | lcm_subscribe_instance_->subscribe(kLCMBridgeMotorChannel, &MotorBridge::ReadLcm, this); 34 | lcm_handle_thread_ = 35 | std::thread( 36 | [this]() { 37 | while (rclcpp::ok()) { 38 | while (0 == this->lcm_subscribe_instance_->handleTimeout(5000)) { 39 | ERROR("Cannot read motor temp LCM from MR813"); 40 | } 41 | } 42 | }); 43 | lcm_handle_thread_.detach(); 44 | } 45 | 46 | void MotorBridge::Spin() 47 | { 48 | rclcpp::spin(node_); 49 | rclcpp::shutdown(); 50 | } 51 | 52 | void MotorBridge::ReadLcm( 53 | const lcm::ReceiveBuffer *, const std::string &, 54 | const danger_states_lcmt * msg) 55 | { 56 | motor_temp_ = *msg; 57 | } 58 | 59 | void MotorBridge::HandleMotorTempCallback( 60 | const protocol::srv::MotorTemp_Request::SharedPtr, 61 | protocol::srv::MotorTemp_Response::SharedPtr res) 62 | { 63 | res->result = true; 64 | res->motor_temp = std::vector( 65 | std::begin(motor_temp_.motor_temperature), 66 | std::end(motor_temp_.motor_temperature)); 67 | } 68 | 69 | } // namespace motion 70 | } // namespace cyberdog 71 | 72 | int main(int argc, char const * argv[]) 73 | { 74 | rclcpp::init(argc, argv); 75 | rclcpp::Node::SharedPtr node(new rclcpp::Node("motor_temp")); 76 | cyberdog::motion::MotorBridge mb(node); 77 | mb.Spin(); 78 | return 0; 79 | } 80 | -------------------------------------------------------------------------------- /motion_bridge/src/odom_out_publisher.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 "motion_bridge/odom_out_publisher.hpp" 18 | 19 | namespace cyberdog 20 | { 21 | namespace motion 22 | { 23 | OdomOutPublisher::OdomOutPublisher(const rclcpp::Node::SharedPtr node) 24 | { 25 | node_ = node; 26 | } 27 | 28 | bool OdomOutPublisher::Init() 29 | { 30 | odom_frame_ = node_->declare_parameter("odom_frame", std::string("odom")); 31 | base_frame_ = node_->declare_parameter("base_frame", std::string("base_link_leg")); 32 | map_frame_ = node_->declare_parameter("map_frame", std::string("map")); 33 | tf_pub_ = node_->declare_parameter("tf_pub", true); 34 | leg_odom_publisher_ = node_->create_publisher( 35 | kBridgeOdomTopicName, 36 | rclcpp::SystemDefaultsQoS()); 37 | tf2_broadcaster_ = std::make_shared(node_); 38 | lcm_ = std::make_shared(kLCMBirdgeSubscribeURL); 39 | if (!lcm_->good()) { 40 | ERROR("LegOdom reading lcm initialized error"); 41 | return false; 42 | } 43 | lcm_->subscribe(kLCMBridgeOdomChannel, &OdomOutPublisher::OdomLCMCabllback, this); 44 | std::thread{ 45 | [this]() { 46 | while (rclcpp::ok()) { 47 | while (0 == this->lcm_->handleTimeout(1000) && rclcpp::ok()) { 48 | ERROR("Cannot read LegOdomLCM from MR813"); 49 | if (ready_publish_) { 50 | ready_publish_ = false; 51 | } 52 | } 53 | if (!ready_publish_) { 54 | ready_publish_ = true; 55 | cv_.notify_one(); 56 | } 57 | } 58 | } 59 | }.detach(); 60 | std::thread{ 61 | [this]() { 62 | while (rclcpp::ok()) { 63 | if (!ready_publish_) { 64 | std::unique_lock lk(mutex_); 65 | cv_.wait(lk); 66 | } 67 | leg_odom_publisher_->publish(odom_); 68 | if (this->tf_pub_) { 69 | tf2_broadcaster_->sendTransform(transform_stamped_); 70 | } 71 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); 72 | } 73 | } 74 | }.detach(); 75 | } 76 | 77 | void OdomOutPublisher::OdomLCMCabllback( 78 | const lcm::ReceiveBuffer *, const std::string &, 79 | const localization_lcmt * msg) 80 | { 81 | odom_.header.frame_id = transform_stamped_.header.frame_id = odom_frame_; 82 | odom_.header.stamp.sec = transform_stamped_.header.stamp.sec = 83 | msg->timestamp / 1000000000; 84 | odom_.header.stamp.nanosec = transform_stamped_.header.stamp.nanosec = 85 | msg->timestamp % 1000000000; 86 | odom_.child_frame_id = transform_stamped_.child_frame_id = base_frame_; 87 | odom_.pose.pose.position.x = transform_stamped_.transform.translation.x = msg->xyz[0]; 88 | odom_.pose.pose.position.y = transform_stamped_.transform.translation.y = msg->xyz[1]; 89 | odom_.pose.pose.position.z = transform_stamped_.transform.translation.z = msg->xyz[2]; 90 | tf2::Quaternion q; 91 | q.setRPY(msg->rpy[0], msg->rpy[1], msg->rpy[2]); 92 | odom_.pose.pose.orientation.x = transform_stamped_.transform.rotation.x = q.getX(); 93 | odom_.pose.pose.orientation.y = transform_stamped_.transform.rotation.y = q.getY(); 94 | odom_.pose.pose.orientation.z = transform_stamped_.transform.rotation.z = q.getZ(); 95 | odom_.pose.pose.orientation.w = transform_stamped_.transform.rotation.w = q.getW(); 96 | odom_.twist.twist.linear.x = msg->vBody[0]; 97 | odom_.twist.twist.linear.y = msg->vBody[1]; 98 | odom_.twist.twist.linear.z = msg->vBody[2]; 99 | odom_.twist.twist.angular.x = msg->omegaBody[0]; 100 | odom_.twist.twist.angular.y = msg->omegaBody[1]; 101 | odom_.twist.twist.angular.z = msg->omegaBody[2]; 102 | } 103 | 104 | void OdomOutPublisher::Spin() 105 | { 106 | rclcpp::spin(node_); 107 | rclcpp::shutdown(); 108 | } 109 | 110 | } // namespace motion 111 | } // namespace cyberdog 112 | 113 | int main(int argc, char const * argv[]) 114 | { 115 | rclcpp::init(argc, argv); 116 | rclcpp::Node::SharedPtr node(new rclcpp::Node("leg_odom_publisher")); 117 | cyberdog::motion::OdomOutPublisher lop(node); 118 | lop.Init(); 119 | lop.Spin(); 120 | return 0; 121 | } 122 | -------------------------------------------------------------------------------- /motion_bridge/test/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/motion/4d241d1cfca1610f3658bac31acea01277b7a47c/motion_bridge/test/AMENT_IGNORE -------------------------------------------------------------------------------- /motion_bridge/test/as_controller.cpp: -------------------------------------------------------------------------------- 1 | #include "protocol/lcm/heightmap_t.hpp" 2 | #include "lcm/lcm-cpp.hpp" 3 | #include "rclcpp/rclcpp.hpp" 4 | #include "grid_map_core/grid_map_core.hpp" 5 | #include "grid_map_msgs/msg/grid_map.hpp" 6 | #include "grid_map_ros/grid_map_ros.hpp" 7 | #include "cyberdog_common/cyberdog_log.hpp" 8 | #include "motion_bridge/elevation_bridge.hpp" 9 | namespace cyberdog 10 | { 11 | namespace motion 12 | { 13 | class AsController 14 | { 15 | public: 16 | AsController() 17 | { 18 | node_.reset(new rclcpp::Node("as_controller")); 19 | lcm_.reset(new lcm::LCM(kLCMBirdgeSubscribeURL)); 20 | pub_ = node_->create_publisher( 21 | "elevation_submap", 22 | rclcpp::SystemDefaultsQoS()); 23 | lcm_->subscribe("local_heightmap", &AsController::LCMCallback, this); 24 | std::thread{[this]() { 25 | // while (0 == lcm_->handle()) {}} 26 | while (rclcpp::ok()) { 27 | while (0 == this->lcm_->handleTimeout(1000)) { 28 | ERROR("Cannot read LCM from bridge"); 29 | } 30 | } 31 | }}.detach(); 32 | length_x_ = SIZE_X * RESOLUTION; 33 | length_y_ = SIZE_Y * RESOLUTION; 34 | } 35 | ~AsController() {} 36 | void Spin() 37 | { 38 | rclcpp::spin(node_); 39 | rclcpp::shutdown(); 40 | } 41 | 42 | private: 43 | void LCMCallback(const lcm::ReceiveBuffer *, const std::string &, const heightmap_t * msg) 44 | { 45 | grid_map::GridMap map; 46 | map.setFrameId("map"); 47 | map.add(LAYER_ELEVATION); 48 | grid_map::Position center(msg->robot_loc[0], msg->robot_loc[1]); 49 | map.setGeometry(grid_map::Length(length_x_, length_y_), RESOLUTION, center); 50 | grid_map::GridMapIterator iter(map); // NOTE 迭代器从左上角开始向下遍历，不同于SubmapIterator向右开始遍历。？ 51 | for (int8_t j = 0; j < SIZE_Y; ++j) { 52 | for (int8_t i = 0; i < SIZE_X; ++i) { 53 | map.at(LAYER_ELEVATION, *iter) = msg->map[i][j]; 54 | ++iter; 55 | } 56 | } 57 | std::unique_ptr map_ros; 58 | map_ros = grid_map::GridMapRosConverter::toMessage(map); 59 | pub_->publish(*map_ros); 60 | } 61 | std::shared_ptr lcm_; 62 | rclcpp::Node::SharedPtr node_; 63 | rclcpp::Publisher::SharedPtr pub_; 64 | double length_x_, length_y_; 65 | }; 66 | } 67 | } 68 | 69 | int main(int argc, char ** argv) 70 | { 71 | rclcpp::init(argc, argv); 72 | LOGGER_MAIN_INSTANCE("AsController"); 73 | std::shared_ptr ac = 74 | std::make_shared(); 75 | ac->Spin(); 76 | } 77 | -------------------------------------------------------------------------------- /motion_bridge/test/as_controller_file.cpp: -------------------------------------------------------------------------------- 1 | #include "protocol/lcm/file_send_lcmt.hpp" 2 | #include "lcm/lcm-cpp.hpp" 3 | #include "rclcpp/rclcpp.hpp" 4 | #include "cyberdog_common/cyberdog_log.hpp" 5 | #include "motion_bridge/file_bridge.hpp" 6 | namespace cyberdog 7 | { 8 | namespace motion 9 | { 10 | class AsController 11 | { 12 | public: 13 | AsController() 14 | { 15 | node_.reset(new rclcpp::Node("as_controller")); 16 | lcm_.reset(new lcm::LCM(kLCMBirdgeSubscribeURL)); 17 | lcm_->subscribe(kLCMBridgeFileChannel, &AsController::LCMCallback, this); 18 | std::thread{[this]() { 19 | while (rclcpp::ok()) { 20 | while (0 == this->lcm_->handleTimeout(1000)) { 21 | ERROR("Cannot read LCM from bridge"); 22 | } 23 | } 24 | } 25 | }.detach(); 26 | 27 | } 28 | ~AsController() {file_.close();} 29 | void Spin() 30 | { 31 | rclcpp::spin(node_); 32 | rclcpp::shutdown(); 33 | } 34 | 35 | private: 36 | void LCMCallback(const lcm::ReceiveBuffer *, const std::string &, const file_send_lcmt * msg) 37 | { 38 | INFO("%s", msg->data.c_str()); 39 | // std::ofstream file("/home/harvey/test.toml"); 40 | if(file_.is_open()) { 41 | file_ << msg->data; 42 | } 43 | // file_ << "\n"; 44 | } 45 | std::shared_ptr lcm_; 46 | rclcpp::Node::SharedPtr node_; 47 | std::ofstream file_{"/home/harvey/test.toml"}; 48 | 49 | }; 50 | } 51 | } 52 | 53 | int main(int argc, char ** argv) 54 | { 55 | rclcpp::init(argc, argv); 56 | LOGGER_MAIN_INSTANCE("AsController"); 57 | std::shared_ptr ac = 58 | std::make_shared(); 59 | ac->Spin(); 60 | } 61 | -------------------------------------------------------------------------------- /motion_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(motion_manager) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic -g) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(manager_base REQUIRED) 11 | find_package(cyberdog_common REQUIRED) 12 | find_package(cyberdog_system REQUIRED) 13 | find_package(protocol REQUIRED) 14 | find_package(motion_action REQUIRED) 15 | find_package(cyberdog_debug REQUIRED) 16 | find_package(cyberdog_machine REQUIRED) 17 | find_package(bes_transmit REQUIRED) 18 | 19 | include_directories(include) 20 | 21 | add_executable(${PROJECT_NAME} 22 | src/motion_handler.cpp 23 | src/motion_decision.cpp 24 | src/motion_manager.cpp 25 | src/main.cpp) 26 | 27 | ament_target_dependencies(${PROJECT_NAME} 28 | rclcpp 29 | protocol 30 | manager_base 31 | cyberdog_system 32 | cyberdog_common 33 | motion_action 34 | protocol 35 | bes_transmit 36 | ) 37 | 38 | install(TARGETS 39 | ${PROJECT_NAME} 40 | DESTINATION lib/${PROJECT_NAME}) 41 | 42 | install(DIRECTORY 43 | config 44 | DESTINATION share/${PROJECT_NAME}) 45 | 46 | if(BUILD_TESTING) 47 | find_package(ament_lint_auto REQUIRED) 48 | ament_lint_auto_find_test_dependencies() 49 | endif() 50 | 51 | ament_package() 52 | -------------------------------------------------------------------------------- /motion_manager/config/priority.toml: -------------------------------------------------------------------------------- 1 | App = 0 # App 2 | Audio = 1 # Audio 3 | Vis = 2 # Vis 4 | BluTele = 3 # BluTele 5 | Algo = 4 # Algo -------------------------------------------------------------------------------- /motion_manager/include/motion_manager/motion_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 MOTION_MANAGER__MOTION_MANAGER_HPP_ 15 | #define MOTION_MANAGER__MOTION_MANAGER_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include "pluginlib/class_loader.hpp" 23 | #include "protocol/msg/motion_servo_cmd.hpp" 24 | #include "protocol/msg/motion_servo_response.hpp" 25 | #include "protocol/srv/motion_result_cmd.hpp" 26 | #include "protocol/srv/audio_text_play.hpp" 27 | #include "protocol/lcm/robot_control_cmd_lcmt.hpp" 28 | #include "protocol/lcm/robot_control_response_lcmt.hpp" 29 | #include "manager_base/manager_base.hpp" 30 | #include "motion_manager/motion_decision.hpp" 31 | #include "motion_action/motion_action.hpp" 32 | #include "cyberdog_common/cyberdog_log.hpp" 33 | #include "cyberdog_machine/cyberdog_fs_machine.hpp" 34 | #include "cyberdog_machine/cyberdog_heartbeats.hpp" 35 | 36 | namespace cyberdog 37 | { 38 | namespace motion 39 | { 40 | 41 | class MotionManager final : public machine::MachineActuator 42 | { 43 | public: 44 | explicit MotionManager(const std::string & name); 45 | ~MotionManager(); 46 | bool Init(); 47 | void Run(); 48 | 49 | private: 50 | int32_t OnSetUp(); 51 | int32_t OnTearDown(); 52 | int32_t OnSelfCheck(); 53 | int32_t OnActive(); 54 | int32_t OnDeActive(); 55 | int32_t OnProtected(); 56 | int32_t OnLowPower(); 57 | int32_t OnOTA(); 58 | int32_t OnError(); 59 | void SetState(const MotionMgrState & state) 60 | { 61 | std::unique_lock lk(status_mutex_); 62 | state_ = state; 63 | decision_ptr_->SetState(state_); 64 | } 65 | MotionMgrState & 66 | GetState() 67 | { 68 | std::unique_lock lk(status_mutex_); 69 | return state_; 70 | } 71 | /** 72 | * @brief 73 | * 74 | * @param code 75 | * @param protected_cmd 除行走、站立、趴下、急停以外的所有动作 76 | * @return true 77 | * @return false 78 | */ 79 | bool IsStateValid(int32_t & code, bool protected_cmd = true); 80 | void MotionServoCmdCallback(const MotionServoCmdMsg::SharedPtr msg); 81 | void MotionResultCmdCallback( 82 | const MotionResultSrv::Request::SharedPtr request, 83 | MotionResultSrv::Response::SharedPtr response); 84 | void MotionCustomCmdCallback( 85 | const MotionCustomSrv::Request::SharedPtr request, 86 | MotionCustomSrv::Response::SharedPtr response); 87 | void MotionQueueCmdCallback( 88 | const MotionQueueCustomSrv::Request::SharedPtr request, 89 | MotionQueueCustomSrv::Response::SharedPtr response); 90 | void MotionSequenceShowCmdCallback( 91 | const MotionSequenceShowSrv::Request::SharedPtr request, 92 | MotionSequenceShowSrv::Response::SharedPtr response); 93 | void OnlineAudioPlay(const std::string & text) 94 | { 95 | static bool playing = false; 96 | if (playing) { 97 | return; 98 | } 99 | auto request = std::make_shared(); 100 | request->is_online = true; 101 | request->module_name = "Motion"; 102 | request->text = text; 103 | playing = true; 104 | auto callback = [](rclcpp::Client::SharedFuture future) { 105 | playing = false; 106 | INFO("Audio play result: %s", future.get()->status == 0 ? "success" : "failed"); 107 | }; 108 | auto future = audio_client_->async_send_request(request, callback); 109 | if (future.wait_for(std::chrono::milliseconds(2000)) == std::future_status::timeout) { 110 | playing = false; 111 | ERROR("Cannot get response from AudioPlay"); 112 | } 113 | } 114 | bool TryGetDownOnFsm(bool passive_getdown = false) 115 | { 116 | auto request = std::make_shared(); 117 | request->motion_id = passive_getdown ? 102 : MotionIDMsg::GETDOWN; 118 | request->cmd_source = MotionResultSrv::Request::FSM; 119 | auto response = std::make_shared(); 120 | decision_ptr_->DecideResultCmd(request, response); 121 | if (response->code != code_ptr_->GetKeyCode(system::KeyCode::kOK)) { 122 | return false; 123 | } 124 | return true; 125 | } 126 | void Report(); 127 | 128 | private: 129 | std::string name_; 130 | std::shared_ptr decision_ptr_ {nullptr}; 131 | rclcpp::Publisher::SharedPtr servo_response_pub_; 132 | rclcpp::Subscription::SharedPtr motion_servo_sub_ {nullptr}; 133 | rclcpp::Service::SharedPtr motion_result_srv_ {nullptr}; 134 | rclcpp::Service::SharedPtr motion_custom_srv_ {nullptr}; 135 | rclcpp::Service::SharedPtr motion_queue_srv_ {nullptr}; 136 | rclcpp::Service::SharedPtr motion_sequence_srv_ {nullptr}; 137 | rclcpp::Service::SharedPtr motion_sequence_show_srv_ {nullptr}; 138 | rclcpp::Client::SharedPtr audio_client_{nullptr}; 139 | rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_{nullptr}; 140 | rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; 141 | rclcpp::Node::SharedPtr node_ptr_ {nullptr}; 142 | std::shared_ptr code_ptr_{nullptr}; 143 | std::unique_ptr heart_beats_ptr_{nullptr}; 144 | MotionMgrState state_{MotionMgrState::kUninit}; 145 | std::mutex status_mutex_; 146 | std::unordered_map status_map_; 147 | std::unique_ptr thread_{nullptr}; 148 | std::condition_variable msg_condition_; 149 | std::mutex msg_mutex_; 150 | int32_t code_; 151 | int32_t motion_id_; 152 | bool isreporting_{false}; 153 | }; // class MotionManager 154 | } // namespace motion 155 | } // namespace cyberdog 156 | 157 | #endif // MOTION_MANAGER__MOTION_MANAGER_HPP_ 158 | -------------------------------------------------------------------------------- /motion_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | motion_manager 5 | 0.0.0 6 | TODO: Package description 7 | dukun 8 | Apache License, Version 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | cyberdog_system 14 | cyberdog_common 15 | cyberdog_debug 16 | manager_base 17 | motion_action 18 | protocol 19 | cyberdog_machine 20 | bes_transmit 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /motion_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 16 | #include "cyberdog_common/cyberdog_log.hpp" 17 | #include "cyberdog_debug/backtrace.hpp" 18 | #include "motion_manager/motion_manager.hpp" 19 | 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | LOGGER_MAIN_INSTANCE("MotionManager"); 24 | cyberdog::debug::register_signal(); 25 | rclcpp::init(argc, argv); 26 | 27 | auto motion_manager = 28 | std::make_shared(std::string("motion_manager")); 29 | 30 | if (!motion_manager->Init()) { 31 | ERROR("Init failed!"); 32 | return -1; 33 | } 34 | motion_manager->Run(); 35 | return 0; 36 | } 37 | -------------------------------------------------------------------------------- /motion_manager/test/gait_factory.toml: -------------------------------------------------------------------------------- 1 | [gait.a] 2 | motion_id = 0 3 | description = "null" 4 | [gait.a.pre] 5 | 6 | -------------------------------------------------------------------------------- /motion_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(motion_utils) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic -g) 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(ament_cmake_python REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(rclpy REQUIRED) 21 | find_package(protocol REQUIRED) 22 | find_package(cyberdog_system REQUIRED) 23 | find_package(cyberdog_debug REQUIRED) 24 | find_package(nav_msgs REQUIRED) 25 | find_package(PCL EXACT 1.8.1 REQUIRED COMPONENTS filters) 26 | find_package(pcl_conversions REQUIRED) 27 | # find_package(visualization_msgs REQUIRED) 28 | find_package(cyberdog_common) 29 | find_package(motion_manager) 30 | 31 | include_directories(include ${PCL_INCLUDE_DIRS}) 32 | 33 | add_library(${PROJECT_NAME} SHARED src/motion_utils.cpp) 34 | # target_link_libraries(${PROJECT_NAME} lcm) 35 | ament_target_dependencies(${PROJECT_NAME} 36 | rclcpp 37 | protocol 38 | cyberdog_system 39 | cyberdog_common 40 | cyberdog_debug 41 | nav_msgs 42 | ) 43 | 44 | add_executable(distance_test test/distance_test.cpp) 45 | target_link_libraries(distance_test ${PROJECT_NAME}) 46 | ament_target_dependencies(distance_test lcm cyberdog_common cyberdog_debug) 47 | 48 | add_executable(duration_test test/duration_test.cpp) 49 | target_link_libraries(duration_test ${PROJECT_NAME}) 50 | ament_target_dependencies(duration_test lcm cyberdog_common cyberdog_debug) 51 | 52 | add_executable(stair_align test/stair_align.cpp test/stair_perception.cpp test/edge_perception.cpp) 53 | target_link_libraries(stair_align ${PROJECT_NAME} ${PCL_LIBRARIES}) 54 | ament_target_dependencies(stair_align cyberdog_common cyberdog_debug pcl_conversions) 55 | 56 | add_executable(stair_perception_test test/stair_perception.cpp test/stair_perception_test.cpp) 57 | target_link_libraries(stair_perception_test ${PROJECT_NAME} ${PCL_LIBRARIES}) 58 | ament_target_dependencies(stair_perception_test cyberdog_common cyberdog_debug pcl_conversions) 59 | 60 | add_executable(edge_align test/edge_align.cpp test/edge_perception.cpp) 61 | target_link_libraries(edge_align ${PROJECT_NAME} ${PCL_LIBRARIES}) 62 | ament_target_dependencies(edge_align cyberdog_common cyberdog_debug pcl_conversions) 63 | 64 | add_executable(edge_perception_test test/edge_perception.cpp test/edge_perception_test.cpp) 65 | target_link_libraries(edge_perception_test ${PROJECT_NAME} ${PCL_LIBRARIES}) 66 | ament_target_dependencies(edge_perception_test cyberdog_common cyberdog_debug pcl_conversions) 67 | 68 | install( 69 | DIRECTORY include/ 70 | DESTINATION include 71 | ) 72 | 73 | install(TARGETS 74 | ${PROJECT_NAME} 75 | distance_test 76 | duration_test 77 | stair_align 78 | edge_align 79 | stair_perception_test 80 | edge_perception_test 81 | LIBRARY DESTINATION lib 82 | RUNTIME DESTINATION lib/${PROJECT_NAME} 83 | ) 84 | 85 | ament_python_install_package(${PROJECT_NAME}) 86 | 87 | install(PROGRAMS 88 | scripts/servo_delay_statistics.py 89 | scripts/head_tof_pcl_publisher.py 90 | DESTINATION lib/${PROJECT_NAME} 91 | ) 92 | 93 | install( 94 | DIRECTORY launch config 95 | DESTINATION share/${PROJECT_NAME} 96 | ) 97 | 98 | if(BUILD_TESTING) 99 | find_package(ament_lint_auto REQUIRED) 100 | ament_lint_auto_find_test_dependencies() 101 | endif() 102 | 103 | ament_export_include_directories( 104 | include 105 | ) 106 | ament_export_libraries( 107 | ${PROJECT_NAME} 108 | ) 109 | ament_export_dependencies(cyberdog_common cyberdog_debug) 110 | ament_package() 111 | -------------------------------------------------------------------------------- /motion_utils/config/edge_align.toml: -------------------------------------------------------------------------------- 1 | # Align 2 | vel_x = 0.1 # 线速度 3 | vel_omega = 0.25 # 角速度 4 | jump_after_align = false # 对齐边沿后是否执行跳跃动作 5 | auto_start = false # 是否自动启动 6 | 7 | # Perception 8 | radius = 0.055 # 离散点半径 9 | min_neighbors = 5 # 近邻点最小数量 10 | orientation_dead_zone = 2 # 角度调整时的死区 11 | orientation_correction = 0 # 角度调整时对两侧tof点数量的修正 12 | orientation_filter = false # 角度调整时两侧点的数量是否采用均值滤波 13 | filter_size = 10 # 均值滤波的步长 14 | blind_forward_threshold = 95 # 停止盲向行走时点数量的阈值 15 | 16 | threshold = 40 # 停止靠近边沿时点数量的阈值 17 | max_depth = 0.43 18 | 19 | -------------------------------------------------------------------------------- /motion_utils/config/stair_align.toml: -------------------------------------------------------------------------------- 1 | # Align 2 | vel_x = 0.1 # 线速度 3 | vel_omega = 0.25 # 角速度 4 | jump_after_align = false # 对齐台阶后是否执行跳跃动作 5 | auto_start = false # 是否自动启动 6 | is_stair_mode = true 7 | 8 | # Perception 9 | radius = 0.04 # 离散点半径 10 | min_neighbors = 5 # 近邻点最小数量 11 | orientation_dead_zone = 2 # 角度调整时的死区 12 | orientation_correction = 0 # 角度调整时对两侧tof点数量的修正 13 | orientation_filter = false # 角度调整时两侧点的数量是否采用均值滤波 14 | filter_size = 10 # 均值滤波的步长 15 | blind_forward_threshold = 25 # 停止盲向行走时点数量的阈值 16 | 17 | [[approach_threshold]] 18 | z_range = [0.0, 0.10] 19 | threshold = 100 # 停止靠近台阶时点数量的阈值 20 | 21 | [[approach_threshold]] 22 | z_range = [0.10, 0.13] 23 | threshold = 80 # 停止靠近台阶时点数量的阈值 24 | 25 | [[approach_threshold]] 26 | z_range = [0.13, 0.5] 27 | threshold = 60 # 停止靠近台阶时点数量的阈值 28 | -------------------------------------------------------------------------------- /motion_utils/include/motion_utils/edge_align.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 MOTION_UTILS__EDGE_ALIGN_HPP_ 15 | #define MOTION_UTILS__EDGE_ALIGN_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "cyberdog_common/cyberdog_log.hpp" 22 | #include "cyberdog_common/cyberdog_toml.hpp" 23 | #include "motion_utils/edge_perception.hpp" 24 | #include "motion_action/motion_macros.hpp" 25 | #include "std_srvs/srv/trigger.hpp" 26 | #include "std_msgs/msg/bool.hpp" 27 | 28 | namespace cyberdog 29 | { 30 | namespace motion 31 | { 32 | class EdgeAlign 33 | { 34 | public: 35 | explicit EdgeAlign(rclcpp::Node::SharedPtr node); 36 | void Spin() 37 | { 38 | rclcpp::spin(node_); 39 | } 40 | 41 | private: 42 | void HandleServiceCallback( 43 | const std_srvs::srv::Trigger_Request::SharedPtr request, 44 | std_srvs::srv::Trigger_Response::SharedPtr response); 45 | void Loop(); 46 | 47 | rclcpp::Node::SharedPtr node_; 48 | rclcpp::Publisher::SharedPtr servo_cmd_pub_; 49 | rclcpp::Service::SharedPtr edge_align_srv_; 50 | rclcpp::Client::SharedPtr result_cmd_client_; 51 | MotionServoCmdMsg servo_cmd_; 52 | rclcpp::Publisher::SharedPtr align_finish_pub_; 53 | std_msgs::msg::Bool align_finish_; 54 | std::shared_ptr edge_perception_; 55 | std::mutex loop_mutex_; 56 | std::condition_variable cv_; 57 | float vel_x_, vel_omega_; 58 | bool jump_after_align_, auto_start_; 59 | }; // calss EdgeAlign 60 | } // namespace motion 61 | } // namespace cyberdog 62 | #endif // MOTION_UTILS__EDGE_ALIGN_HPP_ 63 | -------------------------------------------------------------------------------- /motion_utils/include/motion_utils/edge_perception.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 MOTION_UTILS__EDGE_PERCEPTION_HPP_ 15 | #define MOTION_UTILS__EDGE_PERCEPTION_HPP_ 16 | 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 "cyberdog_common/cyberdog_log.hpp" 30 | #include "cyberdog_common/cyberdog_toml.hpp" 31 | #include "motion_action/motion_macros.hpp" 32 | namespace cyberdog 33 | { 34 | namespace motion 35 | { 36 | 37 | class EdgePerception 38 | { 39 | public: 40 | enum class State 41 | { 42 | IDLE, 43 | BLIND_FORWARD, 44 | TURN_LEFT, 45 | TURN_RIGHT, 46 | APPROACH, 47 | FINISH 48 | }; 49 | struct ApproachThreshold 50 | { 51 | std::array range; 52 | int threshold; 53 | }; 54 | 55 | public: 56 | EdgePerception(const rclcpp::Node::SharedPtr node, const toml::value & config); 57 | void Launch(bool launch) {launch_ = launch;} 58 | bool CheckLaunched() {return launch_;} 59 | const State & GetStatus() const {return state_;} 60 | inline void SetStatus(const State & state) {state_ = state;} 61 | void SetTrigger() {trigger_ = true;} 62 | const bool & GetEdgeIsDeep() const {return is_edge_deep_;} 63 | 64 | private: 65 | void HandlePointCloud(const sensor_msgs::msg::PointCloud2 & msg); 66 | inline int GetMeanDiff(int diff) 67 | { 68 | diffs_.emplace_back(diff); 69 | if (static_cast(diffs_.size()) > filter_size_) { 70 | diffs_.pop_front(); 71 | } 72 | int sum = 0; 73 | for (auto diff : diffs_) { 74 | sum += diff; 75 | } 76 | return sum / filter_size_; 77 | } 78 | 79 | pcl::RadiusOutlierRemoval ro_filter_; 80 | pcl::PassThrough pt_filter_; 81 | pcl::PointCloud::Ptr pc_raw_; 82 | pcl::PointCloud::Ptr pc_ptfiltered_; 83 | pcl::PointCloud::Ptr pc_rofiltered_; 84 | 85 | sensor_msgs::msg::PointCloud2 pc_filtered_ros_; 86 | rclcpp::Publisher::SharedPtr pc_ro_filtered_pub_; 87 | rclcpp::Node::SharedPtr node_; 88 | rclcpp::Subscription::SharedPtr pcl_sub_; 89 | State state_; 90 | bool is_edge_deep_{false}; 91 | std::deque diffs_; 92 | // std::vector approach_thresholds_; 93 | double radius_{0.05}; 94 | std::string norms_frame_{"robot"}, pc_frame_{"robot"}, base_link_frame_{"robot"}; 95 | int min_neighbors_{5}; 96 | int filter_size_ {10}; 97 | int orientation_dead_zone_{2}, orientation_correction_{0}; 98 | int blind_forward_threshold_{15}, approach_threshold_{100}; 99 | bool trigger_ {false}, orientation_filter_ {false}; 100 | double max_depth_{0.5}; 101 | bool launch_ {false}; 102 | }; // calss EdgePerception 103 | } // namespace motion 104 | } // namespace cyberdog 105 | #endif // MOTION_UTILS__EDGE_PERCEPTION_HPP_ 106 | -------------------------------------------------------------------------------- /motion_utils/include/motion_utils/motion_utils.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 MOTION_UTILS__MOTION_UTILS_HPP_ 15 | #define MOTION_UTILS__MOTION_UTILS_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 "motion_action/motion_macros.hpp" 28 | #include "protocol/msg/motion_servo_cmd.hpp" 29 | #include "protocol/srv/motion_result_cmd.hpp" 30 | #include "protocol/msg/motion_status.hpp" 31 | #include "protocol/msg/motion_servo_response.hpp" 32 | #include "protocol/lcm/robot_control_response_lcmt.hpp" 33 | #include "protocol/lcm/robot_control_cmd_lcmt.hpp" 34 | #include "cyberdog_common/cyberdog_log.hpp" 35 | #include "cyberdog_common/cyberdog_toml.hpp" 36 | #include "ament_index_cpp/get_package_share_directory.hpp" 37 | #include "ament_index_cpp/get_package_prefix.hpp" 38 | #include "nav_msgs/msg/odometry.hpp" 39 | 40 | namespace cyberdog 41 | { 42 | namespace motion 43 | { 44 | 45 | class OdomHelper 46 | { 47 | public: 48 | explicit OdomHelper(rclcpp::Node::SharedPtr node) 49 | { 50 | node_ = node; 51 | odom_sub_ = node_->create_subscription( 52 | kBridgeOdomTopicName, 53 | rclcpp::SystemDefaultsQoS(), 54 | std::bind(&OdomHelper::HandleOdomCallback, this, std::placeholders::_1)); 55 | odom_.reset(new nav_msgs::msg::Odometry); 56 | } 57 | ~OdomHelper() {} 58 | double GetDistance() 59 | { 60 | return distance_; 61 | } 62 | bool SetStartPoint() 63 | { 64 | std::unique_lock lk(odom_msg_mutex_); 65 | odom_waiting_ = true; 66 | if (cv_.wait_for(lk, std::chrono::milliseconds(1000)) == std::cv_status::timeout) { 67 | ERROR("Wait for odom timeout"); 68 | odom_waiting_ = false; 69 | return false; 70 | } 71 | odom_waiting_ = false; 72 | last_x_ = odom_->pose.pose.position.x; 73 | last_y_ = odom_->pose.pose.position.y; 74 | distance_ = 0; 75 | start_point_set_ = true; 76 | return true; 77 | } 78 | void Reset() 79 | { 80 | start_point_set_ = false; 81 | odom_waiting_ = false; 82 | } 83 | 84 | private: 85 | void HandleOdomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) 86 | { 87 | if (odom_waiting_) {cv_.notify_one();} 88 | odom_ = msg; 89 | if (!start_point_set_) {return;} 90 | auto delta_x = odom_->pose.pose.position.x - last_x_; 91 | auto delta_y = odom_->pose.pose.position.y - last_y_; 92 | distance_ += sqrt(delta_x * delta_x + delta_y * delta_y); 93 | last_x_ = odom_->pose.pose.position.x; 94 | last_y_ = odom_->pose.pose.position.y; 95 | } 96 | rclcpp::Node::SharedPtr node_; 97 | rclcpp::Subscription::SharedPtr odom_sub_; 98 | nav_msgs::msg::Odometry::SharedPtr odom_; 99 | std::mutex odom_msg_mutex_; 100 | std::condition_variable cv_; 101 | double distance_{0}; 102 | double last_x_{0}, last_y_{0}; 103 | bool start_point_set_{false}, odom_waiting_{false}; 104 | }; // class OdomHelper 105 | 106 | class MotionUtils final 107 | { 108 | public: 109 | static MotionUtils & GetMotionUtils() 110 | { 111 | static MotionUtils mu_; 112 | return mu_; 113 | } 114 | ~MotionUtils(); 115 | 116 | public: 117 | /** 118 | * @brief 控制机器人按照特定的时间和完整的伺服指令行走 119 | * 120 | * @param duration 行走时间, 单位ms 121 | * @param msg 伺服指令 122 | * @return true: 无异常, false: 异常退出 123 | */ 124 | bool ExecuteWalkDuration(int duration, const MotionServoCmdMsg::SharedPtr msg); 125 | /** 126 | * @brief 控制机器人按照特定的时间和速度, 以自变频步态(motion_id: 303)行走 127 | * 128 | * @param duration 行走时间, 单位ms 129 | * @param vel_x x方向的速度, 单位m/s 130 | * @param vel_y y方向的速度, 单位m/s 131 | * @param omega 角速度, 单位rad/s 132 | * @return true: 无异常, false: 异常退出 133 | */ 134 | bool ExecuteWalkDuration(int duration, float vel_x = 0.0, float vel_y = 0.0, float omega = 0.0); 135 | /** 136 | * @brief 控制机器人按照完整的伺服指令行走特定的距离 137 | * 138 | * @param distance 行走距离, 单位m 139 | * @param msg 伺服指令 140 | * @return true: 无异常, false: 异常退出 141 | */ 142 | bool ExecuteWalkDistance(double distance, MotionServoCmdMsg::SharedPtr msg); 143 | /** 144 | * @brief 控制机器人按照特定的速度, 以自变频步态(motion_id: 303)行走特定的距离 145 | * 146 | * @param distance 行走距离, 单位m 147 | * @param vel_x x方向的速度, 单位m/s 148 | * @param vel_y y方向的速度, 单位m/s 149 | * @param omega 角速度，单位rad/s 150 | * @return true: 无异常, false: 异常退出 151 | */ 152 | bool ExecuteWalkDistance( 153 | double distance, float vel_x = 0.0, float vel_y = 0.0, 154 | float omega = 0.0); 155 | 156 | private: 157 | MotionUtils(); 158 | MotionUtils(const MotionUtils &) = delete; 159 | MotionUtils & operator=(const MotionUtils &) = delete; 160 | void HandleMotionStatusCallback(const MotionStatusMsg::SharedPtr msg) 161 | { 162 | motion_status_ = msg; 163 | if (motion_status_waiting_) {motion_status_cv_.notify_one();} 164 | } 165 | rclcpp::Node::SharedPtr node_; 166 | rclcpp::Publisher::SharedPtr servo_cmd_pub_; 167 | rclcpp::Subscription::SharedPtr motion_status_sub_; 168 | rclcpp::Client::SharedPtr result_cmd_client_; 169 | MotionStatusMsg::SharedPtr motion_status_; 170 | std::shared_ptr odom_helper_; 171 | std::mutex motion_status_mutex_; 172 | std::condition_variable motion_status_cv_; 173 | bool motion_status_waiting_{false}; 174 | LOGGER_MINOR_INSTANCE("MotionUtils"); 175 | }; // class MotionUtils 176 | } // namespace motion 177 | } // namespace cyberdog 178 | #endif // MOTION_UTILS__MOTION_UTILS_HPP_ 179 | -------------------------------------------------------------------------------- /motion_utils/include/motion_utils/stair_align.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 MOTION_UTILS__STAIR_ALIGN_HPP_ 15 | #define MOTION_UTILS__STAIR_ALIGN_HPP_ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include "cyberdog_common/cyberdog_log.hpp" 22 | #include "cyberdog_common/cyberdog_toml.hpp" 23 | #include "motion_utils/stair_perception.hpp" 24 | #include "motion_utils/edge_perception.hpp" 25 | #include "motion_action/motion_macros.hpp" 26 | #include "std_srvs/srv/trigger.hpp" 27 | #include "std_msgs/msg/bool.hpp" 28 | #include "std_srvs/srv/set_bool.hpp" 29 | 30 | namespace cyberdog 31 | { 32 | namespace motion 33 | { 34 | class StairAlign 35 | { 36 | public: 37 | enum class State 38 | { 39 | IDLE, 40 | BLIND_FORWARD, 41 | TURN_LEFT, 42 | TURN_RIGHT, 43 | APPROACH, 44 | FINISH 45 | }; 46 | explicit StairAlign(rclcpp::Node::SharedPtr node); 47 | void Spin() 48 | { 49 | rclcpp::spin(node_); 50 | } 51 | 52 | private: 53 | void HandleStartAlignCallback( 54 | const std_srvs::srv::SetBool_Request::SharedPtr request, 55 | std_srvs::srv::SetBool_Response::SharedPtr response); 56 | void HandleStopAlignCallback( 57 | const std_srvs::srv::Trigger::Request::SharedPtr request, 58 | std_srvs::srv::Trigger::Response::SharedPtr response); 59 | void Loop(); 60 | 61 | rclcpp::Node::SharedPtr node_; 62 | rclcpp::Publisher::SharedPtr servo_cmd_pub_, align_status_pub_; 63 | rclcpp::Service::SharedPtr start_stair_align_srv_; 64 | rclcpp::Service::SharedPtr stop_stair_align_srv_; 65 | // rclcpp::Service::SharedPtr jump_srv_; 66 | rclcpp::Client::SharedPtr result_cmd_client_; 67 | MotionServoCmdMsg servo_cmd_; 68 | rclcpp::Publisher::SharedPtr align_finish_pub_; 69 | std_msgs::msg::Bool align_finish_; 70 | std::shared_ptr stair_perception_; 71 | std::shared_ptr edge_perception_; 72 | std::mutex loop_mutex_; 73 | std::condition_variable cv_; 74 | float vel_x_, vel_omega_; 75 | bool jump_after_align_, auto_start_, is_stair_mode; 76 | bool task_start_{false}; 77 | }; // calss StairAlign 78 | } // namespace motion 79 | } // namespace cyberdog 80 | #endif // MOTION_UTILS__STAIR_ALIGN_HPP_ 81 | -------------------------------------------------------------------------------- /motion_utils/include/motion_utils/stair_perception.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 MOTION_UTILS__STAIR_PERCEPTION_HPP_ 15 | #define MOTION_UTILS__STAIR_PERCEPTION_HPP_ 16 | 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 "cyberdog_common/cyberdog_log.hpp" 30 | #include "cyberdog_common/cyberdog_toml.hpp" 31 | #include "motion_action/motion_macros.hpp" 32 | namespace cyberdog 33 | { 34 | namespace motion 35 | { 36 | 37 | class StairPerception 38 | { 39 | public: 40 | enum class State 41 | { 42 | IDLE, 43 | BLIND_FORWARD, 44 | TURN_LEFT, 45 | TURN_RIGHT, 46 | APPROACH, 47 | FINISH 48 | }; 49 | struct ApproachThreshold 50 | { 51 | std::array range; 52 | int threshold; 53 | }; 54 | 55 | public: 56 | StairPerception(const rclcpp::Node::SharedPtr node, const toml::value & config); 57 | void Launch(bool launch) {launch_ = launch;} 58 | bool CheckLaunched() {return launch_;} 59 | const State & GetStatus() const {return state_;} 60 | inline void SetStatus(const State & state) {state_ = state;} 61 | void SetTrigger() {trigger_ = true;} 62 | 63 | private: 64 | void HandlePointCloud(const sensor_msgs::msg::PointCloud2 & msg); 65 | inline int GetMeanDiff(int diff) 66 | { 67 | diffs_.emplace_back(diff); 68 | if (static_cast(diffs_.size()) > filter_size_) { 69 | diffs_.pop_front(); 70 | } 71 | int sum = 0; 72 | for (auto diff : diffs_) { 73 | sum += diff; 74 | } 75 | return sum / filter_size_; 76 | } 77 | 78 | pcl::RadiusOutlierRemoval ro_filter_; 79 | pcl::PointCloud::Ptr pc_raw_; 80 | pcl::PointCloud::Ptr pc_filtered_; 81 | pcl::PointCloud::Ptr pc_ro_filtered_; 82 | 83 | sensor_msgs::msg::PointCloud2 pc_filtered_ros_; 84 | rclcpp::Publisher::SharedPtr pc_ro_filtered_pub_; 85 | rclcpp::Node::SharedPtr node_; 86 | rclcpp::Subscription::SharedPtr pcl_sub_; 87 | State state_; 88 | std::deque diffs_; 89 | std::vector approach_thresholds_; 90 | double radius_{0.05}; 91 | std::string norms_frame_{"robot"}, pc_frame_{"robot"}, base_link_frame_{"robot"}; 92 | int min_neighbors_{5}; 93 | int filter_size_ {10}; 94 | int orientation_dead_zone_{2}, orientation_correction_{0}; 95 | int blind_forward_threshold_{15}, approach_threshold_{100}; 96 | bool trigger_ {false}, orientation_filter_ {false}; 97 | bool launch_ {false}; 98 | }; // calss StairPerception 99 | } // namespace motion 100 | } // namespace cyberdog 101 | #endif // MOTION_UTILS__STAIR_PERCEPTION_HPP_ 102 | -------------------------------------------------------------------------------- /motion_utils/launch/stair_align_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Xiaomi Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | import os 15 | 16 | from ament_index_python.packages import get_package_share_directory 17 | 18 | from launch import LaunchDescription 19 | from launch.actions import IncludeLaunchDescription 20 | from launch.launch_description_sources import PythonLaunchDescriptionSource 21 | from launch_ros.actions import Node 22 | 23 | 24 | def generate_launch_description(): 25 | return LaunchDescription([ 26 | IncludeLaunchDescription( 27 | PythonLaunchDescriptionSource([ 28 | os.path.join(get_package_share_directory('motion_utils'), 'launch'), 29 | '/tof_node_launch.py']) 30 | ), 31 | Node( 32 | package='motion_utils', 33 | executable='stair_align' 34 | ) 35 | ]) 36 | -------------------------------------------------------------------------------- /motion_utils/launch/tof_node_launch.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Xiaomi Corporation 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 | from launch import LaunchDescription 16 | from launch_ros.actions import Node 17 | 18 | 19 | def generate_launch_description(): 20 | return LaunchDescription([ 21 | Node( 22 | package='motion_utils', 23 | executable='head_tof_pcl_publisher.py', 24 | ), 25 | Node( 26 | package='tf2_ros', 27 | executable='static_transform_publisher', 28 | arguments=['0.259', '0.03', '0.102', '0', '-0.266', '0.296', 'robot', 'left_head'] 29 | ), 30 | Node( 31 | package='tf2_ros', 32 | executable='static_transform_publisher', 33 | arguments=['0.259', '-0.03', '0.102', '0', '-0.266', '-0.296', 'robot', 'right_head'] 34 | ), 35 | Node( 36 | package='tf2_ros', 37 | executable='static_transform_publisher', 38 | arguments=['-0.021', '0.042', '-0.051', '0', '0', '0.296', 'robot', 'left_rear'] 39 | ), 40 | Node( 41 | package='tf2_ros', 42 | executable='static_transform_publisher', 43 | arguments=['-0.021', '-0.042', '-0.051', '0', '0', '-0.296', 'robot', 'right_rear'] 44 | ) 45 | ]) 46 | -------------------------------------------------------------------------------- /motion_utils/motion_utils/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023 Xiaomi Corporation 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 | -------------------------------------------------------------------------------- /motion_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | motion_utils 5 | 0.0.0 6 | TODO: Package description 7 | dukun 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclcpp 14 | rclpy 15 | cyberdog_system 16 | cyberdog_common 17 | cyberdog_debug 18 | protocol 19 | nav_msgs 20 | motion_manager 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /motion_utils/scripts/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/motion/4d241d1cfca1610f3658bac31acea01277b7a47c/motion_utils/scripts/AMENT_IGNORE -------------------------------------------------------------------------------- /motion_utils/scripts/servo_delay_statistics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from protocol.msg import MotionServoCmd 4 | import rclpy 5 | import datetime 6 | from rclpy.node import Node 7 | from std_msgs.msg import Float32 8 | 9 | class ServoCmdSubscriber(Node): 10 | 11 | def __init__(self): 12 | super().__init__('servo_subscriber') 13 | self.last_timestamp = datetime.datetime.now().timestamp() 14 | self.subscription = self.create_subscription( 15 | MotionServoCmd, 16 | 'motion_servo_cmd', 17 | self.topic_callback, 18 | 10) 19 | self.publisher = self.create_publisher(Float32, "interval", 1) 20 | 21 | def topic_callback(self, msg): 22 | this_timestamp = datetime.datetime.now().timestamp() 23 | interval = this_timestamp - self.last_timestamp 24 | print(interval) 25 | interval_msg = Float32() 26 | interval_msg.data = interval 27 | self.publisher.publish(interval_msg) 28 | self.last_timestamp = this_timestamp 29 | 30 | def main(args=None): 31 | rclpy.init(args=args) 32 | servo_subscriber = ServoCmdSubscriber() 33 | rclpy.spin(servo_subscriber) 34 | servo_subscriber.destroy_node() 35 | rclpy.shutdown() 36 | 37 | if __name__ == '__main__': 38 | main() 39 | -------------------------------------------------------------------------------- /motion_utils/src/motion_utils.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 "motion_utils/motion_utils.hpp" 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace cyberdog 21 | { 22 | namespace motion 23 | { 24 | 25 | MotionUtils::MotionUtils() 26 | { 27 | node_ = rclcpp::Node::SharedPtr(new rclcpp::Node("motion_utils")); 28 | motion_status_.reset(new MotionStatusMsg); 29 | servo_cmd_pub_ = node_->create_publisher( 30 | kMotionServoCommandTopicName, rclcpp::SystemDefaultsQoS()); 31 | motion_status_sub_ = node_->create_subscription( 32 | kMotionStatusTopicName, rclcpp::SystemDefaultsQoS(), 33 | std::bind(&MotionUtils::HandleMotionStatusCallback, this, std::placeholders::_1)); 34 | result_cmd_client_ = node_->create_client(kMotionResultServiceName); 35 | odom_helper_.reset(new OdomHelper(node_)); 36 | std::thread{ 37 | [this]() {rclcpp::spin(node_);} 38 | }.detach(); 39 | } 40 | 41 | MotionUtils::~MotionUtils() {} 42 | 43 | bool MotionUtils::ExecuteWalkDuration(int duration, MotionServoCmdMsg::SharedPtr msg) 44 | { 45 | std::unique_lock lk(motion_status_mutex_); 46 | motion_status_waiting_ = true; 47 | if (motion_status_cv_.wait_for(lk, std::chrono::milliseconds(1000)) == std::cv_status::timeout) { 48 | ERROR("Cannot get motion status"); 49 | motion_status_waiting_ = false; 50 | return false; 51 | } 52 | motion_status_waiting_ = false; 53 | if (motion_status_->motion_id != MotionIDMsg::RECOVERYSTAND) { 54 | MotionResultSrv::Request::SharedPtr req(new MotionResultSrv::Request); 55 | MotionResultSrv::Response::SharedPtr res(new MotionResultSrv::Response); 56 | req->motion_id = MotionIDMsg::RECOVERYSTAND; 57 | auto future = result_cmd_client_->async_send_request(req); 58 | auto status = future.wait_for(std::chrono::milliseconds(5000)); 59 | if (status != std::future_status::ready) { 60 | ERROR("Call Service to RecoveryStand error"); 61 | return false; 62 | } 63 | if (!future.get()->result) { 64 | ERROR("Motion to RecoveryStand error"); 65 | return false; 66 | } 67 | } 68 | auto deadline = std::chrono::system_clock::now() + std::chrono::milliseconds(duration); 69 | while (rclcpp::ok() && std::chrono::system_clock::now() < deadline) { 70 | servo_cmd_pub_->publish(*msg); 71 | std::this_thread::sleep_for(std::chrono::milliseconds(50)); 72 | } 73 | return true; 74 | } 75 | 76 | bool MotionUtils::ExecuteWalkDuration(int duration, float x_vel, float y_vel, float omega) 77 | { 78 | MotionServoCmdMsg::SharedPtr msg(new MotionServoCmdMsg); 79 | msg->motion_id = MotionIDMsg::WALK_ADAPTIVELY; 80 | msg->vel_des = std::vector{x_vel, y_vel, omega}; 81 | msg->step_height = std::vector{0.05, 0.05}; 82 | return ExecuteWalkDuration(duration, msg); 83 | } 84 | 85 | 86 | bool MotionUtils::ExecuteWalkDistance(double distance, MotionServoCmdMsg::SharedPtr msg) 87 | { 88 | if (!odom_helper_->SetStartPoint()) { 89 | return false; 90 | } 91 | while (rclcpp::ok() && odom_helper_->GetDistance() < distance * distance) { 92 | servo_cmd_pub_->publish(*msg); 93 | std::this_thread::sleep_for(std::chrono::milliseconds(50)); 94 | } 95 | odom_helper_->Reset(); 96 | return true; 97 | } 98 | 99 | bool MotionUtils::ExecuteWalkDistance(double distance, float x_vel, float y_vel, float omega) 100 | { 101 | MotionServoCmdMsg::SharedPtr msg(new MotionServoCmdMsg); 102 | msg->motion_id = MotionIDMsg::WALK_ADAPTIVELY; 103 | msg->vel_des = std::vector{x_vel, y_vel, omega}; 104 | msg->step_height = std::vector{0.05, 0.05}; 105 | return ExecuteWalkDistance(distance, msg); 106 | } 107 | 108 | } // namespace motion 109 | } // namespace cyberdog 110 | -------------------------------------------------------------------------------- /motion_utils/test/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/motion/4d241d1cfca1610f3658bac31acea01277b7a47c/motion_utils/test/AMENT_IGNORE -------------------------------------------------------------------------------- /motion_utils/test/distance_test.cpp: -------------------------------------------------------------------------------- 1 | #include "motion_utils/motion_utils.hpp" 2 | #include "motion_action/motion_macros.hpp" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | rclcpp::init(argc, argv); 7 | cyberdog::motion::MotionUtils& mu = cyberdog::motion::MotionUtils::GetMotionUtils(); 8 | // mu.ExecuteWalkDuration(10000, 0.1, 0.2, 0.3); 9 | mu.ExecuteWalkDistance(1.0, 0.1, 0.0, 0.0); 10 | } -------------------------------------------------------------------------------- /motion_utils/test/duration_test.cpp: -------------------------------------------------------------------------------- 1 | #include "motion_utils/motion_utils.hpp" 2 | #include "motion_action/motion_macros.hpp" 3 | 4 | int main(int argc, char** argv) 5 | { 6 | rclcpp::init(argc, argv); 7 | cyberdog::motion::MotionUtils& mu = cyberdog::motion::MotionUtils::GetMotionUtils(); 8 | // mu.ExecuteWalkDuration(10000, 0.1, 0.2, 0.3); 9 | mu.ExecuteWalkDuration(2000, 0.1, 0.0, 0.0); 10 | } -------------------------------------------------------------------------------- /motion_utils/test/edge_align.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 "motion_utils/edge_align.hpp" 15 | 16 | namespace cyberdog 17 | { 18 | namespace motion 19 | { 20 | 21 | EdgeAlign::EdgeAlign(rclcpp::Node::SharedPtr node) 22 | { 23 | node_ = node; 24 | servo_cmd_pub_ = node_->create_publisher(kMotionServoCommandTopicName, 1); 25 | align_finish_pub_ = node_->create_publisher("edge_align_finished_flag", 1); 26 | result_cmd_client_ = node_->create_client(kMotionResultServiceName); 27 | edge_align_srv_ = node->create_service( 28 | "edge_align", std::bind(&EdgeAlign::HandleServiceCallback, this, std::placeholders::_1, std::placeholders::_2)); 29 | servo_cmd_.motion_id = MotionIDMsg::WALK_ADAPTIVELY; 30 | servo_cmd_.step_height = std::vector{0.05, 0.05}; 31 | servo_cmd_.value = 2; 32 | align_finish_.data = false; 33 | 34 | std::string toml_file = ament_index_cpp::get_package_share_directory("motion_utils") + "/config/edge_align.toml"; 35 | toml::value config; 36 | if(!cyberdog::common::CyberdogToml::ParseFile(toml_file, config)) { 37 | FATAL("Cannot parse %s", toml_file.c_str()); 38 | exit(-1); 39 | } 40 | GET_TOML_VALUE(config, "vel_x", vel_x_); 41 | GET_TOML_VALUE(config, "vel_omega", vel_omega_); 42 | GET_TOML_VALUE(config, "jump_after_align", jump_after_align_); 43 | GET_TOML_VALUE(config, "auto_start", auto_start_); 44 | // INFO("%f, %f, %d", vel_x_, vel_omega_, jump_after_align_); 45 | edge_perception_ = std::make_shared(node, config); 46 | std::thread{std::bind(&EdgeAlign::Loop, this)}.detach(); 47 | } 48 | 49 | void EdgeAlign::HandleServiceCallback( 50 | const std_srvs::srv::Trigger_Request::SharedPtr, 51 | std_srvs::srv::Trigger_Response::SharedPtr) 52 | { 53 | cv_.notify_one(); 54 | } 55 | 56 | void EdgeAlign::Loop() 57 | { 58 | static bool task_waiting = true; 59 | if (!auto_start_ && task_waiting) { 60 | std::unique_lock lk(loop_mutex_); 61 | cv_.wait(lk); 62 | task_waiting = false; 63 | } 64 | static bool perception_launched = false; 65 | if (!perception_launched) { 66 | edge_perception_->Launch(true); 67 | perception_launched = false; 68 | } 69 | while(rclcpp::ok()){ 70 | switch (edge_perception_->GetStatus()) 71 | { 72 | case EdgePerception::State::IDLE: 73 | break; 74 | 75 | case EdgePerception::State::BLIND_FORWARD: 76 | servo_cmd_.vel_des = std::vector{vel_x_, 0.0, 0.0}; 77 | servo_cmd_pub_->publish(servo_cmd_); 78 | break; 79 | 80 | case EdgePerception::State::TURN_LEFT: 81 | servo_cmd_.vel_des = std::vector{0.0, 0.0, vel_omega_}; 82 | servo_cmd_pub_->publish(servo_cmd_); 83 | break; 84 | 85 | case EdgePerception::State::TURN_RIGHT: 86 | servo_cmd_.vel_des = std::vector{0.0, 0.0, -vel_omega_}; 87 | servo_cmd_pub_->publish(servo_cmd_); 88 | break; 89 | 90 | case EdgePerception::State::APPROACH: 91 | servo_cmd_.vel_des = std::vector{vel_x_, 0.0, 0.0}; 92 | servo_cmd_pub_->publish(servo_cmd_); 93 | break; 94 | 95 | case EdgePerception::State::FINISH: 96 | { 97 | edge_perception_->SetStatus(EdgePerception::State::IDLE); 98 | servo_cmd_.cmd_type = MotionServoCmdMsg::SERVO_END; 99 | servo_cmd_pub_->publish(servo_cmd_); 100 | align_finish_.data = true; 101 | if(jump_after_align_&&!edge_perception_->GetEdgeIsDeep()) { 102 | std::this_thread::sleep_for(std::chrono::milliseconds(2000)); 103 | MotionResultSrv::Request::SharedPtr req(new MotionResultSrv::Request); 104 | req->motion_id = 137; 105 | result_cmd_client_->async_send_request(req); 106 | } 107 | edge_perception_->Launch(false); 108 | task_waiting = true; 109 | break; 110 | } 111 | 112 | default: 113 | break; 114 | } 115 | align_finish_pub_->publish(align_finish_); 116 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); 117 | } 118 | } 119 | 120 | } 121 | } 122 | 123 | int main(int argc, char ** argv) 124 | { 125 | rclcpp::init(argc, argv); 126 | rclcpp::Node::SharedPtr node = std::make_shared("edge_align"); 127 | cyberdog::motion::EdgeAlign sa(node); 128 | sa.Spin(); 129 | rclcpp::shutdown(); 130 | } 131 | -------------------------------------------------------------------------------- /motion_utils/test/edge_perception.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 "motion_utils/edge_perception.hpp" 15 | 16 | namespace cyberdog 17 | { 18 | namespace motion 19 | { 20 | 21 | EdgePerception::EdgePerception(rclcpp::Node::SharedPtr node, const toml::value& config) 22 | { 23 | node_ = node; 24 | 25 | pc_raw_.reset(new pcl::PointCloud); 26 | GET_TOML_VALUE(config, "radius", radius_); 27 | GET_TOML_VALUE(config, "min_neighbors", min_neighbors_); 28 | GET_TOML_VALUE(config, "orientation_dead_zone", orientation_dead_zone_); 29 | GET_TOML_VALUE(config, "orientation_correction", orientation_correction_); 30 | GET_TOML_VALUE(config, "orientation_filter", orientation_filter_); 31 | GET_TOML_VALUE(config, "filter_size", filter_size_); 32 | GET_TOML_VALUE(config, "blind_forward_threshold", blind_forward_threshold_); 33 | GET_TOML_VALUE(config, "threshold", approach_threshold_); 34 | GET_TOML_VALUE(config, "max_depth", max_depth_); 35 | 36 | pc_rofiltered_.reset(new pcl::PointCloud); 37 | ro_filter_.setRadiusSearch(radius_); 38 | ro_filter_.setMinNeighborsInRadius(min_neighbors_); 39 | 40 | pc_ptfiltered_.reset(new pcl::PointCloud); 41 | pt_filter_.setFilterFieldName("z"); 42 | pt_filter_.setFilterLimits(-0.35, -0.1); 43 | 44 | pcl_sub_ = node_->create_subscription( 45 | "head_pc", 46 | rclcpp::SystemDefaultsQoS(), 47 | std::bind(&EdgePerception::HandlePointCloud, this, std::placeholders::_1)); 48 | pc_ro_filtered_pub_ = node_->create_publisher("ro_filtered", 1); 49 | state_ = State::IDLE; 50 | trigger_ = true; 51 | is_edge_deep_ = false; 52 | } 53 | 54 | void EdgePerception::HandlePointCloud(const sensor_msgs::msg::PointCloud2 & msg) 55 | { 56 | if(!launch_) { 57 | return; 58 | } 59 | INFO("----------------"); 60 | pcl::fromROSMsg(msg, *pc_raw_); 61 | ro_filter_.setInputCloud(pc_raw_); 62 | ro_filter_.filter(*pc_rofiltered_); 63 | pt_filter_.setInputCloud(pc_rofiltered_); 64 | pt_filter_.filter(*pc_ptfiltered_); 65 | 66 | pcl::toROSMsg(*pc_ptfiltered_, pc_filtered_ros_); 67 | pc_ro_filtered_pub_->publish(pc_filtered_ros_); 68 | int total_points_size = pc_ptfiltered_->size(); 69 | int left_point_size = 0; 70 | int right_point_size = 0; 71 | // int dead_zone = 2, correction = 0; 72 | float z = 0, sum = 0; 73 | float x_filtered_max = 0; 74 | float z_beyond_xfmax = 0, sum_beyond_xfmax = 0; 75 | int beyond_xfmax_point_size = 0; 76 | for (auto point : pc_ptfiltered_->points) { 77 | if (point.y > 0) { 78 | left_point_size++; 79 | } else { 80 | right_point_size++; 81 | } 82 | // TODO(): 去除极大极小值 83 | // if(abs(point.z) <= 0.05 ) { 84 | // continue; 85 | // } 86 | sum += abs(point.z); 87 | if (point.x > x_filtered_max) 88 | x_filtered_max = point.x; 89 | } 90 | z = total_points_size > 0 ? sum / total_points_size : 0.5; 91 | 92 | for (auto point : pc_raw_->points) { 93 | if (point.x > x_filtered_max) { 94 | beyond_xfmax_point_size++; 95 | sum_beyond_xfmax += abs(point.z); 96 | } 97 | } 98 | z_beyond_xfmax = beyond_xfmax_point_size > 0 ? sum_beyond_xfmax / beyond_xfmax_point_size : 0.5; 99 | is_edge_deep_ = (z_beyond_xfmax > max_depth_); 100 | INFO("left: %d, right: %d, total: %d, z: %f", left_point_size, right_point_size, total_points_size, z); 101 | INFO("z_beyond_xfmax: %f, beyond_xfmax_point_size: %d",z_beyond_xfmax, beyond_xfmax_point_size); 102 | int diff = 0; 103 | if (orientation_filter_) { 104 | diff = GetMeanDiff(left_point_size - right_point_size); 105 | } else { 106 | diff = left_point_size - right_point_size; 107 | } 108 | switch (state_) { 109 | case State::IDLE: 110 | if (trigger_) { 111 | state_ = State::BLIND_FORWARD; 112 | trigger_ = false; 113 | INFO("Launch!"); 114 | } 115 | break; 116 | 117 | case State::BLIND_FORWARD: 118 | if (total_points_size > blind_forward_threshold_) { 119 | INFO("Points size %d < threshold, edge not found, Blind Forward", total_points_size); 120 | break; 121 | } 122 | if (diff > orientation_dead_zone_ + orientation_correction_) { 123 | INFO("Turn right: %d", diff); 124 | state_ = State::TURN_RIGHT; 125 | } else if (diff < -orientation_dead_zone_ + orientation_correction_) { 126 | INFO("Turn left: %d", diff); 127 | state_ = State::TURN_LEFT; 128 | } else if (total_points_size < approach_threshold_ + 20){ 129 | INFO("Approach Directly: %d", diff); 130 | state_ = State::APPROACH; 131 | } 132 | break; 133 | 134 | case State::TURN_LEFT: 135 | if (diff >= -orientation_dead_zone_ + orientation_correction_) { 136 | if (total_points_size < approach_threshold_) { 137 | INFO("Finish turning left: %d", diff); 138 | state_ = State::FINISH; 139 | } else if (total_points_size < approach_threshold_ + 20){ 140 | INFO("Will approach when turning left: %d", total_points_size); 141 | state_ = State::APPROACH; 142 | } else { 143 | state_ = State::BLIND_FORWARD; 144 | } 145 | } 146 | INFO("Turn left: %d", diff); 147 | break; 148 | 149 | case State::TURN_RIGHT: 150 | if (diff <= orientation_dead_zone_ + orientation_correction_) { 151 | if(total_points_size < approach_threshold_) { 152 | INFO("Finish turning right: %d", diff); 153 | state_ = State::FINISH; 154 | } else if (total_points_size < approach_threshold_ + 20){ 155 | INFO("Will approach when turning right: %d", total_points_size); 156 | state_ = State::APPROACH; 157 | } else { 158 | state_ = State::BLIND_FORWARD; 159 | } 160 | } 161 | INFO("Turn right: %d", diff); 162 | break; 163 | 164 | case State::APPROACH: 165 | if (total_points_size < approach_threshold_) { 166 | INFO("Stop: %d", total_points_size); 167 | state_ = State::FINISH; 168 | } 169 | INFO("Approaching: %d", total_points_size); 170 | break; 171 | 172 | case State::FINISH: 173 | break; 174 | 175 | default: 176 | break; 177 | } 178 | } 179 | } 180 | } 181 | -------------------------------------------------------------------------------- /motion_utils/test/edge_perception_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 | #include "motion_utils/edge_perception.hpp" 15 | #include 16 | int main(int argc, char** argv) 17 | { 18 | rclcpp::init(argc, argv); 19 | rclcpp::Node::SharedPtr node(new rclcpp::Node("edge_perception_test")); 20 | std::string toml_file = ament_index_cpp::get_package_share_directory("motion_utils") + "/config/edge_align.toml"; 21 | toml::value config; 22 | if(!cyberdog::common::CyberdogToml::ParseFile(toml_file, config)) { 23 | FATAL("Cannot parse %s", toml_file.c_str()); 24 | exit(-1); 25 | } 26 | cyberdog::motion::EdgePerception sp(node, config); 27 | rclcpp::spin(node); 28 | return 0; 29 | } -------------------------------------------------------------------------------- /motion_utils/test/stair_align.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 "motion_utils/stair_align.hpp" 15 | 16 | namespace cyberdog 17 | { 18 | namespace motion 19 | { 20 | 21 | StairAlign::StairAlign(rclcpp::Node::SharedPtr node) 22 | { 23 | node_ = node; 24 | servo_cmd_pub_ = node_->create_publisher( 25 | kMotionServoCommandTopicName, 1); 26 | align_finish_pub_ = node_->create_publisher( 27 | "stair_align_finished_flag", 1); 28 | result_cmd_client_ = node_->create_client( 29 | kMotionResultServiceName); 30 | start_stair_align_srv_ = node->create_service( 31 | "start_stair_align", 32 | std::bind(&StairAlign::HandleStartAlignCallback, 33 | this, std::placeholders::_1, std::placeholders::_2)); 34 | stop_stair_align_srv_ = node->create_service( 35 | "stop_stair_align", 36 | std::bind(&StairAlign::HandleStopAlignCallback, 37 | this, std::placeholders::_1, std::placeholders::_2)); 38 | servo_cmd_.motion_id = 308; 39 | servo_cmd_.step_height = std::vector{0.05, 0.05}; 40 | servo_cmd_.value = 2; 41 | align_finish_.data = false; 42 | // params config of stair_align 43 | std::string toml_file = ament_index_cpp::get_package_share_directory( 44 | "motion_utils") + "/config/stair_align.toml"; 45 | toml::value config; 46 | if(!cyberdog::common::CyberdogToml::ParseFile(toml_file, config)) { 47 | FATAL("Cannot parse %s", toml_file.c_str()); 48 | exit(-1); 49 | } 50 | GET_TOML_VALUE(config, "vel_x", vel_x_); 51 | GET_TOML_VALUE(config, "vel_omega", vel_omega_); 52 | GET_TOML_VALUE(config, "jump_after_align", jump_after_align_); 53 | GET_TOML_VALUE(config, "auto_start", auto_start_); 54 | GET_TOML_VALUE(config, "is_stair_mode", is_stair_mode); 55 | // INFO("%f, %f, %d", vel_x_, vel_omega_, jump_after_align_); 56 | stair_perception_ = std::make_shared(node, config); 57 | 58 | // parmas config of edge_align 59 | toml_file = ament_index_cpp::get_package_share_directory( 60 | "motion_utils") + "/config/edge_align.toml"; 61 | if(!cyberdog::common::CyberdogToml::ParseFile(toml_file, config)) { 62 | FATAL("Cannot parse %s", toml_file.c_str()); 63 | exit(-1); 64 | } 65 | edge_perception_ = std::make_shared(node, config); 66 | std::thread{std::bind(&StairAlign::Loop, this)}.detach(); 67 | } 68 | 69 | void StairAlign::HandleStartAlignCallback( 70 | const std_srvs::srv::SetBool_Request::SharedPtr request, 71 | std_srvs::srv::SetBool_Response::SharedPtr response) 72 | { 73 | // auto_start_ = true; 74 | if (request->data) { 75 | INFO("Get request to align upstair"); 76 | is_stair_mode = true; 77 | } else { 78 | INFO("Get request to align downstair"); 79 | is_stair_mode = false; 80 | } 81 | cv_.notify_one(); 82 | response->success = true; 83 | } 84 | 85 | void StairAlign::HandleStopAlignCallback( 86 | const std_srvs::srv::Trigger::Request::SharedPtr request, 87 | std_srvs::srv::Trigger::Response::SharedPtr response) 88 | { 89 | (void)request; 90 | task_start_= false; 91 | if ( is_stair_mode ) { 92 | stair_perception_->Launch(false); 93 | stair_perception_->SetStatus(StairPerception::State::IDLE); 94 | } 95 | else { 96 | edge_perception_->Launch(false); 97 | edge_perception_->SetStatus(EdgePerception::State::IDLE); 98 | } 99 | response->success = true; 100 | INFO("Get request to stop aligning"); 101 | } 102 | 103 | void StairAlign::Loop() 104 | { 105 | while(rclcpp::ok()){ 106 | if (auto_start_ && !task_start_) { 107 | task_start_ = true; 108 | if ( is_stair_mode ) { 109 | if (!stair_perception_->CheckLaunched()) { 110 | stair_perception_->Launch(true); 111 | } 112 | } else { 113 | if (!edge_perception_->CheckLaunched()) { 114 | edge_perception_->Launch(true); 115 | } 116 | } 117 | servo_cmd_.cmd_type = MotionServoCmdMsg::SERVO_START; 118 | } 119 | else if (!task_start_) { 120 | std::unique_lock lk(loop_mutex_); 121 | cv_.wait(lk); 122 | task_start_ = true; 123 | if ( is_stair_mode ) { 124 | if (!stair_perception_->CheckLaunched()) { 125 | stair_perception_->Launch(true); 126 | } 127 | stair_perception_->SetTrigger(); 128 | } else { 129 | if (!edge_perception_->CheckLaunched()) { 130 | edge_perception_->Launch(true); 131 | } 132 | edge_perception_->SetTrigger(); 133 | } 134 | servo_cmd_.cmd_type = MotionServoCmdMsg::SERVO_START; 135 | } 136 | int status = is_stair_mode ? static_cast(stair_perception_->GetStatus()) : static_cast(edge_perception_->GetStatus()); 137 | switch ( status ) 138 | { 139 | case static_cast(StairAlign::State::IDLE): 140 | break; 141 | 142 | case static_cast(StairAlign::State::BLIND_FORWARD): 143 | servo_cmd_.vel_des = std::vector{vel_x_, 0.0, 0.0}; 144 | servo_cmd_pub_->publish(servo_cmd_); 145 | break; 146 | 147 | case static_cast(StairAlign::State::TURN_LEFT): 148 | servo_cmd_.vel_des = std::vector{0.0, 0.0, vel_omega_}; 149 | servo_cmd_pub_->publish(servo_cmd_); 150 | break; 151 | 152 | case static_cast(StairAlign::State::TURN_RIGHT): 153 | servo_cmd_.vel_des = std::vector{0.0, 0.0, -vel_omega_}; 154 | servo_cmd_pub_->publish(servo_cmd_); 155 | break; 156 | 157 | case static_cast(StairAlign::State::APPROACH): 158 | servo_cmd_.vel_des = std::vector{(is_stair_mode ? vel_x_ : 0.3 * vel_x_), 0.0, 0.0}; 159 | servo_cmd_pub_->publish(servo_cmd_); 160 | break; 161 | 162 | case static_cast(StairAlign::State::FINISH): 163 | { 164 | if (is_stair_mode) 165 | stair_perception_->SetStatus(StairPerception::State::IDLE); 166 | else 167 | edge_perception_->SetStatus(EdgePerception::State::IDLE); 168 | servo_cmd_.cmd_type = MotionServoCmdMsg::SERVO_END; 169 | servo_cmd_pub_->publish(servo_cmd_); 170 | align_finish_.data = true; 171 | align_finish_pub_->publish(align_finish_); 172 | if(jump_after_align_) { 173 | std::this_thread::sleep_for(std::chrono::milliseconds(2000)); 174 | MotionResultSrv::Request::SharedPtr req(new MotionResultSrv::Request); 175 | req->motion_id = is_stair_mode ? 126 : 137; 176 | result_cmd_client_->async_send_request(req); 177 | } 178 | if (is_stair_mode) 179 | stair_perception_->Launch(false); 180 | else 181 | edge_perception_->Launch(false); 182 | task_start_ = false; 183 | break; 184 | } 185 | 186 | default: 187 | break; 188 | } 189 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); 190 | } 191 | } 192 | 193 | } 194 | } 195 | 196 | int main(int argc, char ** argv) 197 | { 198 | rclcpp::init(argc, argv); 199 | rclcpp::Node::SharedPtr node = std::make_shared("stair_align"); 200 | cyberdog::motion::StairAlign sa(node); 201 | sa.Spin(); 202 | rclcpp::shutdown(); 203 | } 204 | -------------------------------------------------------------------------------- /motion_utils/test/stair_perception.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 "motion_utils/stair_perception.hpp" 15 | 16 | namespace cyberdog 17 | { 18 | namespace motion 19 | { 20 | 21 | StairPerception::StairPerception(rclcpp::Node::SharedPtr node, const toml::value& config) 22 | { 23 | node_ = node; 24 | 25 | pc_raw_.reset(new pcl::PointCloud); 26 | GET_TOML_VALUE(config, "radius", radius_); 27 | GET_TOML_VALUE(config, "min_neighbors", min_neighbors_); 28 | GET_TOML_VALUE(config, "orientation_dead_zone", orientation_dead_zone_); 29 | GET_TOML_VALUE(config, "orientation_correction", orientation_correction_); 30 | GET_TOML_VALUE(config, "orientation_filter", orientation_filter_); 31 | GET_TOML_VALUE(config, "filter_size", filter_size_); 32 | GET_TOML_VALUE(config, "blind_forward_threshold", blind_forward_threshold_); 33 | toml::value thresholds; 34 | GET_TOML_VALUE(config, "approach_threshold", thresholds); 35 | for(size_t i = 0; i < thresholds.size(); ++i) { 36 | toml::value threshold = thresholds.at(i); 37 | std::array z_range; 38 | int approach_threshold; 39 | GET_TOML_VALUE(threshold, "z_range", z_range); 40 | GET_TOML_VALUE(threshold, "threshold", approach_threshold); 41 | approach_thresholds_.emplace_back(ApproachThreshold{z_range, approach_threshold}); 42 | } 43 | pc_filtered_.reset(new pcl::PointCloud); 44 | ro_filter_.setRadiusSearch(radius_); 45 | ro_filter_.setMinNeighborsInRadius(min_neighbors_); 46 | 47 | pcl_sub_ = node_->create_subscription( 48 | "head_pc", 49 | rclcpp::SystemDefaultsQoS(), 50 | std::bind(&StairPerception::HandlePointCloud, this, std::placeholders::_1)); 51 | pc_ro_filtered_pub_ = node_->create_publisher("ro_filtered", 1); 52 | state_ = State::IDLE; 53 | trigger_ = true; 54 | } 55 | 56 | void StairPerception::HandlePointCloud(const sensor_msgs::msg::PointCloud2 & msg) 57 | { 58 | if(!launch_) { 59 | return; 60 | } 61 | INFO("----------------"); 62 | pcl::fromROSMsg(msg, *pc_raw_); 63 | ro_filter_.setInputCloud(pc_raw_); 64 | ro_filter_.filter(*pc_filtered_); 65 | pcl::toROSMsg(*pc_filtered_, pc_filtered_ros_); 66 | pc_ro_filtered_pub_->publish(pc_filtered_ros_); 67 | int total_points_size = pc_filtered_->size(); 68 | int left_point_size = 0; 69 | int right_point_size = 0; 70 | // int dead_zone = 2, correction = 0; 71 | float z = 0, sum = 0; 72 | for (auto point : pc_filtered_->points) { 73 | if (point.y > 0) { 74 | left_point_size++; 75 | } else { 76 | right_point_size++; 77 | } 78 | // TODO(): 去除极大极小值 79 | // if(abs(point.z) <= 0.05 ) { 80 | // continue; 81 | // } 82 | sum += abs(point.z); 83 | } 84 | z = sum / pc_filtered_->points.size(); 85 | for (auto threshold : approach_thresholds_) { 86 | if( z > threshold.range.front() && z <= threshold.range.back()) { 87 | approach_threshold_ = threshold.threshold; 88 | break; 89 | } 90 | } 91 | INFO("left: %d, right: %d, z: %f, th: %d", left_point_size, right_point_size, z, approach_threshold_); 92 | int diff = 0; 93 | if (orientation_filter_) { 94 | diff = GetMeanDiff(left_point_size - right_point_size); 95 | } else { 96 | diff = left_point_size - right_point_size; 97 | } 98 | switch (state_) { 99 | case State::IDLE: 100 | if (trigger_) { 101 | state_ = State::BLIND_FORWARD; 102 | trigger_ = false; 103 | INFO("Launch!"); 104 | } 105 | break; 106 | 107 | case State::BLIND_FORWARD: 108 | if (total_points_size < blind_forward_threshold_) { 109 | INFO("Points size %d < threshold, stair not found, Blind Forward", total_points_size); 110 | break; 111 | } 112 | if (diff < -orientation_dead_zone_ + orientation_correction_) { 113 | INFO("Turn right: %d", diff); 114 | state_ = State::TURN_RIGHT; 115 | } else if (diff > orientation_dead_zone_ + orientation_correction_) { 116 | INFO("Turn left: %d", diff); 117 | state_ = State::TURN_LEFT; 118 | } else { 119 | INFO("Approach Directly: %d", diff); 120 | state_ = State::APPROACH; 121 | } 122 | break; 123 | 124 | case State::TURN_LEFT: 125 | if (diff <= orientation_dead_zone_ + orientation_correction_) { 126 | if (total_points_size > approach_threshold_) { 127 | INFO("Finish turning left: %d", diff); 128 | state_ = State::FINISH; 129 | } else { 130 | INFO("Will approach when turning left: %d", total_points_size); 131 | state_ = State::APPROACH; 132 | } 133 | } 134 | INFO("Turn left: %d", diff); 135 | break; 136 | 137 | case State::TURN_RIGHT: 138 | if (diff >= -orientation_dead_zone_ + orientation_correction_) { 139 | if(total_points_size > approach_threshold_) { 140 | INFO("Finish turning right: %d", diff); 141 | state_ = State::FINISH; 142 | } else { 143 | INFO("Will approach when turning right: %d", total_points_size); 144 | state_ = State::APPROACH; 145 | } 146 | } 147 | INFO("Turn right: %d", diff); 148 | break; 149 | 150 | case State::APPROACH: 151 | if (total_points_size > approach_threshold_) { 152 | INFO("Stop: %d", total_points_size); 153 | state_ = State::FINISH; 154 | } 155 | INFO("Approaching: %d", total_points_size); 156 | break; 157 | 158 | case State::FINISH: 159 | break; 160 | 161 | default: 162 | break; 163 | } 164 | } 165 | } 166 | } 167 | -------------------------------------------------------------------------------- /motion_utils/test/stair_perception_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 | #include "motion_utils/stair_perception.hpp" 15 | #include 16 | int main(int argc, char** argv) 17 | { 18 | rclcpp::init(argc, argv); 19 | rclcpp::Node::SharedPtr node(new rclcpp::Node("stair_perception_test")); 20 | std::string toml_file = ament_index_cpp::get_package_share_directory("motion_utils") + "/config/stair_align.toml"; 21 | toml::value config; 22 | if(!cyberdog::common::CyberdogToml::ParseFile(toml_file, config)) { 23 | FATAL("Cannot parse %s", toml_file.c_str()); 24 | exit(-1); 25 | } 26 | cyberdog::motion::StairPerception sp(node, config); 27 | rclcpp::spin(node); 28 | return 0; 29 | } -------------------------------------------------------------------------------- /skin_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(skin_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(std_msgs REQUIRED) 19 | find_package(builtin_interfaces REQUIRED) 20 | find_package(toml REQUIRED) 21 | find_package(protocol REQUIRED) 22 | find_package(ament_index_cpp REQUIRED) 23 | find_package(cyberdog_common REQUIRED) 24 | find_package(elec_skin REQUIRED) 25 | 26 | include_directories(include) 27 | 28 | add_library(${PROJECT_NAME} SHARED src/skin_manager.cpp) 29 | 30 | 31 | # MESSAGE('--------------${CMAKE_INSTALL_PREFIX}------------------') 32 | 33 | ament_target_dependencies( ${PROJECT_NAME} 34 | rclcpp 35 | std_msgs 36 | toml 37 | protocol 38 | builtin_interfaces 39 | ament_index_cpp 40 | cyberdog_common 41 | elec_skin ) 42 | 43 | install( 44 | DIRECTORY include/ 45 | DESTINATION include 46 | ) 47 | 48 | install(TARGETS ${PROJECT_NAME} 49 | LIBRARY DESTINATION lib 50 | RUNTIME DESTINATION lib/${PROJECT_NAME} 51 | ) 52 | 53 | install( 54 | DIRECTORY include/ 55 | DESTINATION include/ 56 | ) 57 | 58 | install(DIRECTORY config/ 59 | DESTINATION share/${PROJECT_NAME}/config/ 60 | ) 61 | 62 | if(BUILD_TESTING) 63 | find_package(ament_lint_auto REQUIRED) 64 | # the following line skips the linter which checks for copyrights 65 | # uncomment the line when a copyright and license is not present in all source files 66 | #set(ament_cmake_copyright_FOUND TRUE) 67 | # the following line skips cpplint (only works in a git repo) 68 | # uncomment the line when this package is not in a git repo 69 | #set(ament_cmake_cpplint_FOUND TRUE) 70 | ament_lint_auto_find_test_dependencies() 71 | endif() 72 | 73 | ament_export_libraries(${PROJECT_NAME}) 74 | ament_export_dependencies(elec_skin) 75 | ament_package() 76 | -------------------------------------------------------------------------------- /skin_manager/config/elec_skin.toml: -------------------------------------------------------------------------------- 1 | default_color = 0 # 0 white, 1 black 2 | start_direction = 0 # 0 front, 1 back 3 | gradual_duration = 50 # milsecs 4 | defaul_duration = 2000 # milsecs 5 | enable = false 6 | align_contact = true -------------------------------------------------------------------------------- /skin_manager/include/skin_manager/skin_manager.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 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 SKIN_MANAGER__SKIN_MANAGER_HPP_ 15 | #define SKIN_MANAGER__SKIN_MANAGER_HPP_ 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include "rclcpp/rclcpp.hpp" 24 | #include "cyberdog_common/cyberdog_log.hpp" 25 | #include "cyberdog_common/cyberdog_toml.hpp" 26 | #include "ament_index_cpp/get_package_share_directory.hpp" 27 | #include "protocol/srv/elec_skin.hpp" 28 | #include "std_srvs/srv/set_bool.hpp" 29 | #include "elec_skin/elec_skin_base.hpp" 30 | namespace cyberdog 31 | { 32 | namespace motion 33 | { 34 | class SkinManagerNode : public rclcpp::Node 35 | { 36 | public: 37 | explicit SkinManagerNode(std::string name); 38 | 39 | void ShowMoveElecSkin(std::vector & msg) 40 | { 41 | if (!enable_) { 42 | return; 43 | } 44 | if (!align_contact_) { 45 | return; 46 | } 47 | static auto last_contact = std::vector(4, 0); 48 | auto contact = std::vector(4, 0); 49 | for (uint8_t i = 0; i < 4; ++i) { 50 | contact[i] = msg[i] > 0 ? 1 : 0; 51 | if (contact[i] == last_contact[i]) { 52 | continue; 53 | } 54 | last_contact[i] = contact[i]; 55 | if (contact[i] == 1) { 56 | WARN("Leg %d liftdown", i); 57 | for (auto p : leg_map[i]) { 58 | // INFO("%d, %d, %d, %d", p, change_dir_.front(), start_dir_, gradual_duration_); 59 | elec_skin_->PositionContril( 60 | p, 61 | // change_dir_.front(), 62 | liftdown_color_, 63 | start_dir_, 64 | gradual_duration_); 65 | } 66 | } else { 67 | WARN("Leg %d liftup", i); 68 | for (auto p : leg_map[i]) { 69 | // INFO("%d, %d, %d, %d", p, change_dir_.back(), start_dir_, gradual_duration_); 70 | elec_skin_->PositionContril( 71 | p, 72 | liftup_color_, 73 | // change_dir_.back(), 74 | start_dir_, 75 | gradual_duration_); 76 | } 77 | } 78 | } 79 | } 80 | 81 | void ShowBlackElecSkin(int32_t & duration) 82 | { 83 | // align_contact_ = false; 84 | // grandual_duration_ = request->wave_cycle_time; 85 | // INFO("ShowBlackElecSkin"); 86 | for (uint8_t i = 0; i < 4; ++i) { 87 | for (auto p : leg_map[i]) { 88 | // INFO("%d, %d, %d, %d", p, change_dir_.back(), start_dir_, duration); 89 | elec_skin_->PositionContril( 90 | p, 91 | change_dir_.front(), 92 | start_dir_, 93 | duration); 94 | } 95 | } 96 | } 97 | 98 | void ShowWhiteElecSkin(int32_t & duration) 99 | { 100 | // align_contact_ = false; 101 | // grandual_duration_ = request->wave_cycle_time; 102 | // INFO("ShowWhiteElecSkin"); 103 | for (uint8_t i = 0; i < 4; i++) { 104 | for (auto p : leg_map[i]) { 105 | // INFO("%d, %d, %d, %d", p, change_dir_.front(), start_dir_, duration); 106 | elec_skin_->PositionContril( 107 | p, 108 | change_dir_.back(), 109 | start_dir_, 110 | duration); 111 | } 112 | } 113 | } 114 | 115 | void ShowFrontElecSkin(int32_t & duration) 116 | { 117 | elec_skin_->ModelControl(ModelSwitch::MS_WAVEF, duration); 118 | } 119 | 120 | void ShowBackElecSkin(int32_t & duration) 121 | { 122 | elec_skin_->ModelControl(ModelSwitch::MS_WAVEB, duration); 123 | } 124 | 125 | void ShowFlashElecSkin(int32_t & duration) 126 | { 127 | elec_skin_->ModelControl(ModelSwitch::MS_FLASH, duration); 128 | } 129 | 130 | void ShowRandomElecSkin(int32_t & duration) 131 | { 132 | elec_skin_->ModelControl(ModelSwitch::MS_RANDOM, duration); 133 | } 134 | 135 | void SetAlignContact(bool align_contact) 136 | { 137 | if (!align_contact) { 138 | last_align_contact_ = align_contact_; 139 | align_contact_ = align_contact; 140 | } else { 141 | if (!last_align_contact_) { 142 | last_align_contact_ = true; 143 | } else { 144 | align_contact_ = align_contact; 145 | } 146 | } 147 | } 148 | 149 | void ShowDefaultSkin() 150 | { 151 | INFO("ShowDefaultSkin"); 152 | elec_skin_->ModelControl(ModelSwitch::MS_WAVEF, defaul_duration_); 153 | } 154 | 155 | bool GetEnableStatus() 156 | { 157 | return enable_; 158 | } 159 | 160 | void WriteTomlFile() 161 | { 162 | std::string elec_skin = ament_index_cpp::get_package_share_directory("skin_manager") + 163 | "/config/" + "elec_skin.toml"; 164 | toml::value temp; 165 | if (!cyberdog::common::CyberdogToml::ParseFile(elec_skin, temp)) { 166 | FATAL("Cannot parse %s", elec_skin.c_str()); 167 | } 168 | cyberdog::common::CyberdogToml::Set(temp, "default_color", default_color_); 169 | cyberdog::common::CyberdogToml::Set(temp, "start_direction", start_direction_); 170 | cyberdog::common::CyberdogToml::Set(temp, "gradual_duration", gradual_duration_); 171 | cyberdog::common::CyberdogToml::Set(temp, "defaul_duration", defaul_duration_); 172 | cyberdog::common::CyberdogToml::Set(temp, "enable", enable_); 173 | cyberdog::common::CyberdogToml::Set(temp, "align_contact", align_contact_); 174 | if (!cyberdog::common::CyberdogToml::WriteFile(elec_skin, temp)) { 175 | ERROR("write toml file failed"); 176 | } 177 | } 178 | 179 | private: 180 | void StartSkinCallback( 181 | const std::shared_ptr request, 182 | std::shared_ptr response); 183 | void SetModeCallback( 184 | const std::shared_ptr request, 185 | const std::shared_ptr response); 186 | 187 | rclcpp::Service::SharedPtr launch_service_{nullptr}; 188 | rclcpp::Service::SharedPtr skin_mode_service_{nullptr}; 189 | std::shared_ptr elec_skin_{nullptr}; 190 | rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; 191 | std::unordered_map> leg_map; 192 | std::vector change_dir_; 193 | PositionColorStartDirection start_dir_; 194 | PositionColorChangeDirection liftdown_color_; 195 | PositionColorChangeDirection liftup_color_; 196 | int32_t gradual_duration_{0}; 197 | int32_t defaul_duration_{0}; 198 | int8_t default_color_{0}; 199 | int8_t start_direction_{0}; 200 | // int32_t stand_gradual_duration_{0}; 201 | // int32_t twink_gradual_duration_{0}; 202 | // int32_t random_gradual_duration_{0}; 203 | bool enable_{false}; 204 | bool align_contact_{false}; 205 | bool last_align_contact_{true}; 206 | 207 | // bool move_skin_{false}; 208 | }; 209 | } // namespace motion 210 | } // namespace cyberdog 211 | 212 | #endif // SKIN_MANAGER__SKIN_MANAGER_HPP_ 213 | -------------------------------------------------------------------------------- /skin_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | skin_manager 5 | 0.0.0 6 | the elec_skin manager for other module 7 | lijian29 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | rclcpp 12 | std_msgs 13 | toml 14 | protocol 15 | ament_index_cpp 16 | cyberdog_common 17 | builtin_interfaces 18 | elec_skin 19 | 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | -------------------------------------------------------------------------------- /skin_manager/src/skin_manager.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2021 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 "cyberdog_common/cyberdog_log.hpp" 19 | #include "cyberdog_common/cyberdog_toml.hpp" 20 | #include "std_srvs/srv/set_bool.hpp" 21 | #include "skin_manager/skin_manager.hpp" 22 | #include "elec_skin/elec_skin.hpp" 23 | 24 | namespace cyberdog 25 | { 26 | namespace motion 27 | { 28 | 29 | SkinManagerNode::SkinManagerNode(std::string name) 30 | : Node(name) 31 | { 32 | executor_ = std::make_shared(); 33 | executor_->add_node(this->get_node_base_interface()); 34 | elec_skin_ = std::make_shared(); 35 | launch_service_ = this->create_service( 36 | "enable_elec_skin", 37 | std::bind( 38 | &SkinManagerNode::StartSkinCallback, this, 39 | std::placeholders::_1, std::placeholders::_2)); 40 | skin_mode_service_ = this->create_service( 41 | "set_elec_skin", 42 | std::bind( 43 | &SkinManagerNode::SetModeCallback, this, 44 | std::placeholders::_1, std::placeholders::_2)); 45 | std::string elec_skin_config = ament_index_cpp::get_package_share_directory("skin_manager") + 46 | "/config/" + "elec_skin.toml"; 47 | toml::value elec_skin_value; 48 | if (!cyberdog::common::CyberdogToml::ParseFile(elec_skin_config, elec_skin_value)) { 49 | FATAL("Cannot parse %s", elec_skin_config.c_str()); 50 | // return false; 51 | } 52 | // if (!motion_ids.is_table()) { 53 | // FATAL("Toml format error"); 54 | // exit(-1); 55 | // } 56 | // toml::value values; 57 | cyberdog::common::CyberdogToml::Get(elec_skin_value, "default_color", default_color_); 58 | cyberdog::common::CyberdogToml::Get(elec_skin_value, "start_direction", start_direction_); 59 | cyberdog::common::CyberdogToml::Get(elec_skin_value, "gradual_duration", gradual_duration_); 60 | cyberdog::common::CyberdogToml::Get(elec_skin_value, "defaul_duration", defaul_duration_); 61 | cyberdog::common::CyberdogToml::Get(elec_skin_value, "enable", enable_); 62 | cyberdog::common::CyberdogToml::Get(elec_skin_value, "align_contact", align_contact_); 63 | INFO("Default color: %d", default_color_); 64 | INFO("Start direction: %d", start_direction_); 65 | INFO("Gradual duration: %d", gradual_duration_); 66 | INFO("defaul_duration: %d", defaul_duration_); 67 | INFO("enable: %d", enable_); 68 | if (default_color_ == 0) { 69 | change_dir_.push_back(PositionColorChangeDirection::PCCD_WTOB); 70 | change_dir_.push_back(PositionColorChangeDirection::PCCD_BTOW); 71 | } else { 72 | change_dir_.push_back(PositionColorChangeDirection::PCCD_BTOW); 73 | change_dir_.push_back(PositionColorChangeDirection::PCCD_WTOB); 74 | } 75 | if (start_direction_ == 0) { 76 | start_dir_ = PositionColorStartDirection::PCSD_FRONT; 77 | } else { 78 | start_dir_ = PositionColorStartDirection::PCSD_BACK; 79 | } 80 | liftdown_color_ = PositionColorChangeDirection::PCCD_BTOW; 81 | liftup_color_ = PositionColorChangeDirection::PCCD_WTOB; 82 | leg_map.emplace(0, std::vector{PositionSkin::PS_RFLEG, PositionSkin::PS_FRONT}); 83 | leg_map.emplace(1, std::vector{PositionSkin::PS_LFLEG, PositionSkin::PS_BODYL}); 84 | leg_map.emplace(2, std::vector{PositionSkin::PS_RBLEG, PositionSkin::PS_BODYR}); 85 | leg_map.emplace(3, std::vector{PositionSkin::PS_LBLEG, PositionSkin::PS_BODYM}); 86 | std::thread{[this] {this->executor_->spin();}}.detach(); 87 | } 88 | 89 | void SkinManagerNode::StartSkinCallback( 90 | const std::shared_ptr request, 91 | std::shared_ptr response) 92 | { 93 | if (request->data) { 94 | INFO("Request to start elec_skin"); 95 | enable_ = true; 96 | WriteTomlFile(); 97 | } else { 98 | INFO("Request to stop elsc_skin"); 99 | enable_ = false; 100 | ShowDefaultSkin(); 101 | SetAlignContact(true); 102 | WriteTomlFile(); 103 | } 104 | response->success = true; 105 | } 106 | 107 | void SkinManagerNode::SetModeCallback( 108 | const std::shared_ptr request, 109 | const std::shared_ptr response) 110 | { 111 | if (!enable_) { 112 | return; 113 | } 114 | switch (request->mode) { 115 | case 0: 116 | align_contact_ = false; 117 | WriteTomlFile(); 118 | INFO("ShowBlackElecSkin"); 119 | ShowBlackElecSkin(request->wave_cycle_time); 120 | break; 121 | 122 | case 1: 123 | align_contact_ = false; 124 | WriteTomlFile(); 125 | INFO("ShowWhiteElecSkin"); 126 | ShowWhiteElecSkin(request->wave_cycle_time); 127 | break; 128 | 129 | case 2: 130 | align_contact_ = false; 131 | WriteTomlFile(); 132 | INFO("ShowFrontElecSkin"); 133 | ShowFrontElecSkin(request->wave_cycle_time); 134 | break; 135 | 136 | case 3: 137 | align_contact_ = false; 138 | WriteTomlFile(); 139 | INFO("ShowBackElecSkin"); 140 | ShowBackElecSkin(request->wave_cycle_time); 141 | break; 142 | 143 | case 4: 144 | align_contact_ = false; 145 | WriteTomlFile(); 146 | INFO("ShowFlashElecSkin"); 147 | ShowFlashElecSkin(request->wave_cycle_time); 148 | break; 149 | 150 | case 5: 151 | align_contact_ = false; 152 | WriteTomlFile(); 153 | INFO("ShowRandomElecSkin"); 154 | ShowRandomElecSkin(request->wave_cycle_time); 155 | break; 156 | 157 | case 6: 158 | align_contact_ = true; 159 | WriteTomlFile(); 160 | INFO("ShowMoveElecSkin"); 161 | if (!request->wave_cycle_time) { 162 | liftdown_color_ = PositionColorChangeDirection::PCCD_BTOW; 163 | liftup_color_ = PositionColorChangeDirection::PCCD_WTOB; 164 | } else { 165 | liftdown_color_ = PositionColorChangeDirection::PCCD_WTOB; 166 | liftup_color_ = PositionColorChangeDirection::PCCD_BTOW; 167 | } 168 | break; 169 | 170 | default: 171 | break; 172 | } 173 | response->success = true; 174 | } 175 | 176 | } // namespace motion 177 | } // namespace cyberdog 178 | --------------------------------------------------------------------------------